From 6a0650fd6e3ecf6ad77178a5a0481a76df01aea4 Mon Sep 17 00:00:00 2001
From: Tao Chen <taochen904@gmail.com>
Date: Wed, 13 Apr 2022 10:42:15 -0400
Subject: [PATCH] try fixing issue in hard reset

---
 src/airobot/ee_tool/robotiq2f140_pybullet.py  | 36 +++++++++++++------
 .../ee_tool/simple_gripper_pybullet.py        |  2 +-
 2 files changed, 26 insertions(+), 12 deletions(-)

diff --git a/src/airobot/ee_tool/robotiq2f140_pybullet.py b/src/airobot/ee_tool/robotiq2f140_pybullet.py
index d7886db..40180a9 100644
--- a/src/airobot/ee_tool/robotiq2f140_pybullet.py
+++ b/src/airobot/ee_tool/robotiq2f140_pybullet.py
@@ -26,16 +26,8 @@ class Robotiq2F140Pybullet(SimpleGripperPybullet):
     def __init__(self, cfgs, pb_client):
         super(Robotiq2F140Pybullet, self).__init__(cfgs=cfgs,
                                                    pb_client=pb_client)
-
-    def _setup_gripper(self):
-        """
-        Setup the gripper, pass the robot info from the arm to the gripper.
-
-        Args:
-            robot_id (int): robot id in Pybullet.
-            jnt_to_id (dict): mapping from the joint name to joint id.
-
-        """
+    
+    def _create_constraint(self):
         c_mimic = self._pb.createConstraint(
             self.robot_id,
             self.jnt_to_id['finger_joint'],
@@ -71,6 +63,16 @@ def _setup_gripper(self):
             childFramePosition=child_pos)
         self._pb.changeConstraint(c2, erp=0.8, maxForce=9999)
 
+    def _setup_gripper(self):
+        """
+        Setup the gripper, pass the robot info from the arm to the gripper.
+
+        Args:
+            robot_id (int): robot id in Pybullet.
+            jnt_to_id (dict): mapping from the joint name to joint id.
+
+        """
+        self._create_constraint()
         passive_force = 0
         for name in self.jnt_names:
             if name == 'finger_joint':
@@ -153,7 +155,9 @@ def set_jpos(self, pos, wait=True, ignore_physics=False):
 
         if ignore_physics:
             self._zero_vel_mode()
-            self._hard_reset(mic_pos)
+            tgt_pos_mimic = self._mimic_gripper(tgt_pos)
+            self._hard_reset(tgt_pos_mimic)
+            self._create_constraint()
             success = True
         else:
             self._pb.setJointMotorControl2(self.robot_id,
@@ -220,4 +224,14 @@ def get_jvel(self):
         jnt_id = self.jnt_to_id[self.jnt_names[0]]
         vel = self._pb.getJointState(self.robot_id, jnt_id)[1]
         return vel
+    
+    def _mimic_gripper(self, joint_val):
+        """
+        Given the value for the first joint,
+        mimic the joint values for the rest joints.
+        """
+        jnt_vals = [joint_val]
+        for i in range(1, len(self.jnt_names)):
+            jnt_vals.append(joint_val * self.cfgs.EETOOL.MIMIC_COEFF[i])
+        return jnt_vals
 
diff --git a/src/airobot/ee_tool/simple_gripper_pybullet.py b/src/airobot/ee_tool/simple_gripper_pybullet.py
index 7162cc2..e265144 100644
--- a/src/airobot/ee_tool/simple_gripper_pybullet.py
+++ b/src/airobot/ee_tool/simple_gripper_pybullet.py
@@ -179,7 +179,7 @@ def _zero_vel_mode(self):
                                            forces=[10] * len(self.gripper_jnt_ids))
 
     def _hard_reset(self, pos):
-        for i in range(len(self.gripper_jnt_ids)):
+        for i in range(min(len(self.gripper_jnt_ids), len(pos))):
             self._pb.resetJointState(self.robot_id,
                                      self.gripper_jnt_ids[i],
                                      targetValue=pos[i],