Integrate the YOGA++ (floating-base-balancing-torque-control) with the matlab-whole-body-simulator #122
Conversation
…licate of floating-base-balancing-torque-control
- Replaced Gazebo interface blocks (setTorque, getPosition, etc) by:
- RobotDynWithContacts,
- IMUsensor,
- RobotVisualizer.
- Added configRobotSim.m config file specfic to the simulation, making
sure there were no collisions with configRobot.m.
- Defined Visualizer init configuration.
…kHz and the controller at 100Hz
- Define additional joints for the simulation robot model: neck joints.
The neck joints are not part of the joints controlled by the high-level torque controller.
- Fixes in the controller sub-system for settling the same Config.tStep sample time in all
the sub-system blcoks:
- 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").
- In the "neck transform" block, replaced port read of neck joints by input from the simulator.
(Routed neck position from main inputs of the controller system)
(Added selectors to the controller inputs for removing the neck joints, as these are processed
separately)
- Added a zero order hold to the clock input of the state machine, otherwise the sampling time type
becomes "continuous" and propagates throughout the model, preventing the compilation.
- Added a unit delay between the controller output and the simulator.
- Other minor fixes in the controller sub-system, inr:
- Removed "measured joint torques" from the 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).
- Treat the controller sub-system as an atomic unit.
…UM_BASED_TORQUE_CONTROL" as atomic unit. - Moved "Dump and visualze" into "MOMENTUM_BASED_TORQUE_CONTROL" sub-system. - Moved the "STATE_MACHINE" clock outside the controller sub-system. - Removed zero-order holder in front of that clock. - Set all controller input ports sample time to inherited. - Treat "MOMENTUM_BASED_TORQUE_CONTROL" sub-system as an atomic unit. - Replaced delay block by memory block between the controller and the simulator. - Fixed model tasking and sampling time options: Removed "Treat each dicrete rate as a separate task" option, which was causing data rates transition problems between the simulator (1kHz) and the controller (100Hz). - The model is now fully compiling.
…d joints in simulation
- Used QP interface with constraints lower bound input "lbA". Set lbA=-Inf. - For more details refer to original issue tracker https://github.com/dic-iit/element_ironcub-control/issues/164
…+ configuration) - Base position was set to [0 0 0.619], tuned to get positive contact forces from the very first simulation step.
…r as an atomic unit at 40ms sampling time
…ualizer at 10ms - Tuning tests 1 to 3 (Refer to #121) - Tuning tests 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
- 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.
- Changed the initial joint configuration in order to bend the knees. - lower by 10cm the desired CoM hight: `StateMachine.CoM_delta(:,3) = -0.10`.
…`. Increase KD gains. - 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. - Increase 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.
…GA left foot only
How to test this PR
|
|
@traversaro @gabrielenava , the PR is ready for review. |
|
Thanks @nunoguedelha , two comments:
|
|
Tested following #122 (comment) on my PC with Ubuntu 20.04 LTS. Seems working fine: I've found it a bit slow in the A couple of comments:
|
|
Sure @gabrielenava , I'll do a mini-PR to update the documentation covering all the points you've listed. Thanks. |
and addressing this too @traversaro .
Yes indeed.
It makes usually sense to set it separately when there are several models. It makes also sense to have a working model set by default, and right now |
It's slow for me too. Probably because there are more sub-systems treated as atomic blocks (the cntroller, the simulator, the visualizer), which results in more code generation, which is slower at compilation but faster at runtime. |
Update with w.r.t. changes from after #122.
Update with w.r.t. changes from after #122.
- Update README (after #122) - Converted floating-base-balancing-torque-control-with-simulator into a package +floatingBaseBalancingTorqueControlWithSimulator - Added the package installation through CMake.
Implements #121 .