Robot fixed base sinks into the ground at constant speed

The issue is occurring when testing the Closed-Chain Simulator on iCubGenova04 and RRbot1 through a basic position controller or just feeding null torques to the robot model.

Test with the Closed-Chain Simulator

Created Simulink model for testing the library RobotDynWithContacts_ClosedChain on iCubGenova04 or RRbot1 models. Copied base configuration and model files from this repository's root path:

initFcn.m
postLoadFcn.m
./app
./+src
./test_matlab_system_2020b.mdl -> changes saved to test_matlab_system_closed_chain_2024b.slx to be pushed to a branch.

Running the model with free-falling iCubGenova04

Set l_sole and r_sole frames as IN_CONTACT_WITH_GROUND robot frames instead of LEFT_FOOT and RIGHT_FOOT.
Fixed the computation of total number of frames in contact with the ground as it was hard-coded. This is important if you define less or more than two frames which can get in contact with the ground.

Final Robot frames list is:

Frames.BASE = 'root_link';
Frames.IMU = 'head_imu_0';
Frames.COM = 'com';
Frames.IN_CONTACT_WITH_GROUND = {'l_sole','r_sole'};
robot_config.robotFrames = Frames;

contact_config.foot_print = vertex;
contact_config.total_num_vertices = size(vertex,2)*length(Frames.IN_CONTACT_WITH_GROUND);

The simulation runs successfully

Running the model with iCubGenova04 on a pole (invisible)

For this test we set a fixed frame:

We set the robot base a bit higher: initialConditions.base_position = [0; 0; 1];.
We set the fixed frame: Frames.FIXED = Frames.BASE;.
We leave Frames.IN_CONTACT_WITH_GROUND unchanged. This can be useful if for instance the pole is not high enough and the feet touch the ground at some point of the simulation.

In addition we integrate a simple gravity compensation in the controller.

The simulation runs successfully (almost).

We observe the robot (base) shifting downwards at constant speed, as we saw happening previously with the pendulum model with the simulator library RobotDynWithContacts (no closed chains). Not sure both problems are rellated.

Running the model with RRbot1 fixed on the ground

For this test, we set RRbot1 configuration to work with RobotDynWithContacts_ClosedChain:

The base frame of the double pendulum is sitting on the ground:

initialConditions.base_position = [0; 0; 0];
Frames.FIXED = Frames.BASE;

Set last link's frame (end effector) to be the IN_CONTACT_WITH_GROUND frame and add four vertices to it:

Frames.IN_CONTACT_WITH_GROUND = {'RRbot_hokuyo_link'};
contact_config.friction_coefficient = 0.1;
vertex = zeros(3, 4);
vertex(:, 1) = [-0.01; +0.01; 0];
vertex(:, 2) = [+0.01; +0.01; 0];
vertex(:, 3) = [+0.01; -0.01; 0];
vertex(:, 4) = [-0.01; -0.01; 0];

the "world" link in the URDF model has no mass, as we don't need anymore a very large mass and angular inertia to emulate the ground:
```
<link name="world"/>
```

The simulation runs successfully (almost).

We observe the robot leaning to the right and its base sink into the ground at constant speed. We can guess the observed drift is the same behaviour observed on the iCub model. Apparently, the constraint conditions used to keep the base fixed are not "strong" enough, or not well posed, causing it to drift downwards and topple. Typically, the constraint conditions could be ensuring zero acceleration of the "FIXED" frame, instead of zero velocity. We've seen this before in old simulations with iCub feet fixed through holonomic constraints (zero acceleration of the feet). ==> TO BE ANALYZED.

@traversaro , I think an issue could already be raised for this fixed frame drift problem.

Originally posted by @nunoguedelha in https://github.com/ami-iit/matlab-whole-body-simulator/discussions/92#discussioncomment-9689092

Eventually related to issue (1) in:

Hello @nunoguedelha,

Thanks for the quick response. I tried to implement a control loop using the RRbot example. I have two questions regarding this topic.

[ ] 1) Fixed the base frame using robot_config.robotFrames.FIXED, but there is a continuous downward movement along the z-axis during the simulation. Irrlicht visualization is used. My expectation is for the robot base to remain fixed to the ground. I kept the friction coefficient (fric_coeff = 10) and footprint settings (a bit larger footprint) the same in the config file settings.

[x] 2) When I stop the simulation in Simulink, change and reinitialize the robot settings from the config file, and use the new config variables in the Matlab workspace to start the simulation again, the OpenMP forces Matlab to close. However, when I use the same robot settings in the Matlab workspace and restart the simulation several times, it works well.

Error code: _MP: Error #15: Initializing libomp.dll, but found libiomp5md.dll already initialized. OMP: Hint This means that multiple copies of the OpenMP runtime have been linked into the program. That is dangerous, since it can degrade performance or cause incorrect results. The best thing to do is to ensure that only a single OpenMP runtime is linked into the process, e.g. by avoiding static linking of the OpenMP runtime in any library. As an unsafe, unsupported, undocumented workaround you can set the environment variable KMP_DUPLICATE_LIBOK=TRUE to allow the program to continue to execute, but that may cause crashes or silently produce incorrect results. For more information, please see http://openmp.llvm.org/

edit: 2nd issue is not related with Irrlicht engine, its more general with libraries. The reference comment to the Irrlicht engine was removed.

Originally posted by @oztahmet in https://github.com/ami-iit/matlab-whole-body-simulator/discussions/92#discussioncomment-9603906

ami-iit / matlab-whole-body-simulator