Create model integrating the YOGA++ controller with the dynamics with contacts simulator from matlab-whole-body-simulator

[nunoguedelha]

Moved from ami-iit/matlab-whole-body-simulator#32:

We wish to implement a Simulink model which controls a humanoid robot (iCub) in performing a dynamic trajectory while balancing: the YOGA++. For that purpose, the model would integrate the YOGA++ controller with the whole-body simulator from matlab-whole-body-simulator.

Recap of follow-up issues

• YOGA controller + matlab-whole-body-simulator: control neck joints in position #123
• wb-toolbox#192
• wb-toolbox#205
• Map the friction and motor inertia parameters to the joint names list (do not use hardcoded indexes) #124
• Minor fixes and improvements of the YOGA-controller+matlab-simulator model #125 :
  • Restore "measured joint torques" in the scopes visualizer.
  • Replace Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES by a warning.
• Fix the YOGA on right foot in the model YOGA-controller+matlab-simulator #127 : Fix the YOGA starting on the right foot.

[nunoguedelha]

As described in https://github.com/dic-iit/element_software-engineering/issues/24, this consists in replacing the Gazebo target by the library blocks "RobotDynWithContacts", "IMU" and "robot visualizer" from matlab-whole-body-simulator:

• "RobotDynWithContacts" simulates the robot dynamics, handling the contacts of the feet with the ground. This block provides, among other outputs, those required by the controller:
  • joint positions
  • joint velocities
  • joint accelerations
  • left foot wrench
  • right foot wrench
• "IMU" emulates the sensor outputs from the floating base state and linear acceleration provided by "RobotDynWithContacts".
• "robot visualizer" allows to visualize the robot. It avoids using Gazebo as a visualizer, with the following advantages:
  • the overall procedure for running the simulation is simpler (no need to run Yarp nor Gazebo).
  • There's no need for synchronizing Simulink and Gazebo.

[nunoguedelha]

The new Simulink model controllers/floating-base-balancing-torque-control-with-simulator was created from a duplicate of 'controllers/floating-base-balancing-torque-control', then the blocks interfacing the model with Gazebo were replaced with the "RobotDynWithContacts", "IMU" and "Robot Visualizer" blocks.

Notes on particular changes...

Robot model specific to the simulation, distinct from the model used by the controller

This allows to have parameters specific to the simulation, like:

• controlled joints, in order to account them in the simulation, even if the controller is not controlling them, leaving them to the default position control.
• model tuning specific to the simulation stabilization if required.
• robot initial configuration, independently from the one eventually set by the controller as an initial step.
• contact vertices definition.
• etc.

Neck joints and option Config.CORRECT_NECK_IMU

The neck joints are not part of the joints controlled by the high-level torque controller, but their position was directly read from the "Yarp port Read" block to account for it when propagating the IMU orientation measurement throughout the kinematic tree (in case Config.CORRECT_NECK_IMU = true).

Change with the integration of the simulator: The neck joint positions are now read directly from the simulator outputs. Routed neck position from main inputs of the controller system to the "neck transform" block.

Caveat: The neck position of the simulated model is fixed to [0 0 0] for now, as the respective joints are not defined in the joint list given to the ConfigIm block inside the simulator sub-system. The neck and torso are lumped in the iDynTree imported model. For this reason, the setting Config.CORRECT_NECK_IMU = false is the only one valid for now.

Shall be fixed with whole-body-controllers#123.

Changes for treating the controller as atomic unit

We have to treat the controller sub-system as an atomic unit in order to be able to run it at a sampling rate different from the simulator. This forbids the use of any continuous blocks (like the "clock" Simulink native block), and requires all the blocks within the target sub-system to either inherit the sampling time, either set the same as the one set in the sub-system block.

Fixes in the controller sub-system for settling the same Config.tStep sample time in all the sub-system blocks:

• Set "Inf" sampling time in reflected inertia block.
• Replaced the Holder blocks from the WBT library by a re-implementation in the "utilityBlocks" library ("Joint Torque Saturation", "LFoot to base link transform", "RFoot to base link transform", "State Machine"). => wb-toolbox#205
• Moved the clock, previously connected to one of the inputs of the "STATE_MACHINE", outside the controller.
• Added a memory block between the controller output and the simulator (works better than "unit delay" block between sub-systems running at different rates).
• Removed "measured joint torques" from the scopes visualizer.
• Routed the joint acceleration "measurements" from the main input to the joint reflected
  inertia block (tags are not authorized across sub-systems when treating the parent sub-system as an atomic unit).
• Moved "Dump and visualze" into "MOMENTUM_BASED_TORQUE_CONTROL" sub-system.
• Fixed model tasking and sampling time options:
  Removed "Treat each discrete rate as a separate task" option, which was causing data rates transition problems between the simulator (1kHz) and the controller (100Hz).

At this point the model compiles successfully. We can see below a view of the model with the blocks and lines colored following the sampling rates:

Sampling rates:
Green: 10 ms
Red: 1 ms
Pink: Inf (Constant or ConfigIm blocks)

[nunoguedelha]

Simulation run fails at the very beginning:

This is due to a known problem, not fully characterized, nor fully fixed, with the QP block (WBT block implementing the qpOASES solver): https://github.com/dic-iit/element_ironcub-control/issues/164#issuecomment-821027420.

Trying a similar fix... using QP interface with constraints lower bound input "lbA". Set lbA=-Inf.

The workaround fixed the problem!

[nunoguedelha]

Simulation still fails at the very beginning due to a spike detected on the encoders:

Deactivated the termination triggered by the assertion check:

Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES = false;

[nunoguedelha]

Simulation still fails at the very beginning due to:

This is caused by the controller diverging, resulting in computed NaN accelerations and subsequently a NaN rotation matrix, which causes the command norm to crash.

This is probably due to the fact that in its initial configuration the robot has both feet floating.

We need to fix the initial robot condition where the robot is standing on the ground and the joint configuration is the YOGA's joint initial configuration.

The YOGA initial joint configuration is defined in the homePoseBalancing.ini that can be found as follows:

> yarp resource --find homePoseBalancing.ini
.../robotology-superbuild/build/install/share/wbc/robots/iCubGenova04/homePoseBalancing.ini

The required configuration is under the tag [customPosition_YogaPP].

Set the initial conditions in robot_config:

initialConditions.base_position = [0; 0; 0.619];
initialConditions.orientation = diag([-1, -1, 1]);
...
% joint (inital) position
initialConditions.s = [
    0; 0; 0; ...
    -35.97; 29.97; 0.06; 50.00; ...
    -35.97; 29.97; 0.06; 50.00; ...
    0     ; 0    ; 0   ; 0    ; 0;     0; ...
    0     ; 0    ; 0   ; 0    ; 0;     0]*pi/180;

Status after first step (simulation time 0.001s)

Feet are touching the ground (slightly sinking) and ground reaction forces not null.

Behavior within 1s simulation

The robot looks initially stable, but the controller diverges quickly.

yogapp_failing_1.mp4

[nunoguedelha]

ping @traversaro , @gabrielenava

[nunoguedelha]

The cause is most probably the motor reflected inertia: I left it at 0. I have to set the proper values and retest.

[gabrielenava]

given the critical failure I suggest, in case you did not do it yet, to perform first a balancing demo instead of the Yoga, in order to reduce the complexity and isolate problems related to the simulator and problem related to gain tuning.

There are several way to skip the yoga demo, I usually set to inf (or a very long time) the tBalacing variable: https://github.com/robotology/whole-body-controllers/blob/master/controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/configStateMachine.m#L51

[nunoguedelha]

Yes @gabrielenava , indeed it was my intention in my next step. Thanks!

Well, with the reflected inertia it improved, and it fails later, after lifting the right foot from the ground. The jointss start jiggling. I also remembered that the simulator is unstable if we compensate the friction completely, which is the case here since it's a "perfect" torque control.

I'm going to retest with friction. And it still makes sense to test only the balancing for starting. I'll follow your suggestion on the tBalacing.

[nunoguedelha]

Reflected motor inertias and friction torques implemented...

Reflected motor inertias

Refer to ami-iit/matlab-whole-body-simulator#9 for the theory and the implementation in the core block RobotDynWithContacts from matlab-whole-body-simulator.

There is no motor nor joint model in the simulator sub-system in the model floating-base-balancing-torque-control-with-simulator implemented here, so we just added a Constant block with the inertias as an input to the RobotDynWithContacts block. Since some of iCub's joints are coupled, the coupling matrix $\Gamma$ is not diagonal (the gearbox ratio matrix $G$ is though). The same applies to the term $\Gamma^{-\top} G^{-1} I _m G^{-1} \Gamma^{-1}$, which now dos not have only diagonal terms and has to be passed to the RobotDynWithContacts in its full matrix form instead of a column vector.

Join friction torques

The friction torques are computed from the joint velocities and a simple viscous friction model.

• As the friction takes place in each individual motor (the rotor) and respective gearbox, the friction torques are also subject to the coupling.
• In order to keep things simple, and since we don't have an implemented motor model anyway, we assume that the simulation represents a system with motors controlled in current (actually the closest to the present iCub control scheme), such that we can neglect Back EMF effect and account only for the mechanical friction. We could use here any friction coefficient values that stabilizes the simulation, but it makes more sense to use as a baseline, prior to any tuning, the mechanical friction parameters identified on iCub, for instance on iCubGenova04 (https://github.com/dic-iit/element_torque-control-via-current/issues/49).

The new joint torque inputs to the block RobotDynWithContacts are computed as follows:

$$ \tau _{J} = \tau _J^* - \Gamma^{-\top} K _{\mathrm{vmech}} \Gamma^{-1} \dot{s} $$

Where $\tau*$ is the output of the controller, $\Gamma$ the coupling matrix and $K_v$ the diagonal matrix of absolute friction coefficients (absolute values of the ones identified in https://github.com/dic-iit/element_torque-control-via-current/issues/49).

For now I'm giving a try with same friction 0.5 Nm/(rad/s) for all joints.

[nunoguedelha]

Tests 1 to 3

• model iCubGazeboV2_5
• Balancing on two feet

Test 1

• Controller Motor reflected inertia OFF
• Simulator Motor reflected inertia OFF
• Simulator Friction = 0.0

=> Simulation diverges approx. after 1s.

Test 2

• Controller Motor reflected inertia OFF
• Simulator Motor reflected inertia ON
• Simulator Friction = 0.0

=> Simulation diverges approx. after 13s.

Test 3

• Controller Motor reflected inertia OFF
• Simulator Motor reflected inertia ON
• Simulator Friction = 0.2

=> Simulation stable over 1mn.

---

Tests 4 to 7

• model iCubGazeboV2_5
• YOGA++ , support on right foot

Test 4, 5, 6

• Controller Motor reflected inertia ON/OFF / Harmonic Drive inertia ON/OFF / Coupling ON/OFF
• Simulator Motor reflected inertia ON
• Simulator Friction = 0.2

=> As soon as it lifts the feet, the ankle starts oscillating, and after 5s the whole body.

Test 7

• Controller Motor reflected inertia OFF
• Simulator Motor reflected inertia ON
• Simulator Friction = 0.2 for all except the ankle pitch and roll = 0.6

=> More stable for the first 5s then same instability and divergence.

yogapp_failing_test_7_more_stable.mp4

At this point, activating the reflected inertia on the controller side does not improve the behavior.

[nunoguedelha]

In the previous tests we are also exceeding the joint limits, specially on the knee, going through a singular position. In the video below (side view of the test 7), we can see that the oscillations seem to begin when the knee is fully stretched.

yogapp_failing_test_7_side_view.mp4

If possible, I'll tune the center of mass to be lower, for avoiding the legs to stress so much and thus avoid the singular position of the knee.

[nunoguedelha]

@gabrielenava , while I'm doing my own intuitive tuning, could you give me a quick recap of the methodology you follow for tuning the YOGA? Typically the parameters you adjust most often in configStateMachine.m after changing the controller or simulation environment..

[nunoguedelha]

Checks done on the controller & simulation outputs

• No saturation desired joint torques
• Stable and coherent feet contact status
• Mass matrix condition number: high ~ 6700. No spike observed when the joint oscillations begin.

Test 8: Skip YOGA (support foot switching only)

• No saturation desired joint torques
• Stable and coherent feet contact status
• Mass matrix condition number: motor reflected inertia (on control side) improved the mass matrix condition number but simulation becomes unstable (uncontrolled oscillations) after 7s.

Mass matrix condition number ~6700 (Motor Reflected Inertia OFF)

Mass matrix condition number ~420 (Motor Reflected Inertia ON)

Skip Yoga visualization

https://user-images.githubusercontent.com/6848872/117012192-fb1b5b00-acee-11eb-9108-59eb98ee37e0.mp4	https://user-images.githubusercontent.com/6848872/117012229-04a4c300-acef-11eb-90e9-25b609f96b90.mp4
Motor ref. Inertia OFF	Motor ref. Inertia ON

[nunoguedelha]

https://user-images.githubusercontent.com/6848872/117055133-17cd8800-ad1b-11eb-96bc-75a9236186d6.mp4	Example of failure due to a singular position. The controller is trying to lower the center of mass (StateMachine.CoM_delta(9,3) = -0.10), but the legs are fully stretched, which is a singular position (we lost the DoF along the $z$ axis.)

[nunoguedelha]

Test 9: Full Yoga with support on the left foot

(commit f238595)

• Changed the initial joint configuration in order to bend the knees.
• No need to add an offset to the desired CoM since the initial joint configuration already has the knees bent. There is enough margin to maintain the CoM hight when transitioning the CoM to the left or ight foot.
• Increased the Gain.KD_CoM and Gain.KD_postural as the velocity feedback in the simulator is accurate. This stabilizes significantly the joint trajectory.

At this point, the YOGA with support on left foot works and is stable. The robot falls when attempting the second YOGA on the right foot.

yogapp_test_9_onLeftFootPASS_onRightFootFAIL_lightFormat.mp4

Thanks @gabrielenava for your help. As you suggested, I increased the CoM and postural gains. the full critical damping gains were causing the robot to lose balance and fall, so I've used 1/2 of the critical damping.

[nunoguedelha]

The robot falls when attempting the second YOGA on the right foot.

It's probably due to the Postural gain scheduling. For some reason, we're not getting symmetrical gains on the left and right ankles, and we can notice in the video that the robot falls apparently because the right ankle does not keep it's position as if the torque was insufficient:

More tomorrow...

[gabrielenava]

The robot falls when attempting the second YOGA on the right foot.

I wonder what happens if you perform the Yoga only on the left foot, and when the Yoga finishes you just remain into two feet balancing state (it is this option in the Yoga config file: https://github.com/robotology/whole-body-controllers/blob/master/controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/configStateMachine.m#L57).

If the robot can actually come back on two feet and balance, maybe the failure is a matter of timing in the state machine (the robot switches too early on right foot balancing). Another test could be to perform the Yoga on the right foot only and check if the robot behaviour is symmetrical w.r.t. the left (there may be differences in gains and references, the states of the state machine are different for left and right Yoga). Here the option to run the Yoga on right foot only: https://github.com/robotology/whole-body-controllers/blob/master/controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/configStateMachine.m#L56

[nunoguedelha]

Sure, I'll be trying that today. Also, for meeting the deadline, we can consider just doing the YOGA on the left foot.

CC @traversaro @DanielePucci

[nunoguedelha]

The YOGA on left foot only works.

In the plot below, we're running the YOGA left foot only and can see the ankle roll gain decrease to 5 along the state 7, and this happens because the scheduled target gain for the ankle roll is actually 5:

30   50   60   30   5   5,    30   30   30   20   5    5     % state ==  7  TRANSITION TO INITIAL POSITION

Indeed, we can see in the plot of the leg gains (#121 (comment)) the same behavior: the ankle gain decreases along the state 7. Since in that case we were doing also YOGA right foot, the gain starts increasing again as soon as we switch to state 8. But as @gabrielenava mentioned, there's not enough time for the gain to rise up again to 100 (the gain scheduled in state 8).

Side note: I don't see why the ankle gains were set so low in state 7 by the way.

For ruling out or confirming the problem with the gains or timing, I increase the balancing time StateMachine.tBalancing to 5 s, and started the YOGA directly on the right foot. The robot still falls, despite the constant scheduled gain of 100 as we can see in the plot below:

[nunoguedelha]

After further analysis, and after doing some adjustments that improved the behavior, avoiding some QP failures that were occurring when shifting the weight to the right foot, the robot is still falling. At this point it's better to proceed with the PR with only the YOGA left foot, and solve this problem later.

[traversaro]

After further analysis, and after doing some adjustments that improved the behavior, avoiding some QP failures that were occurring when shifting the weight to the right foot, the robot is still falling. At this point it's better to proceed with the PR with only the YOGA left foot, and solve this problem later.

It make sense!

[nunoguedelha]

For the record, this is the last PR to go, for completing the Epic https://github.com/dic-iit/element_software-engineering/issues/24 (apart from a bit of documentation).

[nunoguedelha]

The fix of the YOGA starting on the right foot shall be tracked in #127.

Closing this issue.