Force estimation and contact detection

[alaurenzi]

Hi @IoannisDadiotis ,
please have a look to this launch file in order to set up force estimation on centauro. Write here if you encounter any issue.

[IoannisDadiotis]

Hi @alaurenzi . I have an issue.

I tried to run the launch file but I am getting the following error:

terminate called after throwing an instance of 'std::runtime_error'
  what():  Could not find node 'centauro_cop_estimation' in package 'centauro_tools'
Aborted (core dumped)

I tried to manually find the centauro_cop_estimation node in the cartesio_tools repo but I couldn't.

[IoannisDadiotis]

I tried to run without the following node:

<node pkg="centauro_tools" 
          type="centauro_cop_estimation" 
          name="centauro_cop_estimation"
          output="screen">
		  
          <param name="rate" value="200.0"/>
		  
	</node>

and I get the error
what(): robot_description parameter not set

Adding the parameter:

<param name="robot_description"
         command="$(find xacro)/xacro --inorder '$(find centauro_urdf)/urdf/centauro_robot.urdf.xacro' add_upper_body:=$(arg arms) add_legs:=$(arg legs)"/>

gives me the following error:
what(): robot_description_semantic parameter not set

Finally, adding the parameter:
<param name="robot_description_semantic" textfile="$(find centauro_srdf)/srdf/centauro.srdf"/>
gives me the same error:
what(): robot_description_semantic parameter not set

[alaurenzi]

Yes you don't need centauro_cop_estimation

[alaurenzi]

Double check that rosparam get /robot_description_semantic returns the valid XML

[IoannisDadiotis]

rosparam get /robot_description_semantic returns ERROR: Parameter [/robot_description_semantic] is not set .

I am not sure if this is the right way but I mon launch centauro_cartesio centauro_force_estimation.launch after running the centauro in gazebo and xbot2-core -S.

When l mon launch centauro_cartesio centauro_force_estimation.launch alone, I get the error:

force_estimator: terminate called after throwing an instance of 'std::runtime_error'
     force_estimator:   what():  robot_description parameter not set
     force_estimator: force_estimator died from signal 6
     force_estimator: force_estimator left a core dump
     force_estimator: Determined pattern '/tmp/rosmon-node-CcBHMn/core'
     force_estimator: Found core file '/tmp/rosmon-node-CcBHMn/core'
     force_estimator: Could not remove process working directory '/tmp/rosmon-node-CcBHMn' after process exi
                   ~  t: Directory not empty

which means that neither robot_description nor robot_description_semantic are loaded from the centauro_force_estimation.launch .

[IoannisDadiotis]

I managed to run this.

Parametersrobot_description and robot_description_semantic must be loaded out of the <node> tag.

I will get back in case of other issues.

[alaurenzi]

I managed to run this.

Parametersrobot_description and robot_description_semantic must be loaded out of the <node> tag.

I will get back in case of other issues.

Right, otherwise they're created under the /node_name namespace

[IoannisDadiotis]

Where can I take a look at the code of the force_estimator node, mentioned in the launch file ?

With the parameter <param name="rate" value="200.0"/>, specified under the same node, can I define the frequency of the force estimation module (here 200 Hz) ?

[alaurenzi]

Yes. Since the code of our core repos (including CartesianInterface) is private, we'll have to ask @ntsagarakis if you want to have a look.

[IoannisDadiotis]

Ok, it's not urgent.

[IoannisDadiotis]

Continuing our discussions of today for the force estimation frames:
I write here some thoughts. @alaurenzi, I changed again the reference frames to wheel_* and, suprisingly, the noise we experienced is not here anymore (see following picture). Did you change something yesterday, that might be the cause for fixing this (joint gains or whatever) ?

About the selection of the appropriate frames for expressing the estimated forces:
When using either wheel_* or ankle2_* (see following two pictures) or any other link frames for force estimation, a transformation is always needed in order to compute the force components parallel and vertical to the contact surface (which is what we need at the end of the day). Maybe we could consider inserting this transformation in the force_estimator node (?)

Another option that we discussed is to define a new frame at the bottom of the wheel, but how is this going to save us from the need of the transformation? If I am not wrong, this frame will still have the one axis aligned with the line that connects the ankle frame and the center of the wheel (thus similar to ankle2_* frames), as can be seen in the following picture.

[alaurenzi]

Yes the transformation is needed regardless, but you get a more intuitive measurement close to the nominal homing configuration

[IoannisDadiotis]

I have created this repo for using contact detection as an independent module in any task with centauro.