gripper finger joint limit violated when in contact

[IanYangChina]

Hi, I have been working on a manipulation simulation using a Kuka robot and a simple parallel jaw gripper. There is a joint limit violation issue when the gripper fingers make contact with other objects. In the example figures, the gripper tries to open the door of a chest. Though it succeeded, you can see the fingers of the gripper are at the wrong place. Their joint limits are violated. Do you have any suggestions for this problem? Thanks a lot!

This is the rest state of the robot.

The following are violated states.

[erwincoumans]

It depends on the details of the setup. Turning this into a discussion.

[IanYangChina]

Thanks for your reply. What detailed information do you need?

Below is the urdf code for the two fingers.

  <joint name="iiwa_gripper_finger1_joint" type="prismatic">
    <parent link="iiwa_gripper_base_link"/>
    <child link="iiwa_gripper_finger1"/>
    <origin rpy="0 0 0" xyz="0 0.04 0.04"/>
    <axis xyz="0 -1 0"/>
    <limit effort="20" lower="0.0" upper="0.035" velocity="0.2"/>
  </joint>
  <link name="iiwa_gripper_finger1">
    <contact>
      <lateral_friction value="1.0"/>
      <rolling_friction value="0.0"/>
      <inertia_scaling value="3.0"/>
      <contact_cfm value="0.0"/>
      <contact_erp value="1.0"/>
    </contact>
    <visual>
      <geometry>
        <box size="0.025, 0.01, 0.08"/>
      </geometry>
      <material name="Black"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.025, 0.01, 0.08"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.636951"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000380" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.001110" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.001171"/>
    </inertial>
  </link>

  <joint name="iiwa_gripper_finger2_joint" type="prismatic">
    <parent link="iiwa_gripper_base_link"/>
    <child link="iiwa_gripper_finger2"/>
    <origin rpy="0 0 0" xyz="0 -0.04 0.04"/>
    <axis xyz="0 1 0"/>
    <limit effort="20" lower="0.0" upper="0.035" velocity="0.2"/>
  </joint>
  <link name="iiwa_gripper_finger2">
    <contact>
      <lateral_friction value="1.0"/>
      <rolling_friction value="0.0"/>
      <inertia_scaling value="3.0"/>
      <contact_cfm value="0.0"/>
      <contact_erp value="1.0"/>
    </contact>
    <visual>
      <geometry>
        <box size="0.025, 0.01, 0.08"/>
      </geometry>
      <material name="Black"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.025, 0.01, 0.08"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.636951"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000380" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.001110" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.001171"/>
    </inertial>
  </link>

The gripper moves in a 'mocap' way. I'm running RL programs. The RL agent commands the gripper tip to move in the Cartesian space by setting the displacement of the 3 axes of the gripper tip frame. This action is turned into joint space displacement of the Kuka robot (IK), and the arm is moved using position control. Below is the code for computing IK and moving the Kuka arm.

    def compute_ik(self, bullet_client, target_ee_pos, target_ee_quat=None):
        assert target_ee_pos.shape == (3,)
        if target_ee_quat is None:
            target_ee_quat = self.end_effector_fixed_quaternion
        else:
            assert target_ee_quat.shape == (4,)
        # kuka-specific values for ik computation using null space dumping method,
        #   obtained from https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics.py
        joint_poses = bullet_client.calculateInverseKinematics(
            bodyUniqueId=self.kuka_body_index,
            endEffectorLinkIndex=self.end_effector_tip_joint_index,
            targetPosition=target_ee_pos,
            targetOrientation=target_ee_quat,
            # lower limits for null space
            lowerLimits=[-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05],
            # upper limits for null space
            upperLimits=[.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05],
            # joint ranges for null space
            jointRanges=[5.8, 4, 5.8, 4, 5.8, 4, 6],
            restPoses=self.kuka_rest_pose,
            maxNumIterations=40,
            residualThreshold=0.00001)
        return joint_poses[:7]

    def move_arm(self, bullet_client, joint_poses):
        bullet_client.setJointMotorControlArray(bodyUniqueId=self.kuka_body_index,
                                                jointIndices=self.kuka_joint_index,
                                                controlMode=bullet_client.POSITION_CONTROL,
                                                targetPositions=joint_poses,
                                                targetVelocities=np.zeros((7,)),
                                                forces=np.ones((7,)) * 100,
                                                positionGains=np.ones((7,)) * 0.03,
                                                velocityGains=np.ones((7,)))

[IanYangChina]

It seems like when the fingers collide with an unmovable object (the chest above) while the arm has not yet moved to the target position, the finger joint limits will be violated. The simulation does this to make sure the arm moves to the target configuration regardless of the fingers being misplaced