Error in number of joints in simulation with soft objects

[mmurooka]

This PR avoids an error in mc_mujoco regarding the number of joints that occurs when soft objects are added. But I am not confident that this is a generic fix. So this PR is intended to share the issue and provide an example of a solution, and is not necessarily expected to be merged.

In MuJoCo, soft bodies are described using the composite tag. This implicitly introduces many virtual joints between the small bodies that make up the soft body.

In my trial, I first created a file in MJCF format containing the soft body and added it to the robots of the mc_rtc controller. The following error occurs in mc_mujoco in the main branch.

[error]qpos size: 51. Should be: 141
[error]qvel size: 50. Should be: 140
[critical] [mc_mujoco] Set inital state failed.
terminate called after throwing an instance of 'std::runtime_error'
  what():  [mc_mujoco] Set inital state failed.
[critical] === Backtrace ===
{void mc_rtc::log::error_and_throw<std::runtime_error, char const (&) [37]>(char const (&) [37]) at /home/mmurooka/workspace/install/include/mc_rtc/logging.h:52, mc_mujoco::MjSimImpl::setSimulationInitialState() at /home/mmurooka/workspace/ws_mujoco/src/mc_mujoco/src/mj_sim.cpp:431, mc_mujoco::MjSimImpl::startSimulation() at /home/mmurooka/workspace/ws_mujoco/src/mc_mujoco/src/mj_sim.cpp:550, mc_mujoco::MjSim::MjSim(mc_mujoco::MjConfiguration const&) at /home/mmurooka/workspace/ws_mujoco/src/mc_mujoco/src/mj_sim.cpp:1001, main at /home/mmurooka/workspace/ws_mujoco/src/mc_mujoco/src/main.cpp:73, __libc_start_main at ../csu/libc-start.c:342, _start in /home/mmurooka/workspace/ws_mujoco/install/bin/mc_mujoco}

Using this PR, a simulation is executed as in the following video, showing the deformation of the soft body.

https://user-images.githubusercontent.com/6636600/236622907-b7749cc7-ff1c-4df0-b189-06e20bc033dd.mp4

[rohanpsingh]

Hi @mmurooka san, do you want your composite object be considered as an mc_rtc robot or just inside mujoco?
Actually this problem is not specific to composite objects. mc-mujoco currently does not support adding articulated objects in the scene (where objects = models that exist in the mujoco scene but outside of mc-rtc). And I think, it basically breaks even when the XML representation of your robot has more joints than the mc-rtc representation. But this is only an initialization issue and should be easy to fix. In your PR, I couldn't understand this change. Does your PR work even when external objects are loaded in the scene? (explained here)

[mmurooka]

do you want your composite object be considered as an mc_rtc robot or just inside mujoco?

In my use-case, just inside mujoco is sufficient. So, if there is an easy way to do that, that's sufficient for me (but my understanding is that external objects is not intended for that purpose. Am I right?)

But this is only an initialization issue and should be easy to fix.

Yes, I think this PR is a patch to fix that part.

In your PR, I couldn't understand this change. Does your PR work even when external objects are loaded in the scene? (explained here)

No. I have not found a good way to do it as I wrote in the comment there.

[rohanpsingh]

External objects functionality is precisely for this purpose (if i understand your purpose correctly). See the case of grasp-fsm-sample-controller which has the longtable and box objects in the scene, but are not part of mc_rtc. But, as I said, this functionality may currently break for composite objects.

[mmurooka]

(but my understanding is that external objects is not intended for that purpose. Am I right?)

Thanks for the explanation. Now I understand that I was wrong above and that external objects can be used. However, since the implementation assumes that the additional object is a floating base (in my case, the additional object has a base fixed to the world), it could not be used as is. I got an error in the number of joints (6 different). Also (although it would be easy to fix), the xml files of additional objects must be in mc_mujoco::SHARE_FOLDER (not allowed to be in mc_mujoco::USER_FOLDER), which may prevent installation of additional objects from external packages.

[rohanpsingh]

Okay so let me close this PR.

the implementation assumes that the additional object is a floating base

The current implementation (not this PR) assumes there's only one free joint in the additional object, no more and no less. If we want to load a fixed-base object with 0 joints (i.e. just bodies and geoms), I think the current implementation can still be used.

For the future, I think we need support for following features:

• arbitrary number of joints (fixed/floating/hinge etc.) in additional objects
• specification of initial qpos and qvel of additional objects (using the mc_mujoco.yaml config?)