Get computed dynamics (generalized accelerations)

[raisimtech]

Get computed dynamics (generalized accelerations)

[jhwangbo]

An analytical derivative is not faster nor more accurate. What you compute above is done exactly the same in the raisim code.

[jhwangbo]

There is no single call to do all. You have to call setLinearVelocity(), setAngularVelocity(), setPose() separately

[jhwangbo]

Yes, I get different values here. But as I mentioned above, after adding World.getContactSolver().setOrder(true), I get exactly the same numbers again. The order of iteration is the only thing that is stored inside. This only changes the numerical error directions and is not really changing the dynamics.

system->updateKinematics(); is not necessary for you. This is automatically called by setGeneralizedCooridnate(). You can check the implementation in the header file. I guess the only time this should be called is when the users modify the robot definition.

[jhwangbo]

this is the right place to ask the question.

You can get the mass matrix, nonlinear term, and contact force. Basically, everything you need to compute the forward dynamics. So you can compute the acceleration. But we don't provide generalized acceleration.

[RobertoRoos]

Right. What triggers a re-computation of the inverse mass matrix, non-linear terms, etc. if I change the state of my system?

And this acceleration will be the derivative of v, right? With an amount of terms determined by object->getDOF()?

[jhwangbo]

world.integrate1() triggers forward dynamics. integrate2() computes the contact forces and updates position and velocity.
Yes, generalize acceleration will have the same dimension as object->getDOF()

[RobertoRoos]

Can I avoid integrating when computing accelerations? I want to compute the dynamics without changing the states I specified.

[jhwangbo]

Not using the current API. One workaround is to set the state back to the original state manually

[RobertoRoos]

Thank you for your replies @jhwangbo.

Alright, then this items has become a suggestion to include it in the API. It could be helpful for machine learning-like projects like mine.

[jhwangbo]

Integrating the state doesn't take much time once you computed the forward dynamics. So the workaround I suggested does not compromise much. If I split world.integrate()into three different calls, this will create additional confusion.
At the moment I don't see that many people need such a feature. I'll wait for more requests for now.

[RobertoRoos]

I understand.

If I could bother you one last time? Would this snippet then cover everything, including contact forces?

Eigen::VectorXd getAcceleration(raisim::ArticulatedSystem *system, raisim::World &world) {
 
    auto q = system->getGeneralizedCoordinate();
    auto v = system->getGeneralizedVelocity();
 
    world.integrate(); // Compute dynamics
 
    auto M_inv = system->getInverseMassMatrix();
    auto h = system->getNonlinearities().e();
    auto tau = system->getGeneralizedForce().e(); // Total joint force
    Eigen::VectorXd q_ddot = M_inv.e() * (tau - h);
 
    system->setGeneralizedCoordinate(q); // Reset state to what is was before integration
    system->setGeneralizedVelocity(v);
 
    return q_ddot;
}

[jhwangbo]

''getGeneralizedForce'' only returns your commanded force. It doesnt include the contact forces. Check this link
https://raisim.com/sections/Contact.html#contacts
The last thing you are missing is the joint damping and pd control force (if you are using it).
Now i am curious. Why dont you just compute (u'-u)/dt if thats the only thing yoy need?

[RobertoRoos]

Okay, so I have to include contact explicitly too. It doesn't seem trivial to compute joint torques from those forces.

Using finite difference ((u[k+1] - u[k]) / dt) would be my plan B. But I am also going to be needing gradients of the dynamics function (e.g. d (du/dt) / d \phi), which I was planning on approaching with finite difference too. I figured I could improve both speed and accuracy by at least getting the dynamics analytically.

[RobertoRoos]

Alright. My next question is then how to undo world.integrate()? I can reset the state of my robot, but is there a way to reset every object in the world?
I see world.getOjbList(), but an raisim::Object does not have a setState().
Cloning the World instance with something basic like memcpy would work too, but I think that would have much more overhead.

What I have now:

Eigen::VectorXd getAcceleration(raisim::ArticulatedSystem *system, raisim::World &world) {

    // Get current state

    Eigen::VectorXd q_prev = system->getGeneralizedCoordinate().e();
    Eigen::VectorXd v_prev = system->getGeneralizedVelocity().e();

    double t_prev = world.getWorldTime();

    world.integrate();

    // Restore previous state

    Eigen::VectorXd v = system->getGeneralizedVelocity().e();

    system->setGeneralizedCoordinate(q_prev);
    system->setGeneralizedVelocity(v_prev);

    world.setWorldTime(t_prev);

    return (v - v_prev) / world.getTimeStep();
}

[jhwangbo]

memcpy is not a good idea. the objects are dynamically allocated

[RobertoRoos]

The saga continues.

The objects in world.getObjList() don't have those set...() methods either.
But that's fine, for now I can only focus on a single Articulated System.

The problem I have now is that my dynamics calculations still influence the simulation.
I have the following example: https://gist.github.com/RobertoRoos/1e083c9e44eb14b4dbbe55f5cce11c17#file-raisim_dynamics-cpp-L56 (pretty much the function I posted above.)

Whether or not the call to getAcceleration() is enabled influence the sequence of integrated accelerations. After about 20 iterations the difference starts to show.

Apparently there is some property in the world that is modified by the integration that I am not yet undoing. Any idea what that could be?

Thank you!

[jhwangbo]

This part can be confusing indeed. world.getObjList() returns Object* and you have to static_cast it depending on the type. You can get the type by Object::getObjectType().

I ran your code. So am I supposed to get different numbers if I uncomment getAcceleration()? But I get exactly the same numbers.

The only thing that I can think of now is World.getContactSolver().setOrder(true). The solver switches the order of iteration to mitigate integration error. You can force it to be the same order by just setting it forcefully like that.

[RobertoRoos]

Thank you for checking it out!

Odd. I made a new gist, that literally compares two worlds, one that normal and one that has acceleration queried: https://gist.github.com/RobertoRoos/1e083c9e44eb14b4dbbe55f5cce11c17#file-raisim_dynamics_compare-cpp

I get diverging values: (they are the same with getAcceleration() commented out)

./cmake-build-debug/raisim_dynamics
DOF: 18
Final accelerations are different:
q_ddot_fd_w1: -0.54  -2.79   0.00  -0.05   0.01   4.53  26.85   9.99   2.19   0.04  -0.03   0.01  44.25 -20.85  30.70   0.05   0.04  -0.06
q_ddot_fd_w2: -0.65  -5.74  -0.00  -0.09   0.03  -4.82 -19.41  16.10 -11.38   0.08  -0.02  -0.01  42.48 -19.74  27.19   0.04   0.05  -0.09
Final velocities are different:
q_dot_w1: 0.03   0.06   0.00   0.03  -0.00  -0.22 -13.44  -3.83   2.70  -0.04  -0.00   0.01  -2.83   0.96   1.16  -0.04   0.00  -0.01
q_dot_w2: 0.01  0.07  0.00  0.02  0.00 -0.12 -9.86 -2.56  1.07 -0.04 -0.00  0.01 -3.12  0.99  1.03 -0.04  0.00 -0.01
Final positions are different:
q_w1: 0.00  0.30  0.03  1.00 -0.04  0.00 -0.01  0.04 -1.75  5.19  0.15  1.66 -2.85  0.46  1.56 -4.96  0.15 -1.66  2.85
q_w2: 0.00  0.30  0.03  1.00 -0.04 -0.00 -0.00  0.15 -1.68  5.11  0.13  1.66 -2.85  0.44  1.61 -5.01  0.14 -1.66  2.86

Final world1 time: 1.00

Interestingly, the values converge after some 10 seconds of simulation time, indicating world1 is simulating too fast.

Anyway, for our current use case this might not be a problem so I'll continue anyway.

[RobertoRoos]

There we go, now it's identical for me too.

The final gist: https://gist.github.com/RobertoRoos/1e083c9e44eb14b4dbbe55f5cce11c17#file-raisim_dynamics_compare-cpp

One last time: thank you @jhwangbo !

[edward9503]

Hi @jhwangbo ,

for a floating-base robot, is the generalized acceleration of the base a classic or spatial acceleration when calculating using Forward Dynamics in RaiSim?

Many thanks.