New 2F140 gripper implementation + franka2f140 robot + some changes on creating pybullet clients

setup.py

@@ -42,7 +42,7 @@ def pkg_files(directory):
     package_data={
         'airobot': extra_pkg_files,
     },
-    python_requires='>=2.7.*, <3.10',
+    python_requires='>=2.7, <3.10',
     classifiers=[
         "Programming Language :: Python :: 2.7",
         "Programming Language :: Python :: 3.7",

src/airobot/__init__.py

@@ -27,6 +27,8 @@ class Robot:
             if the robot has an base class
         use_cam (bool): whether to create the robot camera instance
             if the robot has an camera class
+        pybullet_urdf (bool): whether to use a robot URDF from
+            the pybullet data folder
         pb_cfg (dict): arguments to pass int when creating
             the pybullet client
         arm_cfg (dict): arguments to pass in the constructor
@@ -52,6 +54,7 @@ def __init__(self,
                  use_eetool=True,
                  use_base=True,
                  use_cam=True,
+                 pybullet_urdf=False,
                  pb_cfg=None,
                  arm_cfg=None,
                  base_cfg=None,
@@ -91,7 +94,7 @@ def __init__(self,
 
         self.pb_client = None
         if pb:
-            if not 'franka' in robot_name:
+            if not pybullet_urdf:
                 urdfs_root_path = os.path.join(root_path, 'urdfs')
                 urdf = os.path.join(urdfs_root_path,
                                     cfgs.PYBULLET_URDF)

src/airobot/cfgs/assets/franka_arm.py

@@ -10,7 +10,7 @@
 _C.CLASS = 'Franka'
 
 # https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/max-joint-torques-17260/
-_C.MAX_TORQUES = [87, 87, 87, 87, 12, 12, 12]
+_C.MAX_TORQUES = [200, 200, 200, 200, 150, 150, 150]
 _C.JOINT_NAMES = [
     'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4',
     'panda_joint5', 'panda_joint6', 'panda_joint7'
@@ -25,7 +25,6 @@
 _C.IK_POSITION_TOLERANCE = 0.01
 # inverse kinematics orientation tolerance (rad)
 _C.IK_ORIENTATION_TOLERANCE = 0.05
-# _C.HOME_POSITION = [0.98, 0.458, 0.31, -2.24, -0.30, 2.66, 2.32]
 _C.HOME_POSITION = [-0.19, 0.08, 0.23, -2.43, 0.03, 2.52, 0.86]
 _C.MAX_JOINT_ERROR = 0.01
 _C.MAX_JOINT_VEL_ERROR = 0.05

src/airobot/cfgs/franka_2f140_cfg.py

@@ -0,0 +1,27 @@
+from airobot.cfgs.assets.default_configs import get_cfg_defaults
+from airobot.cfgs.assets.franka_arm import get_franka_arm_cfg
+from airobot.cfgs.assets.robotiq2f140 import get_robotiq2f140_cfg
+from airobot.cfgs.assets.pybullet_camera import get_sim_cam_cfg
+
+_C = get_cfg_defaults()
+# whether the robot has an arm or not
+_C.HAS_ARM = True
+# whether the robot has a camera or not
+_C.HAS_CAMERA = True
+# whether the robot has a end effector tool or not
+_C.HAS_EETOOL = True
+
+_C.ROBOT_DESCRIPTION = '/robot_description'
+_C.PYBULLET_URDF = 'panda_2f140.urdf'
+
+_C.ARM = get_franka_arm_cfg()
+
+_C.CAM.SIM = get_sim_cam_cfg()
+_C.CAM.CLASS = 'RGBDCamera'
+
+_C.EETOOL = get_robotiq2f140_cfg()
+_C.EETOOL.CLASS = 'Robotiq2F140'
+
+
+def get_cfg():
+    return _C.clone()

src/airobot/cfgs/franka_cfg.py

@@ -12,7 +12,7 @@
 _C.HAS_EETOOL = True
 
 _C.ROBOT_DESCRIPTION = '/robot_description'
-_C.PYBULLET_URDF = 'franka_panda/panda.urdf'
+_C.PYBULLET_URDF = 'panda.urdf'
 
 _C.ARM = get_franka_arm_cfg()
 

src/airobot/ee_tool/robotiq2f140_pybullet.py

@@ -1,10 +1,11 @@
-from airobot.ee_tool.simple_gripper_mimic_pybullet import SimpleGripperMimicPybullet
+import airobot.utils.common as arutil
+from airobot.ee_tool.simple_gripper_pybullet import SimpleGripperPybullet
+from airobot.utils.arm_util import wait_to_reach_jnt_goal
 
 
-class Robotiq2F140Pybullet(SimpleGripperMimicPybullet):
+class Robotiq2F140Pybullet(SimpleGripperPybullet):
     """
-    Class for interfacing with a Robotiq 2F140 gripper when
-    it is attached to UR5e arm in pybullet.
+    Class for interfacing with a Robotiq 2F140 gripper in pybullet.
 
     Args:
         cfgs (YACS CfgNode): configurations for the gripper.
@@ -25,6 +26,42 @@ class Robotiq2F140Pybullet(SimpleGripperMimicPybullet):
     def __init__(self, cfgs, pb_client):
         super(Robotiq2F140Pybullet, self).__init__(cfgs=cfgs,
                                                    pb_client=pb_client)
+
+    def _create_constraint(self):
+        c_mimic = self._pb.createConstraint(
+            self.robot_id,
+            self.jnt_to_id['finger_joint'],
+            self.robot_id,
+            self.jnt_to_id['right_outer_knuckle_joint'],
+            jointType=self._pb.JOINT_GEAR,
+            jointAxis=[1, 0, 0],
+            parentFramePosition=[0, 0, 0],
+            childFramePosition=[0, 0, 0])
+        self._pb.changeConstraint(c_mimic, gearRatio=1.0, erp=0.2, maxForce=self._max_torque)
+
+        parent_pos = [0.0, -0.01, -0.004]
+        child_pos = [0, 0.049, 0.0]
+        c1 = self._pb.createConstraint(
+            self.robot_id,
+            self.jnt_to_id['right_inner_finger_joint'],
+            self.robot_id,
+            self.jnt_to_id['right_inner_knuckle_joint'],
+            jointType=self._pb.JOINT_POINT2POINT,
+            jointAxis=[1, 0, 0], 
+            parentFramePosition=parent_pos,
+            childFramePosition=child_pos)
+        self._pb.changeConstraint(c1, erp=0.8, maxForce=9999)
+
+        c2 = self._pb.createConstraint(
+            self.robot_id,
+            self.jnt_to_id['left_inner_finger_joint'],
+            self.robot_id,
+            self.jnt_to_id['left_inner_knuckle_joint'],
+            jointType=self._pb.JOINT_POINT2POINT,
+            jointAxis=[1, 0, 0], 
+            parentFramePosition=parent_pos,
+            childFramePosition=child_pos)
+        self._pb.changeConstraint(c2, erp=0.8, maxForce=9999)
 
     def _setup_gripper(self):
         """
@@ -35,6 +72,18 @@ def _setup_gripper(self):
             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':
+                continue
+            # set all joints except driven one to have non-active motors
+            self._pb.setJointMotorControl2(
+                bodyIndex=self.robot_id, 
+                jointIndex=self.jnt_to_id[name], 
+                controlMode=self._pb.VELOCITY_CONTROL, 
+                force=passive_force)
+
         self._pb.changeDynamics(self.robot_id,
                                 self.jnt_to_id['left_inner_finger_pad_joint'],
                                 lateralFriction=2.0,
@@ -46,6 +95,26 @@ def _setup_gripper(self):
                                 spinningFriction=1.0,
                                 rollingFriction=1.0)
 
+        # set a pretty low default for the gripper velocity 
+        self.set_gripper_velocity(1.0)
+
+        # initialize driven joint positions to be open
+        self._pb.setJointMotorControl2(self.robot_id,
+            self.jnt_to_id['finger_joint'],
+            self._pb.POSITION_CONTROL,
+            targetPosition=0,
+            maxVelocity=self.max_velocity,
+            force=self._max_torque)
+
+    def set_gripper_velocity(self, gripper_vel):
+        """
+        Set the speed for gripper position control
+
+        Args: 
+            gripper_vel (float): gripper joint speed
+        """
+        self.max_velocity = gripper_vel
+
     def feed_robot_info(self, robot_id, jnt_to_id):
         """
         Setup the gripper, pass the robot info from the arm to the gripper.
@@ -55,5 +124,114 @@ def feed_robot_info(self, robot_id, jnt_to_id):
             jnt_to_id (dict): mapping from the joint name to joint id.
 
         """
-        super().feed_robot_info(robot_id, jnt_to_id)
+        self.robot_id = robot_id
+        self.jnt_to_id = jnt_to_id
+        self.gripper_jnt_ids = [
+            self.jnt_to_id[jnt] for jnt in self.jnt_names
+        ]
+
         self._setup_gripper()
+
+    def set_jpos(self, pos, wait=True, ignore_physics=False):
+        """
+        Set the gripper position.
+
+        Args:
+            pos (float): joint position.
+            wait (bool): wait until the joint position is set
+                to the target position.
+
+        Returns:
+            bool: A boolean variable representing if the action is
+            successful at the moment when the function exits.
+        """
+        joint_name = self.jnt_names[0]
+        tgt_pos = arutil.clamp(
+            pos,
+            min(self.gripper_open_angle, self.gripper_close_angle),
+            max(self.gripper_open_angle, self.gripper_close_angle))
+
+        jnt_id = self.jnt_to_id[joint_name]
+
+        if ignore_physics:
+            self._zero_vel_mode()
+            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,
+                jnt_id,
+                self._pb.POSITION_CONTROL,
+                targetPosition=tgt_pos,
+                maxVelocity=self.max_velocity,
+                force=self._max_torque)
+
+            # this applies a small spring force on the inner links
+            # unclear if keeping it in is beneficial
+            '''
+            self._pb.setJointMotorControl2(
+                targetPosition=-1.0*self.cfgs.EETOOL.CLOSE_ANGLE, 
+                bodyIndex=self.robot_id, 
+                jointIndex=self.jnt_to_id['right_inner_knuckle_joint'], 
+                controlMode=self._pb.POSITION_CONTROL, 
+                force=1)
+            self._pb.setJointMotorControl2(
+                targetPosition=1.0*self.cfgs.EETOOL.CLOSE_ANGLE, 
+                bodyIndex=self.robot_id, 
+                jointIndex=self.jnt_to_id['left_inner_knuckle_joint'], 
+                controlMode=self._pb.POSITION_CONTROL, 
+                force=1)
+            '''
+
+            success = False
+            if self._pb.in_realtime_mode() and wait:
+                success = wait_to_reach_jnt_goal(
+                    tgt_pos,
+                    get_func=self.get_jpos,
+                    joint_name=joint_name,
+                    get_func_derv=self.get_jvel,
+                    timeout=self.cfgs.ARM.TIMEOUT_LIMIT,
+                    max_error=self.cfgs.ARM.MAX_JOINT_ERROR
+                )
+            else:
+                success = True
+
+        return True
+
+    def get_jpos(self):
+        """
+        Return the joint position(s) of the gripper.
+
+        Returns:
+            float: joint position.
+        """
+        if not self._is_activated:
+            raise RuntimeError('Call activate function first!')
+        jnt_id = self.jnt_to_id[self.jnt_names[0]]
+        pos = self._pb.getJointState(self.robot_id, jnt_id)[0]
+        return pos
+
+    def get_jvel(self):
+        """
+        Return the joint velocity of the gripper.
+
+        Returns:
+            float: joint velocity.
+        """
+        if not self._is_activated:
+            raise RuntimeError('Call activate function first!')
+        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
+

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],