Calculating moment arms in `CableSpan`

[nickbianco]

Now that via point support is almost complete (see #828), I'm turning my attention to the last piece of the puzzle needed for an OpenSim implementation of CableSpan: moment arm calculation. OpenSim does have a moment arm solver which uses the generalized force approach described in "What is a Moment Arm? Calculating Muscle Effectiveness in Biomechanical Models Using Generalized Coordinates". However, since CableSpan provides a direct method for calculating the path velocity, we can implement a simpler and more efficient way to calculate moment arms using the "path velocity method" (also described in the paper above).

Path velocity method

The path velocity method provides an exact computation for moment arms using velocities:

r_{q} = \frac{dl}{dq} = \frac{dl/dt}{dq/dt} \equiv \frac{\dot l}{\dot q}

In other words, by setting $\dot q = 1$ for a desired generalized coordinate we can calculate a moment arm, provided that we have an expression for $l(q,\dot q)$. CableSpan::calcLengthDot() is the public method for calculating $l(q,\dot q)$, and ideally, a similar method could be implemented for calculating a moment arm:

/** Calculate the moment of the cable with respect to a generalized coordinate.
State must be realized to Stage::Position.
@param state   State of the system.
@param whichQ  for which generalized coordinate q should we compute a moment arm?
@return The time derivative of the total cable length. **/
Real calcMomentArm(const State& state, MobilizerQIndex whichQ) const;

Two things need to be addressed for such an implementation: 1) how do we handle the case when $\dot q \neq u$ and 2) how do we manage the State cache to avoid unnecessary path recomputations when calculating moment arms?

Handling $\dot q \neq u$

When $\dot q \neq u$, I believe we need to calculate u for $\dot q = 1$ based on the matrix N(q). One approach might look like this:

// Define a multibody system and add a cable.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
CableSubsystem cables(system);
CableSpan cable(
        cables,
        ....);

// Construct the system and compute the path length.
system.realizeTopology();
State s = system.getDefaultState();
system.realize(s, Stage::Position);
Real length = cable.calcLength(s);

// Choose a generalized coordinate for computing a moment arm.
MobilizerQIndex whichQ(0);

// Set unity speed for the desired generalized coordinate.
SimTK::Vector qdot = s.getQDot();
qdot = 0.;
qdot[whichQ] = 1.;

// Calculate the corresponding change to u and update the state.
SimTK::Vector u = s.getU();
u = 0.;
matter.multiplyByNInv(s, false, qdot, u);
s.updU() = u;

// Realize the system to the velocity stage and retrieve the moment arm.
system.realize(s, Stage::Velocity);
Real momentArm = cable.calcLengthDot(s);

First off, is this mathematically correct? If correct, what is the smartest way to skip this extra calculation when $\dot q = u$ and is there a better way to accomplish this affect without directly manipulating the State? The latter question leads me to the second point.

State cache management

CableSpan::calcMomentArm() method should take a const State& argument, but this presents a challenge if State manipulation is required as described in the previous section. We could of course make a copy of the State internally, but this would discard the cache and require recomputing the path when realizing to the velocity stage. Ideally, we could modify u and realize to the velocity stage while leaving everything from the position stage (including the cached path) intact. I feel like this should be possible, but not quite seeing how yet.

[nickbianco]

@sherm1 would you mind providing some input on this when you have a chance? Your thoughts would be really helpful.

[sherm1]

Looks correct (I think), but would be worth it to see if we can calculate without having to do the state perturbation (although that is probably acceptable in practice). I'm interested also in Paul @mitiguy's thoughts also.

[nickbianco]

It's worth noting that calculating moment arms is not often performed in the middle of a simulation, so optimizing performance might not be the highest priority. But I agree, it would be nice to avoid the state perturbation.

MobilizedBody::lockAt() is another option, but also requires modifying the state.

[mitiguy]

Hi @nickbianco Can you link me to the paper? Also, the simplest example would be a great starting place for me to better understand the question.

[sherm1]

Here's a link to the paper, Paul.

[sherm1]

One thing to consider with any state-perturbing method is that all the constraints must be satisfied after the perturbation. So we have to perturb, lock the perturbed state, satisfy all constraints by updating the other states, then evaluate the result, subtract the nominal (unperturbed) result, calculate the moment arm, and finally restore the state to its original condition (unless we're using a copy).

The generalized force method from the paper doesn't require state perturbation or constraint re-satisfaction. It does need the cable's transmission matrix though. Then we assume a unit cable tension s, map that to body forces F using the transmission matrix, map those to generalized forces f using the system Jacobian, and calculate the moment arm as r=f/s.

Both of these methods have some awkward issues when q̇≠u if the user is interested in the cable's moment arm about one of those unfortunate q's. An easy hack would be to prohibit moment arm calculations about any joint for which q̇≠u. It's not clear to me that there is any value in getting a moment arm about one of the four coordinates of a quaternion! But maybe getting the moment arm about one of the roll-pitch-yaw angles of a ball joint makes more sense? That's no problem if the generalized speeds u are ṙ ṗ ẏ but it's trickier if u is the angular velocity ω. WDYT?

[nickbianco]

@mitiguy, below is a simple example. You can place it at the end of ExampleCableSpan.cpp if you want to compile and run.

// Calculate a moment arm.
// -----------------------
// We will calculate a moment arm for the first coordinate in the system
// (the x-axis of the cable origin mobilizer).
MobilizerQIndex whichQ(0);

// Make a copy of the state that we can use for moment arm calculations.
// PROBLEM: this copy discards all position level information we may have 
// previously calculated.
State s_copy(s);
s_copy.updQ() = s.getQ();

// Set the generalized speeds to zero except for the coordinate of interest
// for which we will set a speed of one. (Note that this is only valid
// because qdot = u.)
s_copy.updU() = 0.;
s_copy.updU()[whichQ] = 1.0;

// Realize the state to Stage::Velocity since we need to compute the 
// lengthening speed of the cable. Since we discarded the state cache in the
// copy above, this realization will recompute the cable path. Ideally, if
// we need to manipulate the state, we could at least hold on to path-related 
// information.
system.realize(s_copy, Stage::Velocity);

// Since we set the generalized speed of the coordinate of interest to 1,
// the lengthening speed of the cable is equal to the moment arm of the
// coordinate of interest.
std::cout << "q0 moment arm: " << cable.calcLengthDot(s_copy) << std::endl;

Again, moment arm calculations are usually done post-hoc for a simulation and not usually a performance bottle neck. And @sherm1 I agree, I'm not sure when calculating moments for $\dot q \neq u$ has much utility. But, just wanted to make sure I'm considering all possibilities to see if there is indeed a "right" way to do this.