Added capability to provide a key frame as starting configuration.

[ndunkelb-nasa]

I mainly added this because we don't have a good way to get load an arbitrary mujoco configuration, other than just setting the initial positions of the joints that ROS cares about in the ros2_control xacro. Mujoco does support this, but we didn't really have an interface to do this. I added an optional parameter to the ros2 control hardware interface that can load that information on startup, so that you can easily save off a configuration, then pass it into your config at runtime to test from there again.

From the documentation I added:
This implementation mainly provides an optional parameter to use the keyframe from a provided file as the starting configuration. This mutually exclusive with the initial_value that can be used for state interfaces. This is intended to provide an alternative method to load an entire mujoco model state from a configuration that was saved by clicking 'Copy state' in the simulate window, and pasted into a config file. Expected use cases are to work on a specific part of an application that involves the environment being in a very specific starting configuration. If this parameter is an empty string, it will be ignored.

The parameter is passed in the ros2_control hardware interface parameters like so

<param name="override_start_position_file">$(find my_description)/config/start_positions.xml</param>

The config files look like this when saved from the simulate window. (Time is ignored)

<key
  time="228.612"
  qpos="-0.249969 0.75"
  qvel="-3.9179e-06 1.06969e-09"
  ctrl="-0.25 0.75"
/>

It has been implemented in the test configuration if you want to take a look!

[saikishor]

Just curious, do you think it would be possible to set them by parameter (or) by yaml, instead of an XML file?. At the end, you are interested in the position information alone right? or am I wrong?

[eholum-nasa]

Some nits mostly, and use auto more often!

Other than that this is very, very handy!

[eholum-nasa]

One of those functions that we should unit test...

[ndunkelb-nasa]

Do you think it is necessary before a merge? Or are should that be added to a future issue of adding unit tests...

[eholum-nasa]

Future problems.

[eholum-nasa]

Just curious, do you think it would be possible to set them by parameter (or) by yaml, instead of an XML file?. At the end, you are interested in the position information alone right? or am I wrong?

We went back and forth on this but ultimately we wanted something to preserve the state of the simulation including the velocity and control values. This lets you set the entire state of the sim using what is copied out of the simulate app as is, which is handy because it's not particularly human readable when models start to get complicated...

e.g. This is one of our sims that has quite a few moveable, though not actuatable, components:

<key
  time="19.038"
  qpos="2.00117e-06 -1.04216e-06 -3.93152e-07 -3.35398 -2.85842 -1.37773 -0.409265 -1.54004 -1.39155 0.0230049 0.0230048 -0.0100147 1.01455e-11 4.12582e-05 0.000910427 -4.85539e-17 0.00335696 -1.62788e-08 -7.68121e-06 -3.7417e-11 -0.000635024 0 0 0 0 0 0 0 0 0 1.89902e-41 2.74411e-18 0.799794 0.800001 0.267509 0.70609 -0.000280325 -0.000278419 0.708122"
  qvel="5.6413e-17 7.33694e-16 8.2523e-16 -1.06689e-13 -1.05929e-13 5.3559e-14 -1.31323e-14 6.02032e-15 5.35333e-14 7.10881e-15 7.19583e-15 2.24957e-16 1.76555e-25 4.33385e-06 -1.35766e-17 9.28267e-23 0.00017641 -8.59332e-10 1.39536e-19 3.30961e-25 1.74596e-17 0 0 0 0 0 0 0 0 0 4.54572e-54 1.44205e-19 0.00105609 0.000748561 -7.64106e-06 0.00416155 0.00291983 0.000152121"
  ctrl="2.00117e-06 -1.14997e-06 -3.35397 -2.85842 -1.37773 -0.409259 -1.54006 -1.39155 0.0230049"
/>

I think @ndunkelb-nasa has found it pretty handy to just copy/paste those things out when testing different capabilities within a single simulation. E.g., tweaking admittance parameters.

[saikishor]

@eholum-nasa Thank you for explaining it. It totally makes sense

[saikishor]

Spoiler alert 🚨

We are currently working on the MuJoCo MJCF generation on the fly. We had a brainstorming session with Ortisa today. The idea is to reuse your work and add some more args to convert URDF on the fly. One of the features, we discussed is to set the scene file at run time, so we can also include this part into the scene or add a key pose part into the MJCF directly.

Would that work for you guys?

I'm trying to avoid hardcoding too many things in the URDF, because that way we lack flexibility to start with different poses.

[ndunkelb-nasa]

Just curious, do you think it would be possible to set them by parameter (or) by yaml, instead of an XML file?. At the end, you are interested in the position information alone right? or am I wrong?

We went back and forth on this but ultimately we wanted something to preserve the state of the simulation including the velocity and control values. This lets you set the entire state of the sim using what is copied out of the simulate app as is, which is handy because it's not particularly human readable when models start to get complicated...

e.g. This is one of our sims that has quite a few moveable, though not actuatable, components:

<key
  time="19.038"
  qpos="2.00117e-06 -1.04216e-06 -3.93152e-07 -3.35398 -2.85842 -1.37773 -0.409265 -1.54004 -1.39155 0.0230049 0.0230048 -0.0100147 1.01455e-11 4.12582e-05 0.000910427 -4.85539e-17 0.00335696 -1.62788e-08 -7.68121e-06 -3.7417e-11 -0.000635024 0 0 0 0 0 0 0 0 0 1.89902e-41 2.74411e-18 0.799794 0.800001 0.267509 0.70609 -0.000280325 -0.000278419 0.708122"
  qvel="5.6413e-17 7.33694e-16 8.2523e-16 -1.06689e-13 -1.05929e-13 5.3559e-14 -1.31323e-14 6.02032e-15 5.35333e-14 7.10881e-15 7.19583e-15 2.24957e-16 1.76555e-25 4.33385e-06 -1.35766e-17 9.28267e-23 0.00017641 -8.59332e-10 1.39536e-19 3.30961e-25 1.74596e-17 0 0 0 0 0 0 0 0 0 4.54572e-54 1.44205e-19 0.00105609 0.000748561 -7.64106e-06 0.00416155 0.00291983 0.000152121"
  ctrl="2.00117e-06 -1.14997e-06 -3.35397 -2.85842 -1.37773 -0.409259 -1.54006 -1.39155 0.0230049"
/>

I think @ndunkelb-nasa has found it pretty handy to just copy/paste those things out when testing different capabilities within a single simulation. E.g., tweaking admittance parameters.

I would say, more explicitly, this enables a kind of "save and reload state" right from the simulation window. If you click this button, it will copy the current state in that xml format, and that is what I was trying to make loadable. In this specific case, the control values are important too, because I want the gripper to be actively applying force, rather than just holding its position which would cause the object to slip.

[ndunkelb-nasa]

Spoiler alert 🚨

We are currently working on the MuJoCo MJCF generation on the fly. We had a brainstorming session with Ortisa today. The idea is to reuse your work and add some more args to convert URDF on the fly. One of the features, we discussed is to set the scene file at run time, so we can also include this part into the scene or add a key pose part into the MJCF directly.

Would that work for you guys?

I'm trying to avoid hardcoding too many things in the URDF, because that way we lack flexibility to start with different poses.

This would be awesome! I think we would be happy to share our thoughts about on the fly conversion if you are interested. We had thought about that, but decided against it for several reasons. It might make sense to talk about it in a different issue though.

Regardless, providing a scene file at run time would definitely be nice! We have also thought about this a bit, but our infrastructure usually has a separate ROS package for a specific application, and we haven't figured out how to properly combine assets that need to be in that package with the assets in a base robot package. But obviously creating stuff on the fly might help solve that!