Improve whole-body and contacts simulator core execution speed

[nunoguedelha]

Currently in the worst case the simulation takes 15 sec for 1 sec (Real Time factor = 1/15 = 0.06). The identified bottleneck is the library RobotDynamicsWithContacts.

Identified Reasons

• Matlab interpreted vs Code generation;
• Code generation is forbidden because of used bindings;

The usage of bindings was initially motivated by the fact that it allows more effcient design of complex algorithms.

Fixed by #15.

[nunoguedelha]

Proposed solution

• Replace bindings by Simulink functions;
• These functions trigger Optimized blocks “code generation ready”;
• Requires creation of a few WBT/Factory blocks as iDynTree proxies;
• Factorize calls to these WBT blocks.

We cannot replace the bindings by Simulink function calls combined with Simulink WBT blocks in a one-to-one mapping basis, mostly for the following reasons:

• it would require a great number of additional WBT blocks to be created,
• the interface of such blocks only supports numerical inputs/outputs, so it does not support objects, while many functions in the usual bindings return or take objects as inputs (Swig objects, etc).
• after a quick analysis of the RobotDynamicsWithContacts code, we can realize that the simplest approach for now is to address the substitution of the bindings in a case by case basis.

RobotDynamicsWithContacts design aspects to consider

From the RobotDynamicsWithContacts code analysis we identified the following aspects to take in account in the substitution.

• The only class among the three (Robot, Contacts, State) to use bindings is the class Robot (except eventually for some isolated case)
• Most of the used bindings are methods of the class iDynTree::KinDynComputations, called through an object created in iDynTreeWrappers.loadReducedModel() function which loads the robot model from a file.
• The loading of the robot model is actually already done by any existing WBT block implementation through the call of RobotInterface::getKinDynComputations(), which either initializes a new KinDynComputations object, either reuses an existing one, which is equivalent to the usage of a singleton.

So we just need to use a Configuration WBT block in order to handle the robot model loading, and create new blocks, if needed, for replacing the used bindings within KinDynComputations.

[nunoguedelha]

Implementation steps

So the following steps are:

• Add a Configuration block in the main Simulink test model and set its configuration as usual, through a variable of class WBToolbox.Configuration.
• Make it clear in the RobotDynamicsWithContacts library interface that it depends on a Configuration block. This can easily be done through a mask which would include, in addition to the other input parameters, a toggle button "Toggle Config block highlighting".
• List all the called iDynTree bindings methods, either directly called through the iDynTree object, either called through the iDynTree::KinDynComputations object.
• Replace the binding methods already implemented in an existing WBT block.
• Create WBT blocks for the missing methods and replace the respective bindings. Used existing qpOASES block instead of OSQP and Matlab quadprog.

[nunoguedelha]

CC @CarlottaSartore @Giulero

[nunoguedelha]

WIP: List all the called iDynTree bindings methods

List all the called iDynTree bindings methods, either directly called through the iDynTree object, either called through the iDynTree::KinDynComputations object.

method	Existing	To Be implemented	Step
iDynTree.ModelLoader	x (*)		1
iDynTree.ModelLoader.model	x (*)		1
iDynTree.KinDynComputations.setRobotState	x (*)		4
iDynTree.KinDynComputations.getFrameIndex	x (**)		4
iDynTree.FreeFloatingGeneralizedTorques	InverseDynamics		4
iDynTree.KinDynComputations.getFreeFloatingMassMatrix	MassMatrix		4
iDynTree.KinDynComputations.generalizedBiasForces	GetBiasForces		4
iDynTree.KinDynComputations.getFrameFreeFloatingJacobian	Jacobian		4
iDynTree.KinDynComputations.getFrameBiasAcc	DotJNu		4
iDynTree.KinDynComputations.getWorldTransform	ForwardKinematics		4
iDynTree.KinDynComputations.setFloatingBase		x (***)	5

(*) Initialized in the library through the loadReducedModel function call:

KinDynModel.kinDynComp = iDynTree.KinDynComputations();
KinDynModel.kinDynComp.loadRobotModel(reducedModel);
KinDynModel.kinDynComp.setFloatingBase(KinDynModel.BASE_LINK);

... it is used for computing the floating base system state, dynamics, and kinematics. The model loading and initialization of the WBT shared KinDynComputations object (singleton) is actually done by any WBT block present in the system. In such case, KinDynModel.BASE_LINK is the base of the tree induced by the URDF loaded in iDynTree.

(**) Can be avoided by using directly the frame name (string) as a block parameter if this parameter is set once at initialization

(***) Not required for now as the computed default floating frame is the correct one (base/root link of the URDF model tree).

[nunoguedelha]

• iDynTree.ModelLoader
• iDynTree.ModelLoader.model
• iDynTree.KinDynComputations.setRobotState
• iDynTree.KinDynComputations.getFrameIndex
• iDynTree.FreeFloatingGeneralizedTorques => was used in an initialization which is not needed anymore.
• iDynTree.KinDynComputations.getFreeFloatingMassMatrix
• iDynTree.KinDynComputations.generalizedBiasForces
• iDynTree.KinDynComputations.getFrameFreeFloatingJacobian
• iDynTree.KinDynComputations.getFrameBiasAcc
• iDynTree.KinDynComputations.getWorldTransform
• iDynTree.KinDynComputations.setFloatingBase => not needed. The default base link is already the correct one: torso_link. As explained in this comment:

the default base link is not the first link read from the URDF, but rather is the root node of the directed tree (or directed acyclic graph, see https://en.wikipedia.org/wiki/Tree_(graph_theory) ) induced by the URDF file. See https://github.com/robotology/idyntree/blob/v2.0.2/src/model_io/urdf/src/URDFDocument.cpp#L87 for the related code.

[nunoguedelha]

Now Fixing compilation & run-time errors. Fixed so far:

• OSQP Matlab bindings not supported, nor quadprog. Use qpOASES Simulink block instead through a Simulink function simFunc_qpOASES sub-system. Selection is done through setOSQP and useQPOASES bool flags.
• Some fields of the robot_config input of step_block Matlab System are modified or modifiable in run-time, so they
  cannot be Tunable. Don't set this input to Nontunable.
• Nontunable inputs have to be initialized.
• Cleanup x_iDyn variables formerly used for interfacing bindings functions.
• Matrices need to be properly initialized (sparse iterative filling of matrix, as done in Matlab, is not supported,
  dynamic size matrices).
• Unsupported features or functions like optimoptions, quadprog or osqp binding will cause the code generation
  compiler to fail even if they are wrapped in a if or switch condition.
• Other features not supported in code generation are: mat2cell, implicit indexing ":".

[nunoguedelha]

Fixed other size mismatch errors by:

• replacing implicit indexing (:, 1:end) by explicit indexing.
• replacing variables used in explicit indexing by constants when necessary.
• using constants in "if" conditions instead of variables, in order to make sure that we do not attempt to compile conditional code which is not supported by the code generator.
• refactor patterns not supported by the code generator (e.g. A = []; A = [A;B]).
• Remove persistent variables.

The Simulator now compiles and runs, with the following behavior:

• ⛔ ⚠️ the robot is not moving ⚠️ . Probably the robot state is not updated. To be analyzed.
• ✅ it runs 3x faster then before optimization (devel branch).

[nunoguedelha]

Issue Analysis: Robot not moving through the whole simulation

For debugging the issue, we set the initial robot base pose to a significant height (e.g. 10m), in order to check a simple scenario, without link/ground impacts:

initialConditions.base_position = [0; 0; 10.65];

As the Matlab System block is now compiled with code generation, the usual MATLAB breakpoint workflow is not available, so we use the MATLAB display command to log critical computed variables. This way, we compare the dynamic quantities computed in step_block.stepImpl between the current version and the devel branch.

The target quantities are the outputs of obj.contacts.compute_contact() and obj.robot.forward_dynamics(). As expected, we observe significant differences on the obj.robot.forward_dynamics() outputs: base_pose_ddot, s_ddot, and more precisely, the computation of the bias forces h = obj.get_bias_forces(). The faulty code is computing h as all zeros instead of the proper values.

Side Note

The expected bias forces are (first 3 components):

    0.0000
    0.0000
  324.3350

Which result in the base linear acceleration: [0 0 -9.81].

There is probably an issue with the WBT GetBiasForces block configuration.

Looking now closer at the problem...

[nunoguedelha]

Issue Analysis: Robot not moving through the whole simulation

There is probably an issue with the WBT GetBiasForces block configuration.

Looking now closer at the problem...

Just found the bug. Because of a double definition, the passed gravity vector was [0 0 0 0 -9.81] instead of [0 0 -9.81], so the resulting gravity value was null as only the first 3 passed components are considered valid.

[nunoguedelha]

Implemented in #15, with the fixes described above.

[nunoguedelha]

Issue: When hitting the round, the visualizer crashes due to singular matrix computation

The issue is actually caused by the fact that the robot right foot sinks in the ground (contact failure), creating very high reaction forces causing the simulation to diverge.

Looking now for the root cause of the foot contact failure. Check:

• Contact points before and after impact in compute_contact_points()
• Velocities correction in compute_velocity()
• forces computation in compute_unilateral_linear_contact()
• A computation in compute_unilateral_linear_contact()

[nunoguedelha]

Looking now for the root cause of the foot contact failure. Check:

• Contact points before and after impact in compute_contact_points()

• Flag left_right_foot_in_contact is set to 1 when the right foot gets in contact with the ground at t=0.272 sec.

• The mapping of the new contact points was wrong. A new contact is considered when a vertex was not in contact and now is: mapVerticesNewContact = obj.is_in_contact & ~obj.was_in_contact
• The computation of the respective indexes in J_feet was also wrong.

Fixed in 228d700 of #15.

[nunoguedelha]

Issue: WIP - Only one of the feet is staying "above the ground"

After the robot sinks in the ground, only the vertices of the right foot stay on the ground, the other foot sinks far under the ground level.

our branch	devel branch

This is probably related, again, with the contact mapping/index computations and velocities corrections. We'll check:

• Contact points before and after impact in compute_contact_points()
• Velocities correction in compute_velocity()
• ⚠️ forces computation in compute_unilateral_linear_contact() => WRONG FORCES COMPUTED ⚠️
• A computation in prepare_optimization_matrix()
• ⚠️ Aeq computation in compute_unilateral_linear_contact() => BLOCK WITH ZEROS CAUSING QPOASES FAILURE ⚠️

Wrong Forces Computation

We checked the mapping of the contact points (vertices that were, and/or are in contact, respective built jacobians, etc) and it is working properly. The contacts are detected properly, as well as the impact detection (when a vertex was not in contact and now is), but the respective computed forces are always 0. Typically, after dropping iCub from 1m high, the first computed set of linear forces at the vertices impacting the ground should give:

forces =
    1.1385
    0.0341
   60.0392
    1.1385
    0.0181
   29.8397
    1.1200
    0.0181
   35.6433
    1.1200
    0.0341
   66.7153
    1.2473
   -0.0205
   60.0591
    1.2372
   -0.0456
   29.2757
    1.2701
   -0.0456
   35.7178
    1.2701
   -0.0205
   66.7359

but give all zeros (values < 10e-7) instead, this is why the feet sink into the ground after impact and although the first velocity correction is done properly at impact. After impact, the feet stay in contact, there is no new contact detected, and the simulation behavior depends solely on the ground reaction forces computation, output of the QR problem solver. The QR processing constraints include the equality constraints Aeq x = 0 which make sure that the reaction forces are null if the foot is not in contact. We observe in the logs that

• these forces are ~ 0 and have the always the same value, before and during impact. This means probably that there's an issue with Aeq, as soon as a vertex contact is detected, the respective line in Aeq is set to all zeros, otherwise, the computed forces stays unchanged and ~ 0.
• the QP solver fails

Warning: block 'test_matlab_system_19/RobotDynWithContacts/Simulink Function8/QP': Internal qpOASES error. Trying to solve the problem with the remaining number of iterations.
forces =

So next check to do is the Aeq computation in compute_unilateral_linear_contact().

Aeq computation

The computation itself is correct. But another problem arises when multiple vertices are in contact:

• When a vertex is in contact, the respective line in Aeq is only zeros.
• When all the vertices of the feet are in contact with the ground, Aeq is a null matrix, and such null block in the overall contraints matrix causes qpOASES solver to fail, and the contact forces to be null.

For avoiding this issue, we use directly the lower/upper bounds inputs for the solver problem variable. The constraint F=0 is set using the "lb=ub=0" inputs, while an unconstrained F is obtained by setting "lb=-inf, ub=inf".

This could have been obtained also by having Aeq=Identity and changing dynamically beq between 0 and inf the same way as lb, ub.

Fixed in f2184bd of the ongoing pull request.

[CarlottaSartore]

C.C. @VenusPasandi

[nunoguedelha]

Last issue fixed in f2184bd of the ongoing pull request.

[nunoguedelha]

Execution speed analysis using the Simulink profiler:

Actually, the last bottleneck slowing down the simulation was the visualizer, as we'll show in the following analysis. We illustrate below the blocks we will focus on in the execution profiler...

The Robot visualizer/MATLAB System and 9 Simulink functions:

The Robot visualizer/MATLAB System in the Robot visualizer subsystem:

Profile with the visualizer on

We obtained the following profile for a total simulated time of 1s:

Final simulation state:

The step time used for the visualizer (confVisualizer.tStep) is 40ms, the total simulation time 1s.

As we can see in the profile report, it took ~48s for simulating 1s, so a rate of 1/48. The time shares for the blocks RobotDynWithContacts/MATLAB_System, Robot visualizer/MATLAB_System, RobotDynWithContacts/Simulink_Function8 are:

Block	Time share
test_matlab_system_19 run time	69.2 %
Robot visualizer/MATLAB System	49.6 %
RobotDynWithContacts/MATLAB System	18.5 %
RobotDynWithContacts/Simulink Function8 (QP)	2.0 %

The time share for the other Simulink functions (Function, Function1, Funciton2, ... Function7) is comparable to the QP function. The individual share of the QP is negligible w.r.t. the Robot visualizer/MATLAB System.

Profile with the visualizer off (commented)

The simulation to real time rate is now 1/15, so the simulation is 3x faster. If we don't count the compile time, we get a ~1/7 rate.

We can achieve the same performance with the visualizer if we treat it as an atomic unit and use a sample time of 40ms instead of 1ms, and treat it as an atomic unit the fake controller block as well, with a sample time of 1ms. This last detail is important as the sample time used by the RobotDynWithContacts block is inherited.

We didn't observe significant improvement by treating the RobotDynWithContacts as an atomic unit. So we can conclude it is not necessary at this point. It can be done separately anyway in any model sub-system including this library, by treating the sub-system as atomic unit.

[traversaro]

Actually, the last bottleneck slowing down the simulation was the visualizer, as we'll show in the following analysis.

Related PR: robotology/idyntree#744 .

[nunoguedelha]

Indeed, with a faster visualizer we could increase the display frequency and get a smoother motion in the visualizer.

[nunoguedelha]

The MATLAB visualizer improvement will be dealt with in robotology/idyntree#744.

The other improvements have been implemented in #15. Closing this issue.