search

robotology / whole-body-controllers

Simulink-based whole body controllers for humanoid robots.
116 stars 43 forks source link

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

Closed nunoguedelha closed 3 years ago

nunoguedelha commented 3 years ago

Moved from https://github.com/dic-iit/matlab-whole-body-simulator/issues/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

nunoguedelha commented 3 years ago

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:

nunoguedelha commented 3 years ago

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:

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:

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:

image

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

nunoguedelha commented 3 years ago

Simulation run fails at the very beginning:

image

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.

image

The workaround fixed the problem!

nunoguedelha commented 3 years ago

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

image

Deactivated the termination triggered by the assertion check:

Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES = false;
nunoguedelha commented 3 years ago

Simulation still fails at the very beginning due to:

image

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.

image
image image

Behavior within 1s simulation

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

https://user-images.githubusercontent.com/6848872/116841609-00d34c80-abda-11eb-9784-ff159ef2a690.mp4

nunoguedelha commented 3 years ago

ping @traversaro , @gabrielenava

nunoguedelha commented 3 years ago

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

gabrielenava commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

Reflected motor inertias and friction torques implemented...

Reflected motor inertias

Refer to https://github.com/dic-iit/matlab-whole-body-simulator/pull/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.

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 commented 3 years ago

Tests 1 to 3

Test 1

=> Simulation diverges approx. after 1s.

Test 2

=> Simulation diverges approx. after 13s.

Test 3

=> Simulation stable over 1mn.

Tests 4 to 7

Test 4, 5, 6

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

Test 7

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

https://user-images.githubusercontent.com/6848872/116960382-802b5380-aca0-11eb-91b7-665745a82203.mp4

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

nunoguedelha commented 3 years ago

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.

https://user-images.githubusercontent.com/6848872/116960802-c634e700-aca1-11eb-82e4-38b780500ee6.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 commented 3 years ago

@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 commented 3 years ago

Checks done on the controller & simulation outputs

Test 8: Skip YOGA (support foot switching only)

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

image

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

image

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 commented 3 years ago
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 commented 3 years ago

Test 9: Full Yoga with support on the left foot

(commit f2385950fc2fc35f913019fdfb4adbacb1f6b20b)

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.

https://user-images.githubusercontent.com/6848872/117080483-36914600-ad3e-11eb-933d-81ab9d134162.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 commented 3 years ago

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:

image

More tomorrow...

gabrielenava commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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
image

Indeed, we can see in the plot of the leg gains (https://github.com/robotology/whole-body-controllers/issues/121#issuecomment-832382848) 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:

image
nunoguedelha commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

The fix of the YOGA starting on the right foot shall be tracked in https://github.com/robotology/whole-body-controllers/issues/127.

Closing this issue.