From 264f0ee8dcdaacdaae13d9fa17a7d06f1fd597a1 Mon Sep 17 00:00:00 2001
From: Ermano Arruda <ermano.arruda@gmail.com>
Date: Wed, 24 Apr 2024 14:42:17 +0100
Subject: [PATCH] updating package.yaml dependencies, minor tip_link bugfix

---
 grip/robot/bullet_robot.py              |  6 +++---
 grip/robot/bullet_robotiq_2f_gripper.py | 10 ++++++----
 grip/robot/robot_arm.py                 |  2 +-
 grip/ros/ros_robot_arm.py               |  4 +++-
 package.xml                             |  7 +++++++
 requirements.txt                        |  3 +--
 requirements_dev.txt                    |  1 -
 7 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/grip/robot/bullet_robot.py b/grip/robot/bullet_robot.py
index bd573a9..94e40f3 100644
--- a/grip/robot/bullet_robot.py
+++ b/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(
diff --git a/grip/robot/bullet_robotiq_2f_gripper.py b/grip/robot/bullet_robotiq_2f_gripper.py
index ad01d95..d586015 100644
--- a/grip/robot/bullet_robotiq_2f_gripper.py
+++ b/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
diff --git a/grip/robot/robot_arm.py b/grip/robot/robot_arm.py
index dbbc04e..3e0c0be 100644
--- a/grip/robot/robot_arm.py
+++ b/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,
             )
diff --git a/grip/ros/ros_robot_arm.py b/grip/ros/ros_robot_arm.py
index c49e3dc..fe0e532 100644
--- a/grip/ros/ros_robot_arm.py
+++ b/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)
 
diff --git a/package.xml b/package.xml
index a412c7c..ab0abde 100644
--- a/package.xml
+++ b/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>
diff --git a/requirements.txt b/requirements.txt
index 9570b5d..0dadd00 100644
--- a/requirements.txt
+++ b/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
\ No newline at end of file
+transforms3d==0.4.1
\ No newline at end of file
diff --git a/requirements_dev.txt b/requirements_dev.txt
index 18b1bcc..4dd1c20 100644
--- a/requirements_dev.txt
+++ b/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