How to get body velocity and acceleration from InverseKinematicsSolver?

[Yury G.]

Hi guys!
I've reproduced OpenSense pipeline in real-time and get a decent skeleton motion (visualizer shows perfectly valid skeleton movements) from a set of IMU sensors by using IK solver in such a way:

Code: Select all

...
    _solver = std::make_unique<OpenSim::InverseKinematicsSolver>(_model, mrefs, refs, coords, SimTK::Zero);
    _solver->setAccuracy(1e-5);
...
    _solver->assemble(state);
...
    _solver->track(state);
    _model.realizePosition(state);
    _model.findComponent<OpenSim::Body>("femur_r").getTransformInGround(state);

I'd like to get not only body transforms but also body velocities and accelerations from the same pipeline. I change the simulation stage to Velocity or Acceleration and expect the following code to print non-zer velocities/accelerations, but all I get are zeros in velocities and "nans" in accelerations:

Code: Select all

...
    _solver->track(state);
    _model.realizeAcceleration(state);
    _model.findComponent<OpenSim::Body>("femur_r").getAccelerationInGround(state);

Output:

Code: Select all

DEBUG 2020-08-28 16:13:33,544 linear acc: ~[-nan(ind),-nan(ind),-nan(ind)], ang acc: ~[-nan(ind),-nan(ind),-nan(ind)]
DEBUG 2020-08-28 16:13:33,544 linear velocity: ~[0,0,0]                                                              
DEBUG 2020-08-28 16:13:33,560 pos: ~[0.0771322,1.0213,0.120613], rot:                                                
[0.119981,-0.00990397,0.992727]                                                                                      
[-0.42229,0.904469,0.0600616]                                                                                        
[-0.898485,-0.426425,0.104337]                                                                                       
                                                                                                                     
DEBUG 2020-08-28 16:13:33,561 linear acc: ~[-nan(ind),-nan(ind),-nan(ind)], ang acc: ~[-nan(ind),-nan(ind),-nan(ind)]
DEBUG 2020-08-28 16:13:33,561 linear velocity: ~[0,0,0]                                                              
DEBUG 2020-08-28 16:13:33,577 pos: ~[0.0895136,0.916742,0.0696376], rot:                                             
[0.126829,0.00611909,0.991906]                                                                                       
[-0.453883,0.88951,0.0525481]                                                                                        
[-0.881989,-0.456874,0.115593]                                                                                       

Do you have any idea why I can't get accelerations and velocities from IK analysis and whether I should get them after running IK Solver?
Thanks!

[Yury G.]

Oh, and another thing I just tried is adding the following test function into file testIK.cpp and running it:

Code: Select all

void testAccelerationsFromInverseKinematicsSolverWithOrientation()
{
    Model model("subject01_simbody.osim");
    SimTK::State& s0 = model.initSystem();

    MarkersReference mRefs{};
    SimTK::Array_<CoordinateReference> coordinateRefs;
    auto orientationsData = convertMotionFileToRotations(model, "std_subject01_walk1_ik.mot");
    OrientationsReference oRefs(orientationsData);
    oRefs.set_default_weight(1.0);

    InverseKinematicsSolver ikSolver(model, mRefs, oRefs, coordinateRefs);
    ikSolver.setAccuracy(1e-4);

    auto timeRange = oRefs.getValidTimeRange();
    cout << "Time range from: " << timeRange[0] << " to " << timeRange[1]
        << "s." << endl;

    s0.updTime() = timeRange[0];
    ikSolver.assemble(s0);

    const std::vector<double>& times = oRefs.getTimes();
    for (double time : times) {
        s0.updTime() = time;
        cout << "time = " << time << endl;
        s0 = model.updWorkingState();

        ikSolver.track(s0);
        // realize to report to get reporter to pull values from model
        model.realizeAcceleration(s0);
        const auto& body = model.getBodySet().get("talus_l");
        cout << "pos: " << body.getTransformInGround(s0).p() << endl;
        cout << "lin vel: " << body.getLinearVelocityInGround(s0) << endl;
        cout << "ang vel: " << body.getAngularVelocityInGround(s0) << endl;
        cout << "lin acc: " << body.getLinearAccelerationInGround(s0) << endl;
        cout << "ang acc: " << body.getAngularAccelerationInGround(s0) << endl << endl;
    }
}

And here's the output. Position changes between frames, but velocity does not. Is it expected?

Code: Select all

time = 1.05
pos: ~[0.592394,0.151767,-0.105156]
lin vel: ~[0,0,0]
ang vel: ~[0,0,0]
lin acc: ~[4.49016e-15,-9.80665,-1.68359e-16]
ang acc: ~[-2.54751e-14,-1.64591e-14,-6.77043e-14]

time = 1.067
pos: ~[0.633468,0.141426,-0.106009]
lin vel: ~[0,0,0]
ang vel: ~[0,0,0]
lin acc: ~[3.11248e-15,-9.80665,-5.95485e-16]
ang acc: ~[-2.25504e-14,-1.92731e-14,-6.43501e-14]

time = 1.083
pos: ~[0.674136,0.13414,-0.106267]
lin vel: ~[0,0,0]
ang vel: ~[0,0,0]
lin acc: ~[8.47173e-16,-9.80665,-2.71962e-16]
ang acc: ~[2.30214e-15,4.01816e-15,7.78658e-15]

[Ayman Habib]

Hi Yury,

You can compute dynamics and accelerations only in the case when forces are applied (not by differentiating data), as such there's no surprise that you don't get accelerations in IK since there's no force other than gravity. What did you have in mind for how these would be calculated?

Best,
-Ayman

[Yury G.]

Hi, Ayman!
Looking at this picture https://simtk-confluence.stanford.edu:8 ... c736f0.png I thought that at least angular velocity and acceleration may be computed easily by differentiating joint angles we get from the IK tool. If you're not doing this, probably there is a good reason behind it =) Could you, please, briefly explain it or give a reference to some learning material?
Also, maybe you can point to an appropriate example (either documentation or C++ source code) with computing velocities/accelerations off the top of your head?
Thanks!

[Ayman Habib]

Hi Yury,

Please check the source for PointKinematics which reports positions, velocities and accelerations.

Hope this helps,
-Ayman