access to Velocity stage from SimbodyEngine without Analysis

[Stefan Lambrecht]

Hi all

I am trying to perform a runtime analysis (frame-by-frame, as data becomes available) based on generalized coordinates. I calculate generalized coordinates and would like to get segment velocity (linear and angular) back from OpenSim. This output can also be obtained from BodyKinematics, but there is no solver for BodyKinematics. I therefore browsed the BodyKinematics.cpp and replicated the record(s) part, as this appears (to me) as what is being done when calling step(State,stepNumber)

From BodyKinematics.cpp

Code: Select all

int BodyKinematics::
step(const SimTK::State& s, int stepNumber)
{
if(!proceed(stepNumber )) return(0);
record(s);
return(0);
}

Doing this (within a loop for each frame):

Code: Select all

//previous steps: create model - load data(generalized coordinates)        
//Update the state with the generalized coordinates of that frame 
        s.setQ(qFromFile_1); 

// I also tried s.updQ(), but noticed in a previous forum comment of Ajay that this could invalidate higher system stages (position and above)

        osimModel.computeStateVariableDerivatives(s); // no difference in commenting this in/out
        osimModel.getMultibodySystem().realize(s,Stage::Velocity); // also tried realizing up to Acceleration

//select a body 
        BodySet& bs = osimModel.updBodySet();
        OpenSim::Body& body=bs.get("femur_r");
        Vec3 COM_femur_r;
        body.getMassCenter(COM_femur_r);
        cout<<"COM femur R = "<<COM_femur_r<<endl;
        osimModel.getSimbodyEngine().getPosition(s,body,COM_femur_r,vec);
        cout<<"position COM femur R = "<<vec<<endl;
        osimModel.getSimbodyEngine().getDirectionCosines(s,body,dirCOSIN);
        osimModel.getSimbodyEngine().convertDirectionCosinesToAngles(dirCOSIN,
            &angVec[0],&angVec[1],&angVec[2]);
        cout<<"dircos femur R = "<<angVec<<endl;
// the above appears to work
        osimModel.getSimbodyEngine().getAngularVelocity(s,body,angVelFem);
        cout<<"ang vel femur "<<angVelFem<<endl;
// when I try calling the velocity values I get zero vectors 

It seems to me that either the Velocity stage is not being realized (I tried providing up to 3 frames (3 consecutive qFromFile_), or that the calculation is not being performed when I call this value.

I am looking for the computationally most efficient way to obtain segment velocity and position values on a runtime (frame-by-frame) basis.
Does anyone know if you can access the Velocity stage directly from SimbodyEngine or if you always have to go through an analysis?
If I have to go through an analysis (BodyKinematics in this example), should I also create a Solver?

[Michael Sherman]

Hi, Stefan. I think you are getting the right results! You didn't specify any velocities, just generalized positions q. You would also have to provide values for the generalized velocities u if you want to get non-zero body velocities. It is not possible to calculate a velocity from the positions in a single frame. With a series of frames you could estimate velocity. You could do it by differencing, for example u_est=(q1-q0)/dt where dt is the time between frames. However, that is likely to produce very noisy velocities. A better way to do it is to fit a smooth curve through the positions, and then differentiate the curve.

Regards,
Sherm

[Stefan Lambrecht]

Hi Sherm,

Thank you for your reply. I have since my post tried various forms and always seemed to end-up at the same result (zero velocities).
I will try the suggested u_est approach since it seems that this will be more efficient.
I plan to use this in an Kalman Filter where my state contains the q's and their derivatives; The angular and linear velocity of the segments come into play in the observation model (in which I simulate inertial sensor data).
Is using SimbodyEngine the most efficient to obtain this data (eg. better than BodyKinematics (I assume BodyKinematics will be almost the same, but since it does not have a solver I assumed the "path of least resistance" would be to use SimbodyEngine).
And if I may be so blunt to pose a follow-up question: what is the difference (when using estimated/prescribed state values) between updQ() and setQ(), and getSimbodyEngine() and updSimbodyEngine(); are they interchangeable?

[Stefan Lambrecht]

Thanks so much Sherm!
The u_est approach works, I will make sure to evaluate the noise this might introduce.

For others interested in this (I will make the full code available once a publication has been submitted):
estimate the velocity using one of the approaches mentioned by Sherm and then add in the previous code a s.setU()=u_est;
The calls to getVelocity() or getAngularVelocity() then no longer produce zero vectors.

[Michael Sherman]

I don't know the answer to your first question, which I think is "how best to use OpenSim to compute this result." Hopefully someone else can answer that.

I can answer your other questions: We use "get" for methods that return read-only references to data, and "upd" (short for "update") for returning writable data. We want the distinction to be clear because when you ask for writable data, you are presumably intending to modify it which will then invalidate anything that was computed from the old values. So for example, if you call getQ() you'll get a reference to a Vector of q's that you can look at, but nothing else changes. If you call updQ() you'll get the same reference to the Vector of q's but first we mark all position calculations as invalid. Then after you finish modifying the q's, you have to call realize(Stage::Position) in order to get those calculations redone with your new values. With that as a preliminary, here is the complete implementation of setQ():

Code: Select all

void setQ(const Vector& q) {updQ() = q;}

That is just a convenience method for updating q's that is a little more natural to look at than the assignment to a writable reference.

getSimbodyEngine() returns a read-only (const) reference to the SimbodyEngine while updSimbodyEngine() returns a writable reference. That affects which methods of SimbodyEngine you can call; use getSimbodyEngine() unless you have to call non-const methods.

Sherm