Closed nunoguedelha closed 3 years ago
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:
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 considerFrom the RobotDynamicsWithContacts
code analysis we identified the following aspects to take in account in the substitution.
Robot
, Contacts
, State
) to use bindings is the class Robot
(except eventually for some isolated case)iDynTree::KinDynComputations
, called through an object created in iDynTreeWrappers.loadReducedModel()
function which loads the robot model from a file.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
.
So the following steps are:
Configuration
block in the main Simulink test model and set its configuration as usual, through a variable of class WBToolbox.Configuration
.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".iDynTree
object, either called through the iDynTree::KinDynComputations
object.quadprog
.CC @CarlottaSartore @Giulero
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).
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.
Now Fixing compilation & run-time errors. Fixed so far:
quadprog
. Use qpOASES Simulink block instead through a Simulink function simFunc_qpOASES
sub-system. Selection is done through setOSQP
and useQPOASES
bool flags.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.x_iDyn
variables formerly used for interfacing bindings functions.optimoptions
, quadprog
or osqp
binding will cause the code generation
compiler to fail even if they are wrapped in a if or switch condition.mat2cell
, implicit indexing ":".Fixed other size mismatch errors by:
:
, 1:end
) by explicit indexing.A = []; A = [A;B]
).The Simulator now compiles and runs, with the following behavior:
devel
branch).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.
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...
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.
Implemented in https://github.com/dic-iit/matlab-whole-body-simulators/pull/15, with the fixes described above.
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:
compute_contact_points()
compute_velocity()
~forces
computation in compute_unilateral_linear_contact()
~A
computation in compute_unilateral_linear_contact()
~Looking now for the root cause of the foot contact failure. Check:
- Contact points before and after impact in compute_contact_points()
left_right_foot_in_contact
is set to 1 when the right foot gets in contact with the ground at t=0.272 sec
.mapVerticesNewContact = obj.is_in_contact & ~obj.was_in_contact
J_feet
was also wrong.Fixed in https://github.com/dic-iit/matlab-whole-body-simulators/pull/15/commits/228d700a3687a3463aea452f958e5daaf102538b of https://github.com/dic-iit/matlab-whole-body-simulators/pull/15.
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:
compute_contact_points()
compute_velocity()
forces
computation in compute_unilateral_linear_contact()
=> WRONG FORCES COMPUTED :warning:A
computation in prepare_optimization_matrix()
Aeq
computation in compute_unilateral_linear_contact()
=> BLOCK WITH ZEROS CAUSING QPOASES FAILURE :warning: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
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.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
computationThe computation itself is correct. But another problem arises when multiple vertices are in contact:
Aeq
is only zeros.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 https://github.com/dic-iit/matlab-whole-body-simulators/pull/15/commits/f2184bded2ffdc322602cef45ebb3b7d16a020da of the ongoing pull request.
C.C. @VenusPasandi
Last issue fixed in https://github.com/dic-iit/matlab-whole-body-simulators/pull/15/commits/f2184bded2ffdc322602cef45ebb3b7d16a020da of the ongoing pull request.
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:
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
.
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.
Actually, the last bottleneck slowing down the simulation was the visualizer, as we'll show in the following analysis.
Related PR: https://github.com/robotology/idyntree/pull/744 .
Indeed, with a faster visualizer we could increase the display frequency and get a smoother motion in the visualizer.
The MATLAB visualizer improvement will be dealt with in https://github.com/robotology/idyntree/pull/744.
The other improvements have been implemented in https://github.com/dic-iit/matlab-whole-body-simulator/pull/15. Closing this issue.
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
The usage of bindings was initially motivated by the fact that it allows more effcient design of complex algorithms.
Fixed by #15.