Controlling for positions and measuring torques (solving for inverse dynamics)

glibesyck commented 1 year ago

Dear MuJoCo Team!

Thank you for creating this valuable tool and making it public, it is so interesting to discover what can be done with that! I would like to find out on the right way of solving for inverse dynamics problem with the help of your tool and whether it is in general possible. That's being said, I would divide the request on 2 parts describing more details:

I've created 2DoF double-pendulum model which moves only in one plane (2 hinge joints). Here is the XML snippet (don't pay attention to the peculiar mass and length numbers, those are scaled to simulate upper arm and forearm):
```
<mujoco>
<worldbody>
  <body pos="0 0 0" euler="0 0 0">
     <joint name="shoulder" type="hinge" axis = "0 1 0" pos = "0 0 0.3348"/>
     <geom type="cylinder" size="0.05 0.3348" rgba="1 0 0 1" mass="1.82"/>
     <body pos="0 0 -0.5976" euler="0 0 0">
       <joint name="elbow" type="hinge" axis = "0 1 0" pos = "0 0 0.2628"/>
       <geom type="cylinder" size="0.05 0.2628" rgba="0 1 0 1" mass="1.04"/>
     </body>
  </body>
</worldbody>
<actuator>
  <position name = "shoulder" joint = "shoulder"/>
  <position name = "elbow" joint = "elbow" />
</actuator>
</mujoco>
```
The idea is to have a predefined vector of consecutive desired angles for these 2 joints which follow the movement from, let's say, (0, pi/2) to (pi/2, pi/4) positions in time. It is totally defined by me, using, for example np.linspace() for some time period, having these start and end positions. As the result, I would like to find out which torques should be applied to each of the joints during the time of movement (in each time step), so that my model would follow this particular movement. So, the first question is what is the way of controlling the movement by myself (by specifying this defined vector of positions in consecutive time steps), so that it's not the process of reinitializing the state (physics are applied, double pendulum goes down but should have gone up as the movement suggests) but following from state to state?
The second question is, what is the way of measuring the resulted torques in joints?

Thank you in advance for your time looking at this question and paying attention!

Balint-H commented 1 year ago

Hello! Just a heads up I'm another user, not part of the MujoCo team so take things I write with a pinch of salt.

If you are fine with having small deviations from the prescribed movement, then you can set the position actuator's corresponding element in data->ctrl to the desired positions. This will use a proportional controller to drive the arm based on the error between the desired and actual position. You would probably want to add some damping in the joints to reduce oscillations. In this case the actual torque can then be calculated based on the gain, length and gearing of the actuator, or through data->qfrc_actuator. You may need to adjust the gearing or gains to get appropriate forces too. This method can be suitable if you go on to introduce perturbations to your system that weren't present when the kinematics were determined, so deviations from the path are automatically resolved.

For true inverse dynamics, that can create a 1:1 reconstruction of experimental data you would set the kinematics of the current frame (including acceleration), call mj_inverse() and read out data->qfrc_inverse. See this discussion and the documentation. In this case you don't need the actuators necessarily.

lieskjur commented 1 year ago

Hi, also just a user. I would suggest you generate the sequence of angles in the form of a twice-differentiable curve and feed accelerations to mj_inverse() (the second approach described by @Balint-H if I am not mistaken).

For generating the sequence you might find cubic Hermite curves useful. They are generated based on positions and velocities at the beginning and end of the desired trajectory (in your case positions (0,pi/2)->(pi/2,pi/4) and I assume velocities (0,0)->(0,0)) and can be differentiated to attain accelerations.

This is a resource I cited in my master's thesis, but I didn't use the library itself, writing my own implementation in julia. If you'd like I can share the code or send you the related chapter (very short) from my thesis.

If you'd then like to be able to deal with perturbations or model inaccuracies a computed torques controller might be a solution, but that might not be required for your use case.

google-deepmind / mujoco

Controlling for positions and measuring torques (solving for inverse dynamics) #555