Hi, I'm trying to infer the contacting forces from the environment objects (e.g. wall, ground) to the human body from a motion sequence of a person interacting with the environment (e.g. walking, climbing). My understanding after reading papers introducing MuJoCo is that this can be done in MuJoCo through Inverse Dynamics. But after reading the MuJoCo document online, it is not very clear to me how this can be done in the program. I understand that at least I need to pass the pos, vel, acc of joint parameters to mj_inverse. Of course, I also need to specify the geometry for the body as well as the environment objects. But what is a good way to access the resolved contacting forces after inverse dynamics? Is the result stored in any of the member variables in mjData? Or do I use functions like mj_contactForce? Thanks for the help and clarification. dhan
Yes, after mj_inverse() the constraint forces (including contact) are stored in mjData.efc_force. You can also use the utility function mj_contactForce() to extract/decode the force for a specified contact id. This id corresponds to the position in the list of contacts mjData.contact. When using pyramidal approximations to the friction cone (which is the case with the default PGS solver) it is better to use mj_contactForce to decode the results from efc_force, because efc_force is in an over-complete coordinate frame with oblique axes corresponding to the edges of the friction pyramid. Note that if you apply inverse dynamics to trajectories inferred from motion capture data, small noise in the position can cause contact penetrations which in turn result in large contact forces inferred by inverse dynamics. When we do this in our work, we design contact-aware state estimators that infer the movement trajectories from mocap data by taking into account the physics, and in particular the physics of contact. See here: http://homes.cs.washington.edu/~todorov/papers/KolevHumanoids15.pdf It may be possible to get away with simpler filtering, but in general, keep in mind that inverse dynamics for a constrained system can be very sensitive to noise. The harder the constraints are, the more sensitive the inverse dynamics become. In the limit of perfectly hard constraints, the inverse is no longer defined.
Thanks for the detailed answer. That also clarifies some of my concerns that I haven't asked about yet. Yes, there is indeed penetrations and noise in our tracking data. I agree that it may influence the resolved constraints. But it is not yet clear to what level such influence is in our case. I'll let you know how this works. Thanks again for the insightful thoughts on this point and referring the paper.
To test if I used MuJoCo correctly, I was trying to compare the RNE result with that computed by other libraries. But it seems there is an obvious difference(e.g. against RBDL which I'm confident that I use correctly). Does mj_rne() also do anything other than the standard RNE as described by Featherstone? Or do I missed anything? I did the following: // mju_copy qpos, qvel, qacc to mjc_d corresponding fields mj_invPosition(mjc_m, mjc_d); mj_invVelocity(mjc_m, mjc_d); mj_invConstraint(mjc_m, mjc_d); mj_rne(mjc_m, mjc_d, 1, mjc_d->qfrc_inverse); // access mjc_d->qfrc_inverse to get generalized force (M(qpos)*qacc + C(qpos,qvel)) // compare this vector with tau from other libraries. I have visualized the mjc_d->xpos after mj_rne and they are correct. So I believe my modeling should be good. Please let me know if I did anything wrong? Thanks!
You are using it correctly. I am not sure why the results are numerically different from other libraries. Here are some possible reasons: 1. The model you are simulating may have different parameters. Are you sure you mapped the MuJoCo model exactly to RBDL's format? 2. MuJoCo's RNE output includes gravitational forces. 3. MuJoCo's RNE output *does not* include armature inertia, passive forces or constraint forces. The end of the inverse dynamics pipeline in MuJoCo is: Code: mj_rne(m, d, 1, d->qfrc_inverse); for( i=0; i<nv; i++ ) d->qfrc_inverse[i] += m->dof_armature[i]*d->qacc[i] - d->qfrc_passive[i] - d->qfrc_constraint[i]; 4. Internally MuJoCo's RNE works in global coordinates while the standard formulation is in local coordinates. These are mathematically equivalent, but could cause small numerical differences. How large are the differences you are seeing? Perhaps you should try this with a really simple model, say a 2-link arm without gravity.
Thanks for giving the possible reasons. I've figured out where the discrepancy is from: 1. I did not include gravitational forces while using other libs, while MuJoCo does by default (reason 2 you mentioned). 2. I did not set <inertial pos> while using explicit inertial properties like mass and inertia tensor. So the one inferred from geom was used by default which was different from the actual physical inertial framework. I've fixed them and now the discrepancy is in a reasonably low level (e.g. ~ 0.001% difference) Thanks for the help.
Hello, hope it's okay to post my question here, I have a question about the same function. I'm trying to implement a computed-torque control for a robot arm following your suggestions to set actuators as "motor". While using function mj_rne to obtain the dynamics term, I'm wondering did the output C(q,q_dot) actually means C(q,q_dot)*q_dot + N(q,q_dot), which is Coriolis matrix*qvel+gravity?
Never mind, I tested the controller with some randomization, and it works fine. I think C(q,q_dot) here means Coriolis matrix*qvel+gravity in this book.