Hello, I'm trying to use MuJoCo to simulate a jumping frog. I have experimentally measured xyz coordinates for landmark joints on one of the legs. I would like to begin my simulation with a posture that approximately matches the xyz joint positions of a real frog at time=0. In the past, I have used Matehmatica to do forward dynamics (slow!!). My approach has been to use quaternions to align the Z axes of local reference frames along each leg segment (e.g. Z1 for segment 1, etc). Specifically, I calculate the quaternion (q) required to rotate one vector onto another. I then end up with q1 for rotating the Z axis onto segment 1 --> Z1. Then q2 for rotating Z1 to align with segment 2, and similar for the remaining leg segments. I then use these quaternions to set the position of my joints by setting d->qpos directly. This does not work for me in MuJoCo. I'm not sure if I'm explaining myself clearly, but how is it recommended to control the joint positions explicitly using quaternions? In other words, how is MuJoCo calculating the forward kinematics? I think there must be something simple about reference frames that I'm missing because my calculated quats will reconstruct the limb xyz coordinates in mathematica, but not in MuJoCo. Here is my model: Code: </asset> <!-- cumulative seglengths, fraction of total: 0.0872651, 0.198335, 0.481108, 0.726627, 1. --> <worldbody> <light pos="0 1 1" dir="0 -1 -1" diffuse="1 1 1"/> <geom name='floor' pos='0 0 -2' size='10 10 0.125' type='plane' material="MatPlane" condim='3'/> <body><!-- Foot --> <geom type="capsule" mass="0.5" fromto="0. 0. 0.087 0. 0. 0" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jhL" type="ball" pos="0. 0. 0"/> <body><!-- Tarsals --> <geom type="capsule" density="1" fromto="0. 0. 0.1983 0. 0. 0.087" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jkL" type="ball" pos="0. 0. 0.087"/> <body><!-- Tibfib --> <geom type="capsule" density="1" fromto="0. 0. 0.4811 0. 0. 0.1983" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jaL" type="ball" pos="0. 0. 0.1983"/> <body><!-- Femur --> <geom type="capsule" density="1" fromto="0. 0. 0.7266 0. 0. 0.4811" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jtL" type="ball" pos="0. 0. 0.4811"/> <body><!-- Torso --> <geom type="capsule" density="1" fromto="0. 0. 1.0 0. 0. 0.7266" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jpL" type="ball" pos="0. 0. 0.7266"/> </body> </body> </body> </body> <!-- ================= RIGHT LEG ================= /--> </body> </worldbody> My simulation code excerpt: Code: double quat[20] = {0.933363,0.18095,-0.309984,0.,0.843715,-0.0250227,0.495141,0.205799,0.887028,-0.449371,-0.106052,0}; // first 3 joints for (int i = 0; i < 20; i ++) { d->qpos[i] = quat[i]; } Thank you very much for your help, Best, Chris
Hard to tell what is going on from looking at the numbers. What exactly is the effect you are seeing? Quaternions in MuJoCo are represented as: (cos(a/2), sin(a/2)*x, sin(a/2)*y, sin(a/2)*z) where a is the rotation angle inr adians and (x,y,z) is the unit vector specifying the axis of rotation. Other conventions put the cos(a/2) term at the end. Actually, instead of trying to compute qpos yourself, you could use the simulator to do it for you. Define mocap bodies corresponding to your markers and set the data in mjData.mocap_pos/quat. Also define "connect" equality constraints between these mocap bodies and the regular bodies in your model. Now when you run the simulation, the constraints will pull the bodies to the marker positions.
Hi Again, I tried your suggestion and it looks promising so far. But, I'm having trouble establishing the initial limb configuration. I've defined one mocap marker for each segment. In the model, I've "anchored" the mocaps to the joint centers of rotation with the limb in the default position (all joints starting at 0 radians). I've then fed the xyz positions in using d->mocap_pos = xi, etc. When I run the simulation, the limb tries to "catch" the markers, but becomes unstable, even though the markers are within the workspace of all joints. It works only for markers where the starting position is close. So, I assume that the limb initial position must be set so that the initial xyz coordinates of the actual simulation are reasonably close to the desired coordinates such that the simulation can converge towards satisfying the constraints. Am I thinking about this correctly? Is there a way to further soften the constraints to let the simulation stabilize? Working with frog kinematics is especially tricky because their leg segment rotations are extremely convoluted -- so thanks for your patience! Best, Chris
Hi Again, I am still having difficulty using the mocap markers to constrain xyz positions of the joints in my limb. Just to restate my problem, I'm trying to input xyz coordinates (from experimental motion capture data) to define the joint centers of rotation. I'm using connect constraints between markers and corresponding segments. My problem is that the simulated limb posture cannot seem to 'catch up' to the marker positions and it goes unstable once the error accumulates. What I've done so far: Since the previous post, I've used other software to calculate perfectly continuous trajectories interpolated between the limb default position (joint angles = 0; limb stands up vertically) and the frog jump posture. These trajectories (representing the target position for joints) are saved then read into MJ to set mocap positions for each simulation iteration. I did this by 'slerping' the quaternions to insure that the target positions are always on the surface of the sphere of any given joint's workspace. What happens: The simulation begins looking OK with the joints aligned with the makers. As the simulation advances, the markers smoothly move to their desired position while the simulation follows - but too slowly to catch up. I've tried smaller interpolation steps to allow the simulation more time to 'find' the markers, but this only slows down the above behavior. Thanks very much for any advice that you can offer. Best, Chris My code: Code: <worldbody> <!-- ----------MARKERS--------------- --> <body name="m_pft" mocap="true"> <geom name="marker_pft" type="sphere" size="0.03" rgba="0 0 1 1"/> </body> <body name="m_tar" mocap="true"> <geom name="marker_tar" type="sphere" size="0.03" rgba="1 0 0 1"/> </body> <body name="m_tib" mocap="true"> <geom name="marker_tib" type="sphere" size="0.03" rgba="1 0 0 1"/> </body> <body name="m_fem" mocap="true"> <geom name="marker_fem" type="sphere" size="0.03" rgba="1 0 0 1"/> </body> <body name="m_bod" mocap="true"> <geom name="marker_bod" type="sphere" size="0.03" rgba="1 0 0 1"/> </body> <!-- --------------------------------- --> <light pos="0 1 1" dir="0 -1 -1" diffuse="1 1 1"/> <geom name='floor' pos='0 0 -2' size='10 10 0.125' type='plane' material="MatPlane" condim='3'/> <body name="s_pft"><!-- Foot --> <geom type="capsule" mass="0.5" fromto="0. 0. 0.087 0. 0. 0" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jhL" type="ball" pos="0. 0. 0"/> <body name="s_tar"><!-- Tarsals --> <geom type="capsule" density="1" fromto="0. 0. 0.1983 0. 0. 0.087" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jkL" type="ball" pos="0. 0. 0.087"/> <body name="s_tib"><!-- Tibfib --> <geom type="capsule" density="1" fromto="0. 0. 0.4811 0. 0. 0.1983" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jaL" type="ball" pos="0. 0. 0.1983"/> <body name="s_fem"><!-- Femur --> <geom type="capsule" density="1" fromto="0. 0. 0.7266 0. 0. 0.4811" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jtL" type="ball" pos="0. 0. 0.4811"/> <body name="s_bod"><!-- Torso --> <geom type="capsule" density="1" fromto="0. 0. 1.0 0. 0. 0.7266" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jpL" type="ball" pos="0. 0. 0.7266"/> </body> </body> </body> </body> <!-- ================= RIGHT LEG ================= /--> </body> </worldbody> <equality> <connect name="c_pft" body1="m_pft" body2="s_pft" anchor="0 0 0.087" active="true"/> <connect name="c_tar" body1="m_tar" body2="s_tar" anchor="4 0 0.1983" active="true"/> <connect name="c_tib" body1="m_tib" body2="s_tib" anchor="0 0 0.4811" active="true"/> <connect name="c_fem" body1="m_fem" body2="s_pft" anchor="0 0 0.7266" active="true"/> <connect name="c_bod" body1="m_bod" body2="s_bod" anchor="0 0 1.000" active="true"/> </equality>
You could make the equality constraints stronger. They have solimp and solref parameters, just like contacts. This should make it follow the markers faster. If it goes unstable, reduce the timestep or use RK4. Try to make them really strong, say: solimp="0.99 0.99 0" solref="0.01 1" Also, you probably realize that this whole thing is a hack. We are essentially trying to use physics to do state estimation. It can sort of do it, but nowhere close to optimal. The ideal solution is to define an optimization problem corresponding to state estimation and solve it numerically at every time step. Incidentally, I am working on MuJoCo 2.0 which will be focused on model-based optimization, and this will be one of the problems that the software solves automatically. But in the meantime, you should be able to get the current approach to work better.
Hi, Thanks - yes, I later realized I should try tweaking those values - I was actually doing so when your message came in. I tried the higher values and switching to RK4 as suggested, but still no success although it was improved. Glad to hear that optimization will be included in the future. In the meantime I think I will go back to manipulating qpos directly. I realized my own forward kinematics algorithm is different from mujoco's, so now I think will figure out how to calculate the quaternions in the proper reference frames. Best, Chris
Hi Again, I recently realized that my convention for defining reference frames is not the standard one. So, after re-calculating my quaternions everything works now producing the expected forward kinematics. Thanks again, Chris