updating package.yaml dependencies, minor tip_link bugfix

grip/robot/bullet_robot.py

@@ -1151,13 +1151,13 @@ def random_texture(self) -> None:
         if self.texture_randomiser is not None:
             self.texture_randomiser.randomise(self)
 
-    def enable_torque_sensor(self, enable: bool) -> None:
+    def enable_torque_sensor(self, enable: bool, joint_id: int = None) -> None:
         """
         Enable or disable torque_sensor.
         """
-
+        joint = self.actuated_joint_ids[-1] if joint_id is None else joint_id
         p.enableJointForceTorqueSensor(
-            self.id, self.ee_index, enable, physicsClientId=self.phys_id
+            self.id, joint, enable, physicsClientId=self.phys_id
         )
 
     def get_contact_force(

grip/robot/bullet_robotiq_2f_gripper.py

@@ -28,7 +28,9 @@ class BulletRobotiq2FGripper(BulletRobot, EndEffectorType):
     def __init__(self, **kwargs):
         kwargs["urdf_file"] = kwargs.get("urdf_file", None)
         kwargs["ee_index"] = kwargs.get("ee_index", -1)
-        kwargs["tip_link"] = kwargs.get("tip_link", "robotiq_arg2f_base_link")
+        kwargs["tip_link"] = kwargs.get("tip_link", "tool0")
+
+        print("Tip link: ", kwargs["tip_link"])
 
         kwargs["joint_names"] = kwargs.get(
             "joint_names",
@@ -65,9 +67,9 @@ def true_ee_pose(self):
         lft_pos, _ = self.get_link_pose(self.link_dict["left_inner_finger_tip"])
         rft_pos, _ = self.get_link_pose(self.link_dict["right_inner_finger_tip"])
         ee_payload_pos = (lft_pos + rft_pos) / 2
-        _, ee_payload_quat = self.get_link_pose(
-            self.link_dict["robotiq_arg2f_base_link"]
-        )
+
+        self.get_link_id()
+        _, ee_payload_quat = self.get_link_pose(self.tip_link)
         return ee_payload_pos, ee_payload_quat
 
     @property

grip/robot/robot_arm.py

@@ -58,7 +58,7 @@ def __init__(
                 self.gripper_type,
                 parent=self,
                 max_force=gripper_max_force,
-                ee_index=self.ee_index,
+                tip_link=self.tip_link,
                 urdf_file=None,
                 sliders=self._sliders,
             )

grip/ros/ros_robot_arm.py

@@ -83,7 +83,9 @@ def __init__(
 
         else:
             self.joint_state_sub = JointStateSub(
-                self.node, joint_update_callback=self.set_angles
+                self.node,
+                topic_name=self.state_topic,
+                joint_update_callback=self.set_angles,
             )
             self.trajectory_client = TrajectoryActionClient(self.node)
 

package.xml

@@ -29,6 +29,13 @@
   <exec_depend>ros2_controllers</exec_depend>
   <exec_depend>gripper_controllers</exec_depend>
   <exec_depend>moveit_ros_perception</exec_depend>
+  <exec_depend>python3-pybullet-pip</exec_depend>
+  <exec_depend>python3-open3d-pip</exec_depend>
+  <exec_depend>python-transforms3d-pip</exec_depend>
+  <exec_depend>python3-scipy</exec_depend>
+  <exec_depend>python3-matplotlib</exec_depend>
+  <exec_depend>python3-trimesh-pip</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
 
   <test_depend>python3-pytest</test_depend>
   <export>

requirements.txt

@@ -7,5 +7,4 @@ matplotlib>=3.3.4 # plotting tools
 pybullet-planning-eaa # pure python motion planning
 trimesh==3.9.20 # mesh processing tools
 xatlas==0.0.7 # mesh parametrisation
-transforms3d==0.4.1
-strenum
+transforms3d==0.4.1

requirements_dev.txt

@@ -8,7 +8,6 @@ pybullet-planning-eaa # pure python motion planning
 trimesh==3.9.20 # mesh processing tools
 xatlas==0.0.7 # mesh parametrisation
 transforms3d==0.4.1
-strenum
 wheel
 pytest
 ipdb