Wrong estimation of torso pitch torque

[fils99]

Today, carrying out some experiments on ergoCubSN001, we noticed a strange behavior on torso pitch joint.
Setup of the experiment:

• The robot was hanging on the crane, without touching ground
• Whole-Body-Dynamics was launched without considering the FTs (leveraging the work done by @traversaro in Support specifying the FT sensors to use in the estimation via multipleAnalogSensorsNames::SixAxisForceTorqueSensorsNames without the need to modify the loaded URDF #194

Below, we show the estimated torque on the torso pitch during the experiment, in which we were driving such joint from a joint limit to the other one, in position control.

experiment.webm

It seems strange because the range of variation of the estimated joint torque is quite small, considering that we reach the two limit joint positions. Moreover, it is always positive. In the meanwhile, as we can se by the plot, the current changes sign.

cc @LoreMoretti @GiulioRomualdi

[GiulioRomualdi]

To be fair we also checked the IMU orientation and it seems fine, there is a slight error on the position in the urdf (@fils99, you may link the issue here). But I don't think this is the cause of this issue.

[fils99]

To be fair we also checked the IMU orientation and it seems fine, there is a slight error on the position in the urdf (@fils99, you may link the issue here). But I don't think this is the cause of this issue.

Here the issue icub-tech-iit/ergocub-software#260 (comment)

[GiulioRomualdi]

@traversaro mentioned via teams that we should set the default contact frame in the chest and not in the root link in wbd config file

[fils99]

@traversaro mentioned via teams that we should set the default contact frame in the chest and not in the root link in wbd config file

So we have to set default contact frame in the chest every time we exclude at least one FT sensor? Or only when we exclude all the FTs?

[traversaro]

The point Is:

The robot was hanging on the crane, without touching ground

If the robot Is hanging on the crane, the external force is on the chest, not ok the root_link, independently from the FTs used.

[traversaro]

It would be easier to understand if we visualized all the external forces published on the contacts:o port, not now but probably we should think of visualized that in some visualizer.

[fils99]

Trial 1

We tried to put default contact frame in the chest. Below the results:

1.webm

As we can see from the video, the behavior of the comment above persists. Moreover, with idyntree-yarp-visualizer we noticed also a strange force acting on the left hand of the robot.

Visualizer

Most likely is the force of traction of the crane on the robot (?), but why is it placed on the hand?

Trial 2

Commit: ami-iit/robots-configuration@f5d7a60

In this case, we are substituting every istance of root_link present in the file with chest (so, not only in the parameter defaultContactFrames).

Below the results

2.webm

On this time, the behavior of the torso pitch estimated torque seems more reasonable.

Btw, with idyntree-yarp-visualizer we still see the strange force acting on the left hand of the robot. Is it a problem of WBD or idyntree-yarp-visualizer :

Visualizer

[traversaro]

If you do not have any ft, then the only contact frame should be the chest one.

[fils99]

So in defafultContactFrames should I remove also l_hand_palm, l_foot_front and so on, right?

[fils99]

Problem Fixed

Today I modified the config file according to SIlvio's suggestion, and everything seem fine, both in the case of zero FTs considered and also in the case in which we use the feet FTs.

No FTs

Configuration wbd

<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">


<device xmlns:xi="http://www.w3.org/2001/XInclude" name="wholebodydynamics" type="wholebodydynamics">
  <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_roll,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_roll,r_wrist_pitch,r_wrist_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
  <param name="modelFile">model.urdf</param>
  <param name="fixedFrameGravity">(0,0,-9.81)</param>
  <!-- <param name="defaultContactFrames">(l_hand_palm,r_hand_palm,root_link,l_foot_front,l_foot_rear,r_foot_front,r_foot_rear,l_upper_leg,r_upper_leg)</param> -->
  <!-- <param name="defaultContactFrames">(chest, l_foot_front, l_foot_rear, r_foot_rear, r_foot_front)</param> -->
    <param name="defaultContactFrames">(chest)</param>
  <param name="imuFrameName">waist_imu_0</param>
  <param name="publishOnROS">true</param>
  <param name="forceTorqueEstimateConfidence">2</param>
  <param name="useJointVelocity">true</param>
  <param name="useJointAcceleration">false</param>
  <param name="streamFilteredFT">true</param>
  <param name="imuFilterCutoffInHz">3.0</param>
  <param name="forceTorqueFilterCutoffInHz">10.0</param>
  <param name="jointVelFilterCutoffInHz">10.0</param> <!-- used if useJointVelocity is set to true -->
  <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->
  <param name="startWithZeroFTSensorOffsets">true</param> <!-- bypass using resetOffset of FT sensors -->
  <param name="devicePeriodInSeconds">0.002</param>
  <param name="useSkinForContacts">false</param>
  <param name="publishNetExternalWrenches">true</param>
  <param name="assume_fixed">root_link</param>
  <group name="HW_USE_MAS_IMU">
    <param name="accelerometer">waist_imu_0</param>
    <param name="gyroscope">waist_imu_0</param>
  </group>

  <group name="multipleAnalogSensorsNames">
    <!-- <param name="SixAxisForceTorqueSensorsNames">(l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft, r_arm_ft, l_arm_ft)</param> -->
    <!-- <param name="SixAxisForceTorqueSensorsNames">(l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft, r_arm_ft, l_arm_ft)</param> -->
    <!-- <param name="SixAxisForceTorqueSensorsNames">(l_foot_front_ft, l_foot_rear_ft, r_foot_front_ft, r_foot_rear_ft)</param> -->
        <param name="SixAxisForceTorqueSensorsNames">()</param>

  </group>


  <group name="GRAVITY_COMPENSATION">
    <param name="enableGravityCompensation">true</param>
    <param name="gravityCompensationBaseLink">root_link</param>
    <param name="gravityCompensationAxesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
  </group>


  <group name="WBD_OUTPUT_EXTERNAL_WRENCH_PORTS">
    <param name="/wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o">(l_hand_palm,l_hand_palm,root_link)</param>
    <param name="/wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o">(r_hand_palm,r_hand_palm,root_link)</param>
    <param name="/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o">(l_lower_leg,l_lower_leg,root_link)</param>
    <param name="/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o">(r_lower_leg,r_lower_leg,root_link)</param>
    <param name="/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o">(l_foot_front,l_sole,l_sole)</param>
    <param name="/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o">(l_foot_rear,l_sole,l_sole)</param>
    <param name="/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o">(r_foot_front,r_sole,r_sole)</param>
    <param name="/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o">(r_foot_rear,r_sole,r_sole)</param>
  </group>

  <action phase="startup" level="15" type="attach">
    <paramlist name="networks">
      <!-- motorcontrol and virtual torque sensors -->
      <elem name="head-j0">head-eb20-j0_1-mc</elem>
      <elem name="head-j2">head-eb21-j2_3-mc</elem>
      <elem name="torso">torso-eb5-j0_2-mc</elem>
      <elem name="left_upper_arm-j0">left_arm-eb2-j0_1-mc</elem>
      <elem name="left_upper_arm-j2">left_arm-eb4-j2_3-mc</elem>
      <elem name="left_lower_arm">left_arm-eb31-j4_6-mc</elem>
      <elem name="right_upper_arm-j0">right_arm-eb1-j0_1-mc</elem>
      <elem name="right_upper_arm-j2">right_arm-eb3-j2_3-mc</elem>
      <elem name="right_lower_arm">right_arm-eb30-j4_6-mc</elem>
      <elem name="left_upper_leg-j0">left_leg-eb8-j0_3-mc</elem>
      <elem name="left_lower_leg-j2">left_leg-eb9-j4_5-mc</elem>
      <elem name="right_upper_leg-j0">right_leg-eb6-j0_3-mc</elem>
      <elem name="right_lower_leg-j2">right_leg-eb7-j4_5-mc</elem>

      <!-- imu -->
      <elem name="imu">waist-inertial</elem>
      <!-- ft -->
      <elem name="l_leg_ft_sensors">left_leg-FT_remapper</elem>
      <elem name="r_leg_ft_sensors">right_leg-FT_remapper</elem>
      <elem name="l_arm_ft_sensors">left_arm-FT_secondary_calib</elem>
      <elem name="r_arm_ft_sensors">right_arm-FT_secondary_calib</elem>
    </paramlist>
  </action>

  <action phase="shutdown" level="2" type="detach" />

</device>

Visualizer

With feet FTs

Commit: ami-iit/robots-configuration@c527497

Visualizer

Conclusions

In both the cases analyzed, when the robot is hanging on the crane, the force (and the related moment) are applied in the right place (i.e. the chest)

[traversaro]

Exactly, we can align in person to explain more how wbd algorithm work.

[fils99]

It's good to me, thanks!