Foot Orientation Issue for New Robot

[willthibault]

HI @manumerous,

Thanks for open sourcing this mpc, very cool work!

I'm interested in adding a couple new models that I work with (REEM-C and TALOS by PAL Robotics). I started with REEM-C by emulating the G1 description and centroidal MPC that are available and produced the following:

wb_humanoid_mpc_reemc_centroidal_mujoco_initial.mp4

It seems like the robot is walking on its ankles and I believe this is due to a difference in frame orientation when the contact frames are created. Here is a snippet of the last joint in the REEM-C description:

  <joint name="leg_right_6_joint" type="revolute">
    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="leg_right_5_link"/>
    <child link="leg_right_6_link"/>
    <dynamics damping="1.0" friction="1.0"/>
    <limit effort="71.48" lower="-0.261799387799" upper="0.523598775598" velocity="3.94"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-0.226892802759" soft_upper_limit="0.488692190558"/>
  </joint>

Visually this looks like this:

My understanding is that for the G1 the contact frame orientation matches the last joint frame so only a translation is needed (looking through humanoid_common_mpc it seems like there are a lot of hard coded identity matrices for the orientation). Can you recommend the best way to add this feature?

Another issue that I will have is that for the G1 the foot contact offsets are identical for the contact frames, but for REEM-C the left and right foot will have different offsets. Again this looks like a similar area of the code that would need modification to differentiate feet. Can you recommend how this could be done?

As a side note, I added a docker file with NVIDIA GPU acceleration. I found this helped my laptop get through the heavy build (reducing PARALLEL_JOBS wasn't enough). Happy to share this if it is of interest.

I am happy to contribute both these features as well as REEM-C and TALOS examples.

Thanks!

[willthibault]

Hi @manumerous,

I just wanted to follow up on this issue, now that I have had time to investigate and resolve it. It was a foot rotation issue as I described above, which can be resolved by adding an orientation offset to the task.info file and then adding the rotation to the contact frame. Here's what I get as a result:

wb_humanoid_mpc_reemc_centroidal_flat_foot.mp4

I'll make a pull request once I get the MuJoCo sims up and set up TALOS.

Thanks!

[manumerous]

Awesome thank you! This is a great contribution!

Btw in rviz there is a topic (not visible per default) to visualize the contact collision primitives. You might want to visualize them when you change the values in the task.info to better fit TALOS or any other robot.

Looking forward to the PR!