Misalignment between Cartesio states and tf from rviz

[FrancescoRuscelli]

When starting Cartesio (using roslaunch), I noticed a misalignment in the values between:

1. cartesian/l_sole/state (or r_sole, I'm not sure about the others)
2. tf tf_echo ci/l_sole ci/world_odom

Shouldn't they be the same (more or less) value?

Roslaunch takes in the full robot urdf and srdf (cogimon.urdf and cogimon.srdf)
get_xbot_config returns cogimon_simple_example

I noticed that the problem is in the srdf file, caused by the starting values of each joint. Changing the values in the srdf creates discrepancies between the /cartesian states and the tf values read while Cartesio is running.

[alaurenzi]

Indeed they should be exactly the same value when the program starts
Useful information might be:

• is this consistent across all tasks?
• what is the actual difference?
• is the robot (or XbotCore -D) running when the program starts?

[FrancescoRuscelli]

All tasks values are equal to the relative tf readings (tf tf_echo ci/joint ci/world_odom) except:

1. l_sole
   

2. r_sole
   

the difference for l_sole (and r_sole) is:

• cartesian/l_sole/state: position x = -0.087

• tf tf_echo ci/l_sole ci/world_odom: position x = 0.03

---

XBotCore is not running in the background. The only programs launched are:

• roslaunch myname.launch (name of my project)
• rviz

If I use my custom srdf, the values when the program starts are the same. If I use a different srdf, the cartesian values are different from the tf ones.

MY SRDF:

<joint name="WaistYaw" value="0.0"/>
<joint name="WaistLat" value="0.0"/>
<joint name="RShSag" value="0.0"/>
<joint name="RShLat" value="0.0"/>
<joint name="RShYaw" value="0.0"/>
<joint name="RElbj" value="0.0"/>
<joint name="RForearmPlate" value="0.0"/>
<joint name="RWrj1" value="0.0"/>
<joint name="RWrj2" value="0.0"/>
<joint name="LShSag" value="0.0"/>
<joint name="LShLat" value="0.0"/>
<joint name="LShYaw" value="0.0"/>
<joint name="LElbj" value="0.0"/>
<joint name="LForearmPlate" value="0.0"/>
<joint name="LWrj1" value="0.0"/>
<joint name="LWrj2" value="0.0"/>
<joint name="RHipLat" value="0.0"/>
<joint name="RHipSag" value="-0.4"/>
<joint name="RHipYaw" value="0.0"/>
<joint name="RKneePitch" value="0.8"/>
<joint name="RAnklePitch" value="-0.4"/>
<joint name="RAnkleRoll" value="-0.0"/>
<joint name="LHipLat" value="-0.0"/>
<joint name="LHipSag" value="-0.4"/>
<joint name="LHipYaw" value="0.0"/>
<joint name="LKneePitch" value="0.8"/>
<joint name="LAnklePitch" value="-0.4"/>
<joint name="LAnkleRoll" value="0.0"/>

---

Different SRDF (for which the misalignment appears):

<joint name="WaistYaw" value="0.0"/>
<joint name="WaistLat" value="0.0"/>
<joint name="RShSag" value="0.959931"/>
<joint name="RShLat" value="-0.007266"/>
<joint name="RShYaw" value="-0.3"/>
<joint name="RElbj" value="-1.919862"/>
<joint name="RForearmPlate" value="0.2"/>
<joint name="RWrj1" value="-0.523599"/>
<joint name="RWrj2" value="0.0"/>
<joint name="LShSag" value="0.959931"/>
<joint name="LShLat" value="0.007266"/>
<joint name="LShYaw" value="0.3"/>
<joint name="LElbj" value="-1.919862"/>
<joint name="LForearmPlate" value="-0.2"/>
<joint name="LWrj1" value="-0.523599"/>
<joint name="LWrj2" value="0.0"/>
<joint name="RHipLat" value="0.0"/>
<joint name="RHipSag" value="-0.363826"/>
<joint name="RHipYaw" value="0.0"/>
<joint name="RKneePitch" value="0.731245"/>
<joint name="RAnklePitch" value="-0.307420"/>
<joint name="RAnkleRoll" value="-0.0"/>
<joint name="LHipLat" value="-0.0"/>
<joint name="LHipSag" value="-0.363826"/>
<joint name="LHipYaw" value="0.0"/>
<joint name="LKneePitch" value="0.731245"/>
<joint name="LAnklePitch" value="-0.307420"/>
<joint name="LAnkleRoll" value="0.0"/>

---

myname.launch is:

<param name="robot_description" textfile="/home/francesco/advr-superbuild/configs/ADVR_shared/cogimon/urdf/cogimon.urdf"/>
<param name="robot_description_semantic" textfile="/home/francesco/advr-superbuild/configs/ADVR_shared/cogimon/srdf/cogimon.srdf"/>
<param name="problem_description" textfile="/home/francesco/advr-superbuild/external/limit_cycle_walking/cogimon_walker/VC_cartesio/problems/comanPlus_VC_stack.yaml"/>

<include file="$(find cartesian_interface)/launch/cartesio_include.launch"/>

______________________________________________________

[alaurenzi]

Commit?

[FrancescoRuscelli]

CartesianInterface is at commit:

• [BUGFIX] com waypoints

[FrancescoRuscelli]