Contact between robot's surfaces and ground is unstable

[aamorel]

I have a robot composed of 4 bodies linked by 3 continuous actuated joints.

I am attaching to each of the bodies some surfaces (using .rsdf files). I would like to programatically add or remove contact (fixed no DOF) between these surfaces and other surfaces (ground for example).

I am testing the behavior by creating a contact between one surface and the ground, and then move the joints to verify that the contact is well set (that the surface in contact does not move). In this setting, I only have kinematic constraints.

Contacts between surfaces attached to the first body and the ground are normal. However, for the other surfaces, the contact is not stable. By this, I mean that when moving the actuated joints, the contacted surface is slightly moving. This perturbation increases when I move the joints faster. But the contact is partially working because the surface does not move freely: it is still constrained, but only loosely.

Do you have any idea where this behavior could come from? I checked the definition of the frames to which the surfaces are attached and everything seems correct.

Here are the surfaces visualized:

Here is the .urdf and .rsdf:

<?xml version="1.0" ?>
<robot name="Roombot" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <material name="White">
    <color rgba="0.9 0.9 0.9 1"/>
  </material>
  <material name="Black">
    <color rgba="0.1 0.1 0.1 1"/>
  </material>

  <material name="WhiteTransparent">
    <color rgba="0.9 0.9 0.9 0.3"/>
  </material>
  <material name="BlackTransparent">
    <color rgba="0.1 0.1 0.1 0.3"/>
  </material>

  <material name="Red">
    <color rgba="1.0 0.1 0.1 1"/>
  </material>

  <link name="base_link"/>

  <!-- Hemisphere links -->

  <link name="HS1">
    <inertial>
      <mass value="0.237" />
      <origin rpy="0 0 0" xyz="0.052 0 0"/>
      <inertia ixx="3.2265179395e-04" ixy="0.0" ixz="0.0" iyy="3.2265206901e-04" iyz="0.0" izz="6.3961588256e-04"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://roombot_description/meshes/HS1.stl"/>
      </geometry>
      <material name="White"/>
    </visual>
  </link>

  <joint name="base" type="fixed">
    <parent link="base_link"/>
    <child link="HS1"/>
    <origin rpy="0 0 -0.2617994" xyz="0 0 0"/>
  </joint>

  <link name="HS2">
    <inertial>
      <mass value="0.237" />
      <origin rpy="0 0 0" xyz="0.052 0 0"/>
      <inertia ixx="3.2265179395e-04" ixy="0.0" ixz="0.0" iyy="3.2265206901e-04" iyz="0.0" izz="6.3961588256e-04"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://roombot_description/meshes/HS2.stl"/>
      </geometry>
      <material name="Black"/>
    </visual>
  </link>

  <link name="HS3">
    <inertial>
      <mass value="0.237" />
      <origin rpy="0 0 0" xyz="0.052 0 0"/>
      <inertia ixx="3.2265179395e-04" ixy="0.0" ixz="0.0" iyy="3.2265206901e-04" iyz="0.0" izz="6.3961588256e-04"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://roombot_description/meshes/HS3.stl"/>
      </geometry>
      <material name="White"/>
    </visual>
  </link>

  <link name="HS4">
    <inertial>
      <mass value="0.237" />
      <origin rpy="0 0 0" xyz="0.052 0 0"/>
      <inertia ixx="3.2265179395e-04" ixy="0.0" ixz="0.0" iyy="3.2265206901e-04" iyz="0.0" izz="6.3961588256e-04"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://roombot_description/meshes/HS4.stl"/>
      </geometry>
      <material name="Black"/>
    </visual>
  </link>

  <!-- ACM latch links

  <link name="latch1ACM0">
    <visual>
      <geometry>
          <mesh filename="package://roombot_description/meshes/latch.stl"/>
      </geometry>
      <material name="Black"/>
    </visual>
  </link>

  <link name="latch2ACM0">
    <visual>
      <geometry>
          <mesh filename="package://roombot_description/meshes/latch.stl"/>
      </geometry>
      <material name="Black"/>
    </visual>
  </link>

  <link name="latch3ACM0">
    <visual>
      <geometry>
          <mesh filename="package://roombot_description/meshes/latch.stl"/>
      </geometry>
      <material name="Black"/>
    </visual>
  </link>

  <link name="latch4ACM0">
    <visual>
      <geometry>
          <mesh filename="package://roombot_description/meshes/latch.stl"/>
      </geometry>
      <material name="Black"/>
    </visual>
  </link>

  <link name="latch1ACM1">
    <visual>
      <geometry>
          <mesh filename="package://roombot_description/meshes/latch.stl"/>
      </geometry>
      <material name="Black"/>
    </visual>
  </link>

  <link name="latch2ACM1">
    <visual>
      <geometry>
          <mesh filename="package://roombot_description/meshes/latch.stl"/>
      </geometry>
      <material name="Black"/>
    </visual>
  </link>

  <link name="latch3ACM1">
    <visual>
      <geometry>
          <mesh filename="package://roombot_description/meshes/latch.stl"/>
      </geometry>
      <material name="Black"/>
    </visual>
  </link>

  <link name="latch4ACM1">
    <visual>
      <geometry>
          <mesh filename="package://roombot_description/meshes/latch.stl"/>
      </geometry>
      <material name="Black"/>
    </visual>
  </link> -->

  <!-- 3 main joints -->

  <link name="preM0" />
  <joint name="prem0" type="fixed">
    <parent link="HS1"/>
    <child link="preM0"/>
    <origin rpy="0 -0.959931 1.0472" xyz="0 0 0.055"/>
  </joint>

  <joint name="M0" type="continuous">
    <parent link="HS1"/>
    <child link="HS2"/>
    <origin rpy="0 -0.959931 1.0472" xyz="0 0 0.055"/>
    <axis xyz="0.0 0.0 -1.0"/>
    <limit effort="1" velocity="10"/>
  </joint>

  <link name="preM1" />
  <joint name="prem1" type="fixed">
    <parent link="HS2"/>
    <child link="preM1"/>
    <origin rpy="0 0.959931 0" xyz="0 0 0"/>
  </joint>

  <joint name="M1" type="continuous">
    <parent link="preM1"/>
    <child link="HS3"/>
    <origin rpy="0 0 0" xyz="0 0 0.055"/>
    <axis xyz="0.0 0.0 -1.0"/>
    <limit effort="1" velocity="10"/>
  </joint>

  <link name="preM2" />
  <joint name="prem2" type="fixed">
    <parent link="HS3"/>
    <child link="preM2"/>
    <origin rpy="0 -0.959931 0" xyz="0 0 0.055"/>
  </joint>

  <joint name="M2" type="continuous">
    <parent link="HS3"/>
    <child link="HS4"/>
    <origin rpy="0 -0.959931 0" xyz="0 0 0.055"/>
    <axis xyz="0.0 0.0 -1.0"/>
    <limit effort="1" velocity="10"/>
  </joint>

  <!--  2 ACM joints -->

  <link name="acm0" />
  <joint name="X0" type="revolute">
    <parent link="HS1"/>
    <child link="acm0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="1.0 0 0"/>
    <limit effort="1" lower="0" upper="1" velocity="0.3"/>
  </joint>

  <link name="acm0Frame" />
  <joint name="acm0frame" type="fixed">
    <parent link="HS1"/>
    <child link="acm0Frame"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>

  <link name="acm0turned" />
  <joint name="acm0Turn" type="fixed">
    <parent link="HS1"/>
    <child link="acm0turned"/>
    <origin rpy="0 0 0.2617994" xyz="0 0 0"/>
  </joint>

  <link name="cp0Frame" />
  <joint name="cp0Joint" type="fixed">
    <parent link="acm0turned"/>
    <child link="cp0Frame"/>
    <origin rpy="-1.570796 -1.8325957 0" xyz="0 0.055 0.055"/>
  </joint>

  <link name="cp1Frame" />
  <joint name="cp1Joint" type="fixed">
    <parent link="acm0turned"/>
    <child link="cp1Frame"/>
    <origin rpy="-1.570796 0 -1.570796" xyz="0.055 0 0.055"/>
  </joint>

  <link name="acm1" />
  <joint name="X1" type="revolute">
    <parent link="HS4"/>
    <child link="acm1"/>
    <origin rpy="0 0.959931 0" xyz="0 0 0"/>
    <axis xyz="1.0 0 0"/>
    <limit effort="1" lower="0" upper="1" velocity="0.3"/>
  </joint>

  <link name="acm1turned" />
  <joint name="acm1Turn" type="fixed">
    <parent link="acm1Fixed"/>
    <child link="acm1turned"/>
    <origin rpy="0 0 0.785398" xyz="0 0 0"/>
  </joint>

  <link name="cp2Frame" />
  <joint name="cp2Joint" type="fixed">
    <parent link="acm1turned"/>
    <child link="cp2Frame"/>
    <origin rpy="-1.570796 0 0" xyz="0 0.055 0"/>
  </joint>

  <link name="cp3Frame" />
  <joint name="cp3Joint" type="fixed">
    <parent link="acm1turned"/>
    <child link="cp3Frame"/>
    <origin rpy="3.14159 1.570796 0" xyz="-0.055 0 0"/>
  </joint>

  <!-- ACM0 mimic joints

  <joint name="latchJoint01" type="continuous">
    <parent link="HS1"/>
    <child link="latch1ACM0"/>
    <origin rpy="-0.1465655 3.14159 0" xyz="0 -0.024 0.0045"/>
    <axis xyz="-1.0 0 0"/>
    <mimic joint="X0" multiplier="2.13" offset="0"/>
  </joint>

  <joint name="latchJoint02" type="continuous">
    <parent link="HS1"/>
    <child link="latch2ACM0"/>
    <origin rpy="-0.1465655 3.14159 3.14159" xyz="0 0.024 0.0045"/>
    <axis xyz="-1.0 0 0"/>
    <mimic joint="X0" multiplier="2.13" offset="0"/>
  </joint>

  <joint name="latchJoint03" type="continuous">
    <parent link="HS1"/>
    <child link="latch3ACM0"/>
    <origin rpy="-0.1465655 3.14159 1.57" xyz="0.024 0 0.0045"/>
    <axis xyz="-1.0 0 0"/>
    <mimic joint="X0" multiplier="2.13" offset="0"/>
  </joint>

  <joint name="latchJoint04" type="continuous">
    <parent link="HS1"/>
    <child link="latch4ACM0"/>
    <origin rpy="-0.1465655 3.14159 -1.57" xyz="-0.024 0 0.0045"/>
    <axis xyz="-1.0 0 0"/>
    <mimic joint="X0" multiplier="2.13" offset="0"/>
  </joint> -->

  <!-- ACM1 mimic joints

  <joint name="latchJoint11" type="continuous">
    <parent link="acm1Frame"/>
    <child link="latch1ACM1"/>
    <origin rpy="-0.1465655 3.14159 0" xyz="0 -0.024 0.0045"/>
    <axis xyz="-1.0 0 0"/>
    <mimic joint="X1" multiplier="2.13" offset="0"/>
  </joint>

  <joint name="latchJoint12" type="continuous">
    <parent link="acm1Frame"/>
    <child link="latch2ACM1"/>
    <origin rpy="-0.1465655 3.14159 3.14159" xyz="0 0.024 0.0045"/>
    <axis xyz="-1.0 0 0"/>
    <mimic joint="X1" multiplier="2.13" offset="0"/>
  </joint>

  <joint name="latchJoint13" type="continuous">
    <parent link="acm1Frame"/>
    <child link="latch3ACM1"/>
    <origin rpy="-0.1465655 3.14159 1.57" xyz="0.024 0 0.0045"/>
    <axis xyz="-1.0 0 0"/>
    <mimic joint="X1" multiplier="2.13" offset="0"/>
  </joint>

  <joint name="latchJoint14" type="continuous">
    <parent link="acm1Frame"/>
    <child link="latch4ACM1"/>
    <origin rpy="-0.1465655 3.14159 -1.57" xyz="-0.024 0 0.0045"/>
    <axis xyz="-1.0 0 0"/>
    <mimic joint="X1" multiplier="2.13" offset="0"/>
  </joint> -->

  <link name="acm1Fixed" />
  <joint name="latchJoint1" type="fixed">
    <parent link="HS4"/>
    <child link="acm1Fixed"/>
    <origin rpy="0 0.959931 0" xyz="0 0 0"/>
  </joint>

  <link name="acm1Frame" />
  <joint name="latchJoint2" type="fixed">
    <parent link="acm1Fixed"/>
    <child link="acm1Frame"/>
    <origin rpy="3.14159 0 1.047198" xyz="0 0 0.055"/>
  </joint>

  <link name="HS2turned" />
  <joint name="HS2Turn" type="fixed">
    <parent link="HS2"/>
    <child link="HS2turned"/>
    <origin rpy="0 0.959931 0" xyz="0 0 0"/>
  </joint>
  <link name="HS2turnedagain" />
  <joint name="HS2Turnagain" type="fixed">
    <parent link="HS2turned"/>
    <child link="HS2turnedagain"/>
    <origin rpy="0 0 0.785398" xyz="0 0 0"/>
  </joint>
  <link name="cp4Frame" />
  <joint name="cp4Joint" type="fixed">
    <parent link="HS2turnedagain"/>
    <child link="cp4Frame"/>
    <origin rpy="-1.570796 0 0" xyz="0 0.055 0"/>
  </joint>

  <link name="cp5Frame" />
  <joint name="cp5Joint" type="fixed">
    <parent link="HS2turnedagain"/>
    <child link="cp5Frame"/>
    <origin rpy="0 -1.570796 0" xyz="-0.055 0 0"/>
  </joint>
  <link name="HS3turned" />
  <joint name="HS3Turn" type="fixed">
    <parent link="HS3"/>
    <child link="HS3turned"/>
    <origin rpy="0 0 0.785398" xyz="0 0 0.055"/>
  </joint>
    <link name="cp6Frame" />
  <joint name="cp6Joint" type="fixed">
    <parent link="HS3turned"/>
    <child link="cp6Frame"/>
    <origin rpy="1.570796 0 0" xyz="0 -0.055 0"/>
  </joint>

  <link name="cp7Frame" />
  <joint name="cp7Joint" type="fixed">
    <parent link="HS3turned"/>
    <child link="cp7Frame"/>
    <origin rpy="0 1.570796 0" xyz="0.055 0 0"/>
  </joint>

</robot>

<robot name="roombot">

  <planar_surface name="ACM0" link="acm0Frame">
    <origin xyz="0 0 0" rpy="0 0 0.7853982" />
    <points>
      <point xy="0 0" />
    </points>
    <material name="plastic" />
  </planar_surface>

  <planar_surface name="CP0" link="cp0Frame">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <points>
      <point xy="0 0" />
    </points>
    <material name="plastic" />
  </planar_surface>

  <planar_surface name="CP1" link="cp1Frame">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <points>
      <point xy="0 0" />
    </points>
    <material name="plastic" />
  </planar_surface>

  <planar_surface name="CP4" link="cp4Frame">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <points>
      <point xy="0 0" />
    </points>
    <material name="plastic" />
  </planar_surface>

  <planar_surface name="CP5" link="cp5Frame">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <points>
      <point xy="0 0" />
    </points>
    <material name="plastic" />
  </planar_surface>

  <planar_surface name="ACM1" link="acm1Frame">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <points>
      <point xy="0 0" />
    </points>
    <material name="plastic" />
  </planar_surface>
  <planar_surface name="CP2" link="cp2Frame">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <points>
      <point xy="0 0" />
    </points>
    <material name="plastic" />
  </planar_surface>

  <planar_surface name="CP3" link="cp3Frame">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <points>
      <point xy="0 0" />
    </points>
    <material name="plastic" />
  </planar_surface>
    <planar_surface name="CP6" link="cp6Frame">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <points>
      <point xy="0 0" />
    </points>
    <material name="plastic" />
  </planar_surface>

  <planar_surface name="CP7" link="cp7Frame">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <points>
      <point xy="0 0" />
    </points>
    <material name="plastic" />
  </planar_surface>

</robot>

Please tell me if you need further information.

[kheddar]

would it be possible to have a video highlighting the instabilties you are mentionning?

[aamorel]

Yes for sure !

Here, the contact is between surface from first body and ground: stable contact. https://user-images.githubusercontent.com/43774341/177783393-5a274935-f625-4ed1-af1d-f9a4abcb41b0.mp4

Here, the contact is between surface from last body (surface on top) and ground: unstable contact. https://user-images.githubusercontent.com/43774341/177783402-ccf71133-e3e7-43cc-ba1d-e86bf3f90e8d.mp4

[gergondet]

Hi @aamorel

Can you try to flip the normal on the top surface (so that it's towards the same direction as the bottom surface) and try again?

[aamorel]

Hi @gergondet, thank you for your time

Flipping the top surface's normal does not change the instability.

Actually, I don't think the bug is due to the direction of the normal, because when I set the contact between the two other surfaces from the first body (white one on the bottom) with the ground, the contact is stable (even if the surfaces are facing the sides).

[gergondet]

Hi @aamorel

I gave it a try locally and I'm able to reproduce the issue.

I will look more into it in the coming days but in the meantime, switching the contact constraint formulation to position instead of velocity improves the situation significantly.

If you are using a basic controller then you simply overwrite the contactConstraint object before adding it to the solver:

using ContactType = mc_solver::ContactConstraint::ContactType;
contactConstraint = mc_solver::ContactConstraint(solver().dt(), ContactType::Position);
solver().addConstraintSet(contactConstraint);

If you are using an FSM controller you simply specify the contact's constraint contact type:

constraints:
- type: contact
  contactType: position

[aamorel]

Thank you for your help.

Even with the contactType: position, the contact is still drifting for the surfaces not linked to the root body, as you can see below (for example at t=5s): https://user-images.githubusercontent.com/43774341/177964640-9065f1f8-7b70-4214-ade2-0731a757e1a5.mp4

We are also trying to dig into this on our side.

[kheddar]

Hi @aamorel Can you please redo the simulation with one module only. This problem is certainly reproducible with one module. In one of the roombot you will attach the base to the ground and normally the terminal point will rotate nicely keeping the constraint to the ground fixed, then you link the terminal point of that one roombot to the ground and let its base unconstrained. Normally at this stage we will see the terminal point constrained not really fullfilled. @gergondet just checked that the numerical constraint (i.e. as formulated in the QP) hold well in both cases! This might means that the solution found for the base motion when the terminal point is constrained is wrong and the rendering since it starts from the base will give the impression that the terminal point constraint doesn't hold. We believe it's a problem that we can identify the root from a very simple one dof case. If you can do this fast, it will help us.

[gergondet]

Hi @aamorel

This is an integration issue which makes the floating base result wrong and results in an apparent motion of the contact when in fact the constraint is properly enforced regardless of the surface you choose for the contact.

It doesn't appear when you choose ACM0 because in that case the contact constraint simply locks the floating base motion. When you choose ACM1 then the floating base motion has to compensate the joints' motion to fix your selected contact surface. Integration issues on the floating base then moves it to the wrong position and the contact surface appears to move.

It is fixed by https://github.com/jrl-umi3218/RBDyn/pull/66 which has unfortunately been stale for a while. When applied, the drift dissapears. I will merge this PR within the afternoon.

[anastasiabolotnikova]

Hi @gergondet

Thank you for investigating this issue. Indeed, the RBDyn library version with corrected integration results in no motion of the surfaces. I tested it on our sample controller with various surfaces, no apparent motion of constrained contact surface anymore.

Looking forward to the merge of this PR.

Thanks a lot for your prompt help resolving this issue.

[aamorel]

Hi @gergondet , @kheddar

Sorry I was away last week. Thank you for your help, it was super efficient ! I'm closing the issue.