Whole Body Simulator - Add Custom Robots

[nunoguedelha]

Quoting below the question submitted by @oztahmet :

Hello Nuno,

I hope you are well.

I'm using Whole Body Simulator for my graduate project. A demo program called "torqueControlBalancingWithSimu" is running well. However, I'd like to add a custom fixed-point multi-DOF manipulator. How can I achieve this?

It seems URDF format and meshes are suitable for adding robots, and I can edit configuration files as well. However, I'm unsure where to place these files and how to compile/rebuild the library (if necessary). I apologize, but I'm not familiar with binary or source installations using conda.

The library was installed using a single command for MATLAB on Windows.

Ahmet,
ITU - Control and Automation Dept.

[nunoguedelha]

Hello @oztahmet,

I'm glad you get to try this simulation framework, and any further feedback on your experience with following the installation instructions and using the software is welcomed.

Regarding your questions:

• Yes, you should be able to add a custom robot with your own URDF and meshes files (although it's less trivial than just selecting a model among the default ones).
  You can check where the simulator looks for the URDF file by running:

  system('yarp resource --find model.urdf')
  

• The simulator framework indeed supports fixed-base as well as floating-base models.
  As far as I remember, you should set the variable robot_config.robotFrames.FIXED, in the config files, to the name of the frame you want fixed.

I haven't worked on this framework for while now, so I need to reproduce your use-case and check some details and get back to you in a few, unless @traversaro already has additional information or clarifications to add.

[traversaro]

For the second error, a simple fix is to use the netlib blas library, see https://github.com/robotology/robotology-superbuild/blob/v2024.02.0/doc/faqs.md#how-to-i-solved-the-initializing-libompdylib-but-found-libiomp5dylib-already-initialized-when-using-matlab-libraries . I will update the one-line installer to use that info, thanks for reporting the problem @oztahmet !

[traversaro]

See robotology/robotology-superbuild#1658 .

[oztahmet]

@traversaro @nunoguedelha,

For the 1st issue,

#18 (comment)

I can't figure out how to fix base point to the ground. It just sinking with respect to friction coefficient magnitude.

[nunoguedelha]

Hello @oztahmet ,
I was having issues installing the simulator and running a simple example on my machine (MacOS M1) because of the unavailability of robotology conda packages for Apple Silicon platforms (#93). While @traversaro is working on a proper fix, I worked around the issue on my setup and could reproduce the issue.

I just started analysing it but I would already say:

• If I remember correctly, the fixed-base models (and variable robot_config.robotFrames.FIXED) are properly supported only by the closed-chain simulator RobotDynWithContacts_ClosedChain:
  
  ...and I guess you are using the other block, meant for systems without closed chains RobotDynWithContacts.
• As far as I remember, the RRBot model was created before the implementation of the support of fixed-base models. So RRbot is actually a floating-base model which uses a workaround which consists in having a very massive base with huge terms in the inertial matrix and significant width. This property, along with the usual handling of floating-base systems and ground impacts, was enough to keep the base "fixed".
• Considering for now the use of RobotDynWithContacts, I suspect there is an issue with the ground impact detection when the initial position xyz of the base is exactly [0, 0, 0].
  => WORKAROUND: you can already use a workaround by setting the initial z position just slightly above the ground, e.g. 0.001 m. You can set it in the file app/robots/RRbot1/configRobot.m:

% Initial condition of iCub and for the integrators.
initialConditions.base_position = [0; 0; 0.001];

Meanwhile, we will analyse the issue and check the use of RobotDynWithContacts_ClosedChain .

[nunoguedelha]

On my previously suggested workaround...

...adding just a plot of the z coordinate time series, for different initial heights z = 0.001, z = 0 and z = -0.001 (in metres), illustrating the effect of the workaround I suggested in my previous post:

Joint Velocity STOPS Being Updated

I ran a test while forcing the input joint torques to 0 and initial position of the robot (double pendulum) at horizontal position (q1 = 90°, q2 = 0º) as we can see depicted below:

As the simulation runs, we see the base lying on the ground and the pendulum still in the initial position while the torques are null, which is physically wrong and indicates that the model state is not updated properly. Indeed the plots show that the velocity stops being updated as soon as the base hits the ground (z <w_H_b>(3,4) = 0).

Zoom in...

Test with the Closed-Chain Simulator

Ongoing...

[nunoguedelha]

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.

[traversaro]

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

Ok, feel free to open it, thanks!

[nunoguedelha]

Submitted #94 .