contact points and contact forces between iCub's feet and the ground

[azadm]

I am trying to find out the contact points between iCub's feet and the ground and also the contact forces. When I put an iCub model in Gazebo and type gz topic -e /gazebo/default/physics/contacts in a terminal I get a list of the contact points (at each foot) and contact wrenches for every millisecond. For example:

contact {
  collision1: "iCub::r_foot::r_foot_collision"
  collision2: "ground_plane::link::collision"
  position {
    x: 0.91273593824525823
    y: -0.90038480816307331
    z: -8.31729668954867e-06
  }
  position {
    x: 0.88173087342535228
    y: -0.92674403534118077
    z: -1.7523234580950342e-05
  }
  position {
    x: 1.01868494731773
    y: -0.95959901052433838
    z: -5.72346394329265e-06
  }
  position {
    x: 1.0200467515764722
    y: -0.90339696372054124
    z: -2.486837448557877e-06
  }
  position {
    x: 1.0381695868547707
    y: -0.93161418047359168
    z: -6.7722325965205443e-06
  }
  normal {
    x: 0
    y: 0
    z: 1
  }
  normal {
    x: 0
    y: 0
    z: 1
  }
  normal {
    x: 0
    y: 0
    z: 1
  }
  normal {
    x: 0
    y: 0
    z: 1
  }
  normal {
    x: 0
    y: 0
    z: 1
  }
  depth: 8.31729668954867e-06
  depth: 1.7523234580950342e-05
  depth: 5.72346394329265e-06
  depth: 2.486837448557877e-06
  depth: 6.7722325965205443e-06
  wrench {
    body_1_name: "iCub::r_foot::r_foot_collision"
    body_1_id: 75
    body_2_name: "ground_plane::link::collision"
    body_2_id: 5
    body_1_wrench {
      force {
        x: 58.275357439213586
        y: -1.4467332837301103
        z: 1.0564902389138431
      }
      torque {
        x: 0.14463716063610363
        y: 4.5420966482475729
        z: -1.758255559495653
      }
    }
    body_2_wrench {
      force {
        x: 0
        y: 0
        z: 0
      }
      torque {
        x: 0
        y: 0
        z: 0
      }
    }
  }
  wrench {
    body_1_name: "iCub::r_foot::r_foot_collision"
    body_1_id: 75
    body_2_name: "ground_plane::link::collision"
    body_2_id: 5
    body_1_wrench {
      force {
        x: 20.457062766427324
        y: -1.7425400103679374
        z: 1.0268374052472291
      }
      torque {
        x: 0.19353832589452949
        y: 2.2272301811703992
        z: -0.076144457871965249
      }
    }
    body_2_wrench {
      force {
        x: 0
        y: 0
        z: 0
      }
      torque {
        x: 0
        y: 0
        z: 0
      }
    }
  }
  wrench {
    body_1_name: "iCub::r_foot::r_foot_collision"
    body_1_id: 75
    body_2_name: "ground_plane::link::collision"
    body_2_id: 5
    body_1_wrench {
      force {
        x: 47.120101049844259
        y: -2.0194766004274229
        z: 0.99457456142650136
      }
      torque {
        x: -0.085484123893888611
        y: -1.3276078626868877
        z: 1.3542951881515077
      }
    }
    body_2_wrench {
      force {
        x: 0
        y: 0
        z: 0
      }
      torque {
        x: 0
        y: 0
        z: 0
      }
    }
  }
  wrench {
    body_1_name: "iCub::r_foot::r_foot_collision"
    body_1_id: 75
    body_2_name: "ground_plane::link::collision"
    body_2_id: 5
    body_1_wrench {
      force {
        x: 0
        y: 0
        z: 0
      }
      torque {
        x: 0
        y: 0
        z: 0
      }
    }
    body_2_wrench {
      force {
        x: 0
        y: 0
        z: 0
      }
      torque {
        x: 0
        y: 0
        z: 0
      }
    }
  }
  wrench {
    body_1_name: "iCub::r_foot::r_foot_collision"
    body_1_id: 75
    body_2_name: "ground_plane::link::collision"
    body_2_id: 5
    body_1_wrench {
      force {
        x: 0.898251354561342
        y: -0.0065730503386497753
        z: -0.084423402906633069
      }
      torque {
        x: -0.00025353371021119011
        y: -0.042738935678584548
        z: 0.0006300169761021073
      }
    }
    body_2_wrench {
      force {
        x: 0
        y: 0
        z: 0
      }
      torque {
        x: 0
        y: 0
        z: 0
      }
    }
  }
  time {
    sec: 1245
    nsec: 550000000
  }
  world: "default"
}
contact {
  collision1: "iCub::l_foot::l_foot_collision"
  collision2: "ground_plane::link::collision"
  position {
    x: 1.0377787690688214
    y: -1.0664612437939363
    z: -4.4628885060710166e-06
  }
  normal {
    x: 0
    y: 0
    z: 1
  }
  depth: 4.4628885060710166e-06
  wrench {
    body_1_name: "iCub::l_foot::l_foot_collision"
    body_1_id: 43
    body_2_name: "ground_plane::link::collision"
    body_2_id: 5
    body_1_wrench {
      force {
        x: 89.5486887170896
        y: -1.8814214232228101
        z: 4.6542377318420636
      }
      torque {
        x: -0.0862604379596399
        y: -4.2608505103002541
        z: -0.062726990025405438
      }
    }
    body_2_wrench {
      force {
        x: 0
        y: 0
        z: 0
      }
      torque {
        x: 0
        y: 0
        z: 0
      }
    }
  }
  time {
    sec: 1245
    nsec: 550000000
  }
  world: "default"
}
time {
  sec: 1245
  nsec: 550000000
}

which shows that at that specific time there are 5 contact points for the right foot and one point for the left foot. The problem is that for the left foot the force in the z direction is positive whereas for the right foot it is negative for one of the points. Also the results are different from what I get by gz topic -e /gazebo/default/iCub/l_foot_ft_sensor/left_foot_ft/wrench or gz topic -e /gazebo/default/iCub/r_foot_ft_sensor/right_foot_ft/wrench

[traversaro]

Sorry for the late reply @azadm . For the fact that the contact points are not diffent between the right foot and the left foot @jeljaik and @DanielePucci can explain their tricks in having stable contacts. On the topic of the direction of the wrench, I guess that the model present in icub-gazebo lacked behind the one properly generated using our two tools for generating icub models (see https://github.com/robotology/codyco-superbuild/issues/55). On newly generated models, the l_foot and r_foot frame are coincident with the ankle ft sensor frame, and so the measurement should be consistent. As a quick test you can try the properly generated model for iCubGenova01 by following the " Use of generated models in Gazebo" instruction in https://github.com/robotology-playground/icub-model-generator .

[DanielePucci]

Any update on this? Can we close this issue?

[DanielePucci]

Closing this issue. Ready to re-open it in case it is needed.

[jeljaik]

:+1: