Estimate qvel from sequence of rotations

google-deepmind / mujoco

Multi-Joint dynamics with Contact. A general purpose physics simulator.

https://mujoco.org

Apache License 2.0

7.84k stars 783 forks source link

Estimate qvel from sequence of rotations #1687

Closed areiner222 closed 4 months ago

areiner222 commented 4 months ago

Hi,

I'm using mujoco(-mjx) for imitation learning and am trying to determine the best (right?) way to compute qvel from mocap data.

I have a humanoid model and using 3 hinge joints associated will all bodies in the kinematic chain besides the root free joint. I'm able to infer qpos from the sequence of kinematic chain relative rotations by converting those rotations into xyz euler angles (I say this with some confidence because the rendered sequence of positions after computing forward kinematics matches my reference motion), but I'm not sure I am computing qvel properly. As of now, I am just doing finite differencing on the qpos (euler) angles (mocap running at 60hz). Is this a good way to do it? I try to account for angle wrapping by computing the qpos / euler angles sequentially ensuring a continuous representation. Do I need to factor joint ranges into this as well?

Thanks!

yuvaltassa commented 4 months ago

Very specifically for the OP question, finite differencing is the correct thing. It is trivial for the hinges and sliders and only slightly less trivial for the quaternion in the free joint.
Side comment: 3 hinge joints everywhere is a bit much, no? Why give the elbow 3 dofs if you know it has 1?
More generally, you are solving an Inverse Kinematics problem with retargeting. I recommend stepping through the Least Squares tutorial notebook, which discusses IK with examples, and possibly @hartikainen's MJPC task (which solves a much harder problem: it finds the controls which achieve the desired tracking):

areiner222 commented 4 months ago

coming back here to say least_squares is magic - inverse kinematics with the previous state continuity regularization working really well

kevinzakka commented 3 months ago

Nice @areiner222 !!

areiner222 commented 3 months ago

Thanks again for your help - really put me on a better track.

Had a couple more question to close the loop - any recommendations on how to:

denoise resulting qpos fits from inverse kinematics? small errors resulting in FD qvels noise.
use a combination of finite differencing of qpos to approximate qvel/qacc + inverse dynamics to smooth the state / improve physical plausibility? (perhaps akin to minimizing qfrc_inverse root forces over the height offsets like in the LQR notebook). Wondering if there's anything straightforward to do here without resulting to MPC.

yuvaltassa commented 3 months ago

Well 2 is just a better version of 1, right? You've got the right idea there. We have some code to do this in the mujoco_mpc repo, perhaps @erez-tom or @thowell can help you locate it, I think it's pretty straightforward to use.