robot_utils and buildModelReduced

[olivier-stasse]

Symptoms:

The robot is pushing on its feet in torque control mode.

>>> robot.dynamic.com.value
(-0.001983704997041107, 7.19678203908911e-05, 0.8926753451423107)
>>> robot.cdc_estimator.c.value
()
>>> robot.cdc_estimator.c.recompute(0)
>>> robot.cdc_estimator.c.value
(0.003948910372791941, 0.00036058788430673737, 0.8746905831496146)

It looks like there is a model discrepancy.

Analysis of the problem:

Due to the fact that robot_utils is providing only the urdf file name and not the string of the model, this has lead to a model discrepancy. For the same reason sot-talos has been modified to use the rosparam urdf_description and build a reduced model:
https://github.com/stack-of-tasks/sot-talos/blob/64bec0f0b9f570da8a421bfd2439189e2cbfb80a/src/dynamic_graph/sot/talos/talos.py#L66
called in the pyrene prolog here:
https://github.com/stack-of-tasks/sot-talos/blob/64bec0f0b9f570da8a421bfd2439189e2cbfb80a/src/dynamic_graph/sot/pyrene/prologue.py#L31

This good policy is unfortunately overwrite by the param_server initialization specifying a talos_reduced.urdf file located in the package talos_data.
This initialization is done in package sot-talos-balance here:
stack-of-tasks/sot-talos/blob/master/src/dynamic_graph/sot/talos/talos.py#L66
and in package talos-torque-control here:
https://github.com/stack-of-tasks/talos-torque-control/blob/c6e59b30fde626a8304faea980348ff41e852005/python/dynamic_graph/sot/torque_control/talos/control_manager_conf.py#L14

Reading the robot_description ros parameter is not equivalent to read talos_reduced.urdf from the talos_data package. Not reading the robot constructor model has led us to many problems.

The control structure should have only one reference.
This result in a slightly different model creating a difference of 2cm in the CoM height.

Proposed solution:

Instead of passing a string with the location of the URDF model, we should provide the model string.
This would also make possible to modify the model on-line. We should also have a timestamp and the number of changes in robot_utils.

[NoelieRamuzat]

In fact in sot-talos-balance the urdf_path is set in the parameter_server_conf file here:
https://github.com/loco-3d/sot-talos-balance/blob/8749650fc5f512a04c349f447a0cfb0d9e0e1e05/src/sot_talos_balance/talos/parameter_server_conf.py#L13

It is the same behaviour than in talos-torque-control.

The rosparam seem not to be used in the initialization of the robot Talos (see https://github.com/stack-of-tasks/sot-talos/blob/64bec0f0b9f570da8a421bfd2439189e2cbfb80a/src/dynamic_graph/sot/pyrene/robot.py#L54), as the fromRosParam parameter is not set.
Then in fact the urdf file used is the one defined by defaultFilename = "talos_reduced_v2.urdf"in sot-talos (https://github.com/stack-of-tasks/sot-talos/blob/64bec0f0b9f570da8a421bfd2439189e2cbfb80a/src/dynamic_graph/sot/talos/talos.py#L31).

A quick fix for now is to replace the urdfFileName in the configuration files by the defaultFilename used in sot-talos, which means use the v2 of the reduced urdf instead of the v1.
Writing in the config files:
urdfFileName = rospack.get_path('talos_data') + "/urdf/talos_reduced_v2.urdf"
fix the problem on my side.

[jmirabel]

I don't know what this file: https://github.com/stack-of-tasks/talos-torque-control/blob/c6e59b30fde626a8304faea980348ff41e852005/python/dynamic_graph/sot/torque_control/talos/control_manager_conf.py#L14
is for but I think it is a really bad idea. Most of those information should be extracted from the model, rather than hardcoding them here.

[olivier-stasse]

Commit c239120 is storing the robot_description string in the parameter server object.
It has been used inside sot-talos and sot-pattern-generator.
A commit will follow in solving:

• the extraction of the mimic joint based on the python code inside sot-dynamic-pinocchio and the construction of a reduced Model
• a tree of properties using boost::property to handle various parameters which are spread over python files such as control_manager_conf.py or srdf files.