From 69aced15e7ebcc75d459a47c6b871ae0a0bdbca2 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Thu, 1 Sep 2022 08:43:45 +0000
Subject: [PATCH 01/35] setting joint_value_target as the current state for the
 joints that are not specified

---
 sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 124623211..f34f5c967 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -285,6 +285,7 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
             self._move_group_commander.set_start_state_to_current_state()
         else:
             self._move_group_commander.set_start_state(custom_start_state)
+        self._move_group_commander.set_joint_value_target(self._move_group_commander.get_current_joint_values())
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
         self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
         return self.__plan

From 5cc23428b7ad2b4fde8f05502d2192240138a451 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 5 Sep 2022 16:12:09 +0000
Subject: [PATCH 02/35] implemented remove_joints_from_plan

---
 .../sr_robot_commander/sr_robot_commander.py  | 36 ++++++++++++++++++-
 1 file changed, 35 insertions(+), 1 deletion(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index f34f5c967..3449ad840 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -285,9 +285,11 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
             self._move_group_commander.set_start_state_to_current_state()
         else:
             self._move_group_commander.set_start_state(custom_start_state)
-        self._move_group_commander.set_joint_value_target(self._move_group_commander.get_current_joint_values())
+
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
         self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
+
+        self.__plan = self.remove_joints_from_plan(self.__plan, list(joint_states.keys()))
         return self.__plan
 
     def check_plan_is_valid(self):
@@ -971,6 +973,38 @@ def change_teach_mode(mode, robot):
         except rospy.ServiceException:
             rospy.logerr("Failed to call service teach_mode")
 
+    @staticmethod
+    def remove_joints_from_plan(plan, joints):
+        """
+        Remove the specified joints from a given plan that contains a trajectory for these joints.
+        It returns a new plan without these joints.
+        @param plan - Plan from which the specified joints will be removed
+        @param joints - List with the name of the joints to be removed
+        """
+        new_plan = RobotTrajectory()
+
+        joint_idxs = []
+        for i in range(0, len(plan.joint_trajectory.joint_names)):
+            if not joints:
+                break
+            if plan.joint_trajectory.joint_names[i] in joints:
+                new_plan.joint_trajectory.joint_names.append(plan.joint_trajectory.joint_names[i])
+                joint_idxs.append(i)
+                joints.remove(plan.joint_trajectory.joint_names[i])
+        
+        # Iterate over points in the trajectory
+        for i in range(0, len(plan.joint_trajectory.points)):
+            new_point = JointTrajectoryPoint()
+            new_point.time_from_start = plan.joint_trajectory.points[i].time_from_start
+            for idx in joint_idxs:
+                new_point.positions.append(plan.joint_trajectory.points[i].positions[idx])
+                new_point.velocities.append(plan.joint_trajectory.points[i].velocities[idx])
+                new_point.accelerations.append(plan.joint_trajectory.points[i].accelerations[idx])
+
+            new_plan.joint_trajectory.points.append(new_point)
+
+        return new_plan
+
     def get_ik(self, target_pose, avoid_collisions=False, joint_states=None, ik_constraints=None):
         """
         Computes the inverse kinematics for a given pose. It returns a JointState.

From 80d1c0a218404f5b2b20f0ae799b0f1006b0eebb Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 5 Sep 2022 16:15:53 +0000
Subject: [PATCH 03/35] style

---
 sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 3449ad840..8f8d79c5f 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -991,7 +991,7 @@ def remove_joints_from_plan(plan, joints):
                 new_plan.joint_trajectory.joint_names.append(plan.joint_trajectory.joint_names[i])
                 joint_idxs.append(i)
                 joints.remove(plan.joint_trajectory.joint_names[i])
-        
+
         # Iterate over points in the trajectory
         for i in range(0, len(plan.joint_trajectory.points)):
             new_point = JointTrajectoryPoint()

From 8fd40b3461a7f39b2ce29a7b8af08db539dac640 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Tue, 6 Sep 2022 16:34:11 +0000
Subject: [PATCH 04/35] created test for plan_joint_value_target with partial
 target and move_to_joint_value_target

---
 .../sr_robot_commander/sr_robot_commander.py  |  6 ++--
 .../test/test_robot_commander.py              | 35 +++++++++++++++++++
 2 files changed, 39 insertions(+), 2 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 8f8d79c5f..df545e229 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -261,7 +261,10 @@ def move_to_joint_value_target(self, joint_states, wait=True,
                                     for joint, i in joint_states_cpy.items())
         self._move_group_commander.set_start_state_to_current_state()
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
-        self._move_group_commander.go(wait=wait)
+
+        self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
+        self.__plan = self.remove_joints_from_plan(self.__plan, list(joint_states.keys()))
+        self.execute_plan(self.__plan)
 
     def set_start_state_to_current_state(self):
         return self._move_group_commander.set_start_state_to_current_state()
@@ -992,7 +995,6 @@ def remove_joints_from_plan(plan, joints):
                 joint_idxs.append(i)
                 joints.remove(plan.joint_trajectory.joint_names[i])
 
-        # Iterate over points in the trajectory
         for i in range(0, len(plan.joint_trajectory.points)):
             new_point = JointTrajectoryPoint()
             new_point.time_from_start = plan.joint_trajectory.points[i].time_from_start
diff --git a/sr_robot_commander/test/test_robot_commander.py b/sr_robot_commander/test/test_robot_commander.py
index ac287c6a8..458c2dfe5 100755
--- a/sr_robot_commander/test/test_robot_commander.py
+++ b/sr_robot_commander/test/test_robot_commander.py
@@ -53,7 +53,14 @@
                         'ra_shoulder_lift_joint': -1.37, 'ra_wrist_1_joint': -0.52,
                         'ra_wrist_2_joint': 1.57, 'ra_wrist_3_joint': 0.00}
 
+CONST_PARTIAL_TARGET = {'ra_shoulder_pan_joint': 0.2, 'ra_wrist_1_joint': -0.52,
+                        'ra_wrist_2_joint': 1.57}
+CONST_FULL_TARGET = {'ra_shoulder_pan_joint': 0.2, 'ra_elbow_joint': 2.00,
+                     'ra_shoulder_lift_joint': -1.25, 'ra_wrist_1_joint': -0.52,
+                     'ra_wrist_2_joint': 1.57, 'ra_wrist_3_joint': 3.14}
+
 TOLERANCE_UNSAFE = 0.04
+TOLERANCE_SAFE = 0.02
 PLANNING_ATTEMPTS = 5
 
 
@@ -146,6 +153,13 @@ def compare_poses(self, pose1, pose2, position_threshold=0.02, orientation_thres
             return True
         return False
 
+    def compare_joint_positions(self, joint_configuration_1, joint_configuration_2, tolerance=0.02):
+        for key in joint_configuration_1.keys():
+            if joint_configuration_1[key] - joint_configuration_2[key] > tolerance or \
+               joint_configuration_1[key] - joint_configuration_2[key] < -tolerance:
+               return False
+        return True
+
     def test_get_and_set_planner_id(self):
         prev_planner_id = self.robot_commander._move_group_commander.get_planner_id()
         test_planner_id = "RRTstarkConfigDefault"
@@ -210,6 +224,27 @@ def test_plan_to_joint_value_target(self):
                                                                custom_start_state=None)
         self.assertIsInstance(plan, RobotTrajectory)
 
+    def test_plan_joint_value_target_and_execute(self):
+        self.reset_to_home()
+        self.robot_commander._move_group_commander.set_joint_value_target([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+        plan = self.robot_commander.plan_to_joint_value_target(CONST_PARTIAL_TARGET, angle_degrees=False,
+                                                               custom_start_state=None)
+        self.robot_commander.execute_plan(plan)
+        self.assertTrue(self.compare_joint_positions(self.robot_commander.get_current_state(),
+                                                     CONST_FULL_TARGET,
+                                                     tolerance=TOLERANCE_SAFE))
+
+    def test_move_to_joint_value_target_partial(self):
+        self.reset_to_home()
+        self.robot_commander._move_group_commander.set_joint_value_target([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+        self.robot_commander.move_to_joint_value_target(CONST_PARTIAL_TARGET, wait=True)
+        self.assertTrue(self.compare_joint_positions(self.robot_commander.get_current_state(),
+                                                     CONST_FULL_TARGET,
+                                                     tolerance=TOLERANCE_SAFE))
+
+    def test_remove_joints_from_plan(self):
+        raise NotImplementedError
+
     def test_execute(self):
         self.reset_to_home()
         self.robot_commander.plan_to_joint_value_target(CONST_EXAMPLE_TARGET, angle_degrees=False,

From dea4b5865bd8971843f71032ee1ee98312b58ce6 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Tue, 6 Sep 2022 16:39:41 +0000
Subject: [PATCH 05/35] style

---
 .../sr_robot_commander/sr_robot_commander.py  | 24 +++++++++----------
 .../test/test_robot_commander.py              |  5 ++--
 2 files changed, 14 insertions(+), 15 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index df545e229..4846ed153 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -986,22 +986,22 @@ def remove_joints_from_plan(plan, joints):
         """
         new_plan = RobotTrajectory()
 
-        joint_idxs = []
-        for i in range(0, len(plan.joint_trajectory.joint_names)):
+        joint_ids_to_keep = []
+        for joint_id in range(0, len(plan.joint_trajectory.joint_names)):
             if not joints:
                 break
-            if plan.joint_trajectory.joint_names[i] in joints:
-                new_plan.joint_trajectory.joint_names.append(plan.joint_trajectory.joint_names[i])
-                joint_idxs.append(i)
-                joints.remove(plan.joint_trajectory.joint_names[i])
+            if plan.joint_trajectory.joint_names[joint_id] in joints:
+                new_plan.joint_trajectory.joint_names.append(plan.joint_trajectory.joint_names[joint_id])
+                joint_ids_to_keep.append(joint_id)
+                joints.remove(plan.joint_trajectory.joint_names[joint_id])
 
-        for i in range(0, len(plan.joint_trajectory.points)):
+        for joint_id in range(0, len(plan.joint_trajectory.points)):
             new_point = JointTrajectoryPoint()
-            new_point.time_from_start = plan.joint_trajectory.points[i].time_from_start
-            for idx in joint_idxs:
-                new_point.positions.append(plan.joint_trajectory.points[i].positions[idx])
-                new_point.velocities.append(plan.joint_trajectory.points[i].velocities[idx])
-                new_point.accelerations.append(plan.joint_trajectory.points[i].accelerations[idx])
+            new_point.time_from_start = plan.joint_trajectory.points[joint_id].time_from_start
+            for joint_id_to_keep in joint_ids_to_keep:
+                new_point.positions.append(plan.joint_trajectory.points[joint_id].positions[joint_id_to_keep])
+                new_point.velocities.append(plan.joint_trajectory.points[joint_id].velocities[joint_id_to_keep])
+                new_point.accelerations.append(plan.joint_trajectory.points[joint_id].accelerations[joint_id_to_keep])
 
             new_plan.joint_trajectory.points.append(new_point)
 
diff --git a/sr_robot_commander/test/test_robot_commander.py b/sr_robot_commander/test/test_robot_commander.py
index 458c2dfe5..0177b7d7b 100755
--- a/sr_robot_commander/test/test_robot_commander.py
+++ b/sr_robot_commander/test/test_robot_commander.py
@@ -155,9 +155,8 @@ def compare_poses(self, pose1, pose2, position_threshold=0.02, orientation_thres
 
     def compare_joint_positions(self, joint_configuration_1, joint_configuration_2, tolerance=0.02):
         for key in joint_configuration_1.keys():
-            if joint_configuration_1[key] - joint_configuration_2[key] > tolerance or \
-               joint_configuration_1[key] - joint_configuration_2[key] < -tolerance:
-               return False
+            if abs(joint_configuration_1[key] - joint_configuration_2[key]) > tolerance:
+                return False
         return True
 
     def test_get_and_set_planner_id(self):

From 9675e55b514ebdff134aec2098bf8cf67f1b2a37 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 26 Sep 2022 06:25:45 +0000
Subject: [PATCH 06/35] undo cropping plan

---
 .../sr_robot_commander/sr_robot_commander.py  | 38 +------------------
 .../test/test_robot_commander.py              |  5 ++-
 2 files changed, 4 insertions(+), 39 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 4846ed153..2ebc3372c 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -261,10 +261,7 @@ def move_to_joint_value_target(self, joint_states, wait=True,
                                     for joint, i in joint_states_cpy.items())
         self._move_group_commander.set_start_state_to_current_state()
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
-
-        self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
-        self.__plan = self.remove_joints_from_plan(self.__plan, list(joint_states.keys()))
-        self.execute_plan(self.__plan)
+        self._move_group_commander.go(wait=wait)
 
     def set_start_state_to_current_state(self):
         return self._move_group_commander.set_start_state_to_current_state()
@@ -288,11 +285,9 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
             self._move_group_commander.set_start_state_to_current_state()
         else:
             self._move_group_commander.set_start_state(custom_start_state)
-
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
         self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
 
-        self.__plan = self.remove_joints_from_plan(self.__plan, list(joint_states.keys()))
         return self.__plan
 
     def check_plan_is_valid(self):
@@ -976,37 +971,6 @@ def change_teach_mode(mode, robot):
         except rospy.ServiceException:
             rospy.logerr("Failed to call service teach_mode")
 
-    @staticmethod
-    def remove_joints_from_plan(plan, joints):
-        """
-        Remove the specified joints from a given plan that contains a trajectory for these joints.
-        It returns a new plan without these joints.
-        @param plan - Plan from which the specified joints will be removed
-        @param joints - List with the name of the joints to be removed
-        """
-        new_plan = RobotTrajectory()
-
-        joint_ids_to_keep = []
-        for joint_id in range(0, len(plan.joint_trajectory.joint_names)):
-            if not joints:
-                break
-            if plan.joint_trajectory.joint_names[joint_id] in joints:
-                new_plan.joint_trajectory.joint_names.append(plan.joint_trajectory.joint_names[joint_id])
-                joint_ids_to_keep.append(joint_id)
-                joints.remove(plan.joint_trajectory.joint_names[joint_id])
-
-        for joint_id in range(0, len(plan.joint_trajectory.points)):
-            new_point = JointTrajectoryPoint()
-            new_point.time_from_start = plan.joint_trajectory.points[joint_id].time_from_start
-            for joint_id_to_keep in joint_ids_to_keep:
-                new_point.positions.append(plan.joint_trajectory.points[joint_id].positions[joint_id_to_keep])
-                new_point.velocities.append(plan.joint_trajectory.points[joint_id].velocities[joint_id_to_keep])
-                new_point.accelerations.append(plan.joint_trajectory.points[joint_id].accelerations[joint_id_to_keep])
-
-            new_plan.joint_trajectory.points.append(new_point)
-
-        return new_plan
-
     def get_ik(self, target_pose, avoid_collisions=False, joint_states=None, ik_constraints=None):
         """
         Computes the inverse kinematics for a given pose. It returns a JointState.
diff --git a/sr_robot_commander/test/test_robot_commander.py b/sr_robot_commander/test/test_robot_commander.py
index 0177b7d7b..8411bc229 100755
--- a/sr_robot_commander/test/test_robot_commander.py
+++ b/sr_robot_commander/test/test_robot_commander.py
@@ -55,12 +55,13 @@
 
 CONST_PARTIAL_TARGET = {'ra_shoulder_pan_joint': 0.2, 'ra_wrist_1_joint': -0.52,
                         'ra_wrist_2_joint': 1.57}
+
 CONST_FULL_TARGET = {'ra_shoulder_pan_joint': 0.2, 'ra_elbow_joint': 2.00,
                      'ra_shoulder_lift_joint': -1.25, 'ra_wrist_1_joint': -0.52,
                      'ra_wrist_2_joint': 1.57, 'ra_wrist_3_joint': 3.14}
 
 TOLERANCE_UNSAFE = 0.04
-TOLERANCE_SAFE = 0.02
+TOLERANCE_SAFE = 0.01
 PLANNING_ATTEMPTS = 5
 
 
@@ -153,7 +154,7 @@ def compare_poses(self, pose1, pose2, position_threshold=0.02, orientation_thres
             return True
         return False
 
-    def compare_joint_positions(self, joint_configuration_1, joint_configuration_2, tolerance=0.02):
+    def compare_joint_positions(self, joint_configuration_1, joint_configuration_2, tolerance=0.01):
         for key in joint_configuration_1.keys():
             if abs(joint_configuration_1[key] - joint_configuration_2[key]) > tolerance:
                 return False

From 4dc91219af808d84c6524acd4588c7f316e5e586 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 26 Sep 2022 06:26:43 +0000
Subject: [PATCH 07/35] style

---
 sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 2ebc3372c..124623211 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -287,7 +287,6 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
             self._move_group_commander.set_start_state(custom_start_state)
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
         self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
-
         return self.__plan
 
     def check_plan_is_valid(self):

From 1fcf0cea87deabf22a1d88faf936d9bd424e0f44 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 26 Sep 2022 08:29:15 +0000
Subject: [PATCH 08/35] implemented reading from set points

---
 .../sr_robot_commander/sr_robot_commander.py  | 62 +++++++++++++++++++
 1 file changed, 62 insertions(+)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 124623211..bc692675e 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -25,6 +25,7 @@
     PlanningSceneInterface
 from moveit_msgs.msg import RobotTrajectory, PositionIKRequest
 from sensor_msgs.msg import JointState
+from control_msgs.msg import JointTrajectoryControllerState
 import geometry_msgs.msg
 from sr_robot_msgs.srv import RobotTeachMode, RobotTeachModeRequest, \
     RobotTeachModeResponse
@@ -106,11 +107,20 @@ def __init__(self, name):
 
         self._set_up_action_client(self._controllers)
 
+        self._set_points_lock = threading.Lock()
+        self._are_set_points_ready = False
+        self._set_points_cv = threading.Condition()
+        self._set_points = {}
+
+        self._set_up_set_points_subscribers(self._controllers)
+
         self.tf_buffer = tf2_ros.Buffer()
         self.listener = tf2_ros.TransformListener(self.tf_buffer)
 
         threading.Thread(None, rospy.spin)
 
+        self._wait_for_set_points()
+
     def _is_trajectory_valid(self, trajectory, required_keys):
         if type(trajectory) != list:
             rospy.logerr("Trajectory is not a list of waypoints")
@@ -285,6 +295,9 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
             self._move_group_commander.set_start_state_to_current_state()
         else:
             self._move_group_commander.set_start_state(custom_start_state)
+
+        set_points = self.get_current_set_points()
+        self._move_group_commander.set_joint_value_target(set_points)
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
         self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
         return self.__plan
@@ -461,6 +474,20 @@ def get_current_state_bounded(self):
 
         return output
 
+    def get_current_set_points(self):
+        """
+        Reads from the set points
+        @return - Dictionary which contains the set points of the joints that belong to the move group
+        """
+        with self._set_points_lock:
+            set_points = self._set_points
+            move_group_set_points = {}
+            for joint_name in set_points:
+                if joint_name in self._move_group_commander._g.get_active_joints():
+                    move_group_set_points.update({joint_name: set_points[joint_name]})
+
+            return move_group_set_points
+
     def get_robot_state_bounded(self):
         return self._move_group_commander._g.get_current_state_bounded()
 
@@ -770,6 +797,14 @@ def _joint_states_callback(self, joint_state):
             self._joints_effort = {n: v for n, v in
                                    zip(joint_state.name, joint_state.effort)}
 
+    def _set_up_set_points_subscribers(self, controllers_list):
+        """
+        Sets up the required subscribers to read from the set points of each given controller
+        @param controller_list - Dictionary mapping a trajectory controller with the list of the joints it has
+        """
+        for controller_name in controllers_list.keys():
+            rospy.Subscriber(f"/{controller_name}/state", JointTrajectoryControllerState, self._set_point_cb, queue_size=1)
+
     def _set_up_action_client(self, controller_list):
         """
         Sets up an action client to communicate with the trajectory controller.
@@ -852,6 +887,33 @@ def _call_action(self, goals):
                 self._clients[client].send_goal(
                     goals[client], lambda terminal_state, result: self._action_done_cb(client, terminal_state, result))
 
+    def _set_point_cb(self, msg):
+        """
+        Updates the dictionary mapping joint names with their desired position in the trajectory controllers
+        @param msg - ROS message of type JointTrajectoryControllerState
+        """
+        with self._set_points_lock:
+            for i in range(0, len(msg.joint_names)):
+                self._set_points.update({msg.joint_names[i]: msg.desired.positions[i]})
+
+            if not self._are_set_points_ready:
+                with self._set_points_cv:
+                    joint_names_group = self._move_group_commander.get_active_joints()
+                    for joint_name in joint_names_group:
+                        if joint_name not in self._set_points.keys():
+                            break
+                    else:
+                        self._are_set_points_ready = True
+                        self._set_points_cv.notifyAll()
+
+    def _wait_for_set_points(self):
+        """
+        Waits until the set points variable has been updated with all joints
+        """
+        if not self._are_set_points_ready:
+            with self._set_points_cv:
+                self._set_points_cv.wait()
+
     def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
         """
         Moves robot through all joint states with specified timeouts.

From 2d6c9b26a08bee6073ecde80d9ac58cecfccf892 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 26 Sep 2022 15:13:05 +0000
Subject: [PATCH 09/35] checked underactuation ratio

---
 .../sr_robot_commander/sr_robot_commander.py  | 82 ++++++++++++++++---
 1 file changed, 69 insertions(+), 13 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index bc692675e..c1884879a 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -15,12 +15,13 @@
 # with this program. If not, see <http://www.gnu.org/licenses/>.
 
 from __future__ import absolute_import
+from ntpath import join
 import threading
 
 import rospy
 from actionlib import SimpleActionClient
 from control_msgs.msg import FollowJointTrajectoryAction, \
-    FollowJointTrajectoryGoal
+    FollowJointTrajectoryGoal, JointControllerState
 from moveit_commander import MoveGroupCommander, RobotCommander, \
     PlanningSceneInterface
 from moveit_msgs.msg import RobotTrajectory, PositionIKRequest
@@ -33,7 +34,7 @@
 from moveit_msgs.srv import GetPositionIK
 from moveit_msgs.srv import ListRobotStatesInWarehouse as ListStates
 from moveit_msgs.srv import GetRobotStateFromWarehouse as GetState
-from moveit_msgs.msg import OrientationConstraint, Constraints
+from moveit_msgs.msg import OrientationConstraint, Constraints, RobotState
 
 from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
 from math import radians
@@ -287,16 +288,17 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
         @return - motion plan (RobotTrajectory msg) that contains the trajectory to the set goal state.
         """
         joint_states_cpy = copy.deepcopy(joint_states)
+        set_points, move_group_robot_state = self.get_current_set_points()
 
+        #TODO: handle degrees argument
         if angle_degrees:
             joint_states_cpy.update((joint, radians(i))
                                     for joint, i in joint_states_cpy.items())
         if custom_start_state is None:
-            self._move_group_commander.set_start_state_to_current_state()
+            self._move_group_commander.set_start_state(move_group_robot_state)
         else:
             self._move_group_commander.set_start_state(custom_start_state)
 
-        set_points = self.get_current_set_points()
         self._move_group_commander.set_joint_value_target(set_points)
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
         self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
@@ -480,13 +482,34 @@ def get_current_set_points(self):
         @return - Dictionary which contains the set points of the joints that belong to the move group
         """
         with self._set_points_lock:
-            set_points = self._set_points
-            move_group_set_points = {}
-            for joint_name in set_points:
-                if joint_name in self._move_group_commander._g.get_active_joints():
-                    move_group_set_points.update({joint_name: set_points[joint_name]})
-
-            return move_group_set_points
+            raw_set_points = copy.deepcopy(self._set_points)
+
+        current_state = self.get_current_state()
+        set_points = {}
+
+        for joint in raw_set_points:
+            if "J0" in joint:
+                # Get j1 j2 ratio from current state
+                underactuation_ratio = current_state[f"{joint[:-2]}J1"]/current_state[f"{joint[:-2]}J2"]
+                ratio_part = raw_set_points[joint]/(underactuation_ratio+1)
+                set_point_j1 = underactuation_ratio*ratio_part
+                set_point_j2 = ratio_part
+                set_points.update({f"{joint[:-2]}J1": set_point_j1})
+                set_points.update({f"{joint[:-2]}J2": set_point_j2})
+            elif joint[5:] not in ["J1", "J2"] or joint[3:-2] in ["WR", "TH"]:
+                set_points.update({joint: raw_set_points[joint]})
+
+        joint_state = JointState()
+        joint_state.header = Header()
+        joint_state.header.stamp = rospy.Time.now()
+        for joint in set_points:
+            joint_state.name.append(joint)
+            joint_state.position.append(set_points[joint])
+
+        move_group_robot_state = RobotState()
+        move_group_robot_state.joint_state = joint_state
+
+        return set_points, move_group_robot_state
 
     def get_robot_state_bounded(self):
         return self._move_group_commander._g.get_current_state_bounded()
@@ -802,8 +825,27 @@ def _set_up_set_points_subscribers(self, controllers_list):
         Sets up the required subscribers to read from the set points of each given controller
         @param controller_list - Dictionary mapping a trajectory controller with the list of the joints it has
         """
+        # Get joint names of the group
+        joint_names_group = self._move_group_commander.get_active_joints()
+        topics_subscribed = []
+
         for controller_name in controllers_list.keys():
-            rospy.Subscriber(f"/{controller_name}/state", JointTrajectoryControllerState, self._set_point_cb, queue_size=1)
+            for joint_name in joint_names_group:
+                if joint_name in controllers_list[controller_name]:
+                    topic_name = f"/{controller_name}/state"
+                    if topic_name not in topics_subscribed:
+                        rospy.Subscriber(topic_name, JointTrajectoryControllerState, self._set_point_cb, queue_size=1)
+                        topics_subscribed.append(topic_name)
+                        continue
+
+        for joint_name in joint_names_group:
+            joint_member = joint_name[3:-2]
+            joint_number = joint_name[5:]
+            if joint_number in ["J1", "J2"] and joint_member not in ["WR", "TH"]:
+                topic_name = f"/sh_{joint_name.lower()[0:5]}j0_position_controller/state"
+                rospy.Subscriber(topic_name, JointControllerState, self._set_point_j0_cb, f"{joint_name[0:5]}J0", queue_size=1)
+                joint_names_group.remove(f"{joint_name[0:5]}J1")
+                joint_names_group.remove(f"{joint_name[0:5]}J2")
 
     def _set_up_action_client(self, controller_list):
         """
@@ -892,19 +934,33 @@ def _set_point_cb(self, msg):
         Updates the dictionary mapping joint names with their desired position in the trajectory controllers
         @param msg - ROS message of type JointTrajectoryControllerState
         """
+        joint_names_group = self._move_group_commander.get_active_joints()
         with self._set_points_lock:
             for i in range(0, len(msg.joint_names)):
+                if msg.joint_names[i] not in joint_names_group:
+                    continue
                 self._set_points.update({msg.joint_names[i]: msg.desired.positions[i]})
-
             if not self._are_set_points_ready:
                 with self._set_points_cv:
                     joint_names_group = self._move_group_commander.get_active_joints()
+                    for joint_name in joint_names_group:
+                        if joint_name[5:] in ["J1", "J2"] and joint_name[3:-2] not in ["WR", "TH"]:
+                            joint_names_group.append(f"{joint_name[0:5]}J0")
                     for joint_name in joint_names_group:
                         if joint_name not in self._set_points.keys():
                             break
                     else:
                         self._are_set_points_ready = True
                         self._set_points_cv.notifyAll()
+    
+    def _set_point_j0_cb(self, msg, joint_name):
+        """
+        Updates the dictionary mapping joint names with their desired position in the trajectory controllers
+        This callback is associated to the position controller of j0
+        @param msg - ROS message of type JointControllerState
+        """
+        with self._set_points_lock:
+            self._set_points.update({joint_name: msg.set_point})
 
     def _wait_for_set_points(self):
         """

From d2b9708064bd3014ca3af6081dfddd2e33b0785b Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 3 Oct 2022 09:38:50 +0000
Subject: [PATCH 10/35] bounded joint limits

---
 .../sr_robot_commander/sr_robot_commander.py  | 66 +++++++++++++++++--
 .../test/test_robot_commander.py              |  5 +-
 2 files changed, 60 insertions(+), 11 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index c1884879a..ce5d916b4 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -31,13 +31,15 @@
 from sr_robot_msgs.srv import RobotTeachMode, RobotTeachModeRequest, \
     RobotTeachModeResponse
 
+from xml.dom import minidom
+
 from moveit_msgs.srv import GetPositionIK
 from moveit_msgs.srv import ListRobotStatesInWarehouse as ListStates
 from moveit_msgs.srv import GetRobotStateFromWarehouse as GetState
 from moveit_msgs.msg import OrientationConstraint, Constraints, RobotState
 
 from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
-from math import radians
+from math import radians, pi
 
 from moveit_msgs.srv import GetPositionFK
 from std_msgs.msg import Header
@@ -103,6 +105,10 @@ def __init__(self, name):
 
         controller_list_param = rospy.get_param("/move_group/controller_list")
 
+        robot_description = rospy.get_param("/robot_description")
+        self._joint_limits = dict()
+        self._get_joint_limits(robot_description)
+
         # create dictionary with name of controllers and corresponding joints
         self._controllers = {item["name"]: item["joints"] for item in controller_list_param}
 
@@ -121,6 +127,32 @@ def __init__(self, name):
         threading.Thread(None, rospy.spin)
 
         self._wait_for_set_points()
+    
+    def _get_joint_limits(self, robot_description):
+        robot_dom = minidom.parseString(robot_description)
+        robot = robot_dom.getElementsByTagName('robot')[0]
+
+        for child in robot.childNodes:
+            if child.nodeType is child.TEXT_NODE:
+                continue
+            if child.localName == 'joint':
+                jtype = child.getAttribute('type')
+                if jtype in ['fixed', 'floating', 'planar']:
+                    continue
+                name = child.getAttribute('name')
+
+                if jtype == 'continuous':
+                    minval = -pi
+                    maxval = pi
+                else:
+                    try:
+                        limit = child.getElementsByTagName('limit')[0]
+                        minval = float(limit.getAttribute('lower'))
+                        maxval = float(limit.getAttribute('upper'))
+                    except:
+                        rospy.logwarn("%s is not fixed, nor continuous, but limits are not specified!" % name)
+                        continue
+                self._joint_limits.update({name: (minval, maxval)})
 
     def _is_trajectory_valid(self, trajectory, required_keys):
         if type(trajectory) != list:
@@ -270,7 +302,9 @@ def move_to_joint_value_target(self, joint_states, wait=True,
         if angle_degrees:
             joint_states_cpy.update((joint, radians(i))
                                     for joint, i in joint_states_cpy.items())
-        self._move_group_commander.set_start_state_to_current_state()
+        set_points, move_group_robot_state = self.get_current_set_points(bound=True)
+        self._move_group_commander.set_start_state(move_group_robot_state)
+        self._move_group_commander.set_joint_value_target(set_points)
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
         self._move_group_commander.go(wait=wait)
 
@@ -288,7 +322,7 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
         @return - motion plan (RobotTrajectory msg) that contains the trajectory to the set goal state.
         """
         joint_states_cpy = copy.deepcopy(joint_states)
-        set_points, move_group_robot_state = self.get_current_set_points()
+        set_points, move_group_robot_state = self.get_current_set_points(bound=True)
 
         #TODO: handle degrees argument
         if angle_degrees:
@@ -476,9 +510,24 @@ def get_current_state_bounded(self):
 
         return output
 
-    def get_current_set_points(self):
+    @staticmethod
+    def _bound_joint(joint_value, joint_limits):
+        """
+        Forces a joint value to be within the given joint limits
+        @joint_value - Value of the joint to be bounded
+        @joint_limits - Tuple with the lower limit and upper limit of the joint
+        @return - Joint value within the given joints
+        """
+        if joint_value < joint_limits[0]:
+            joint_value = joint_limits[0]
+        elif joint_value > joint_limits[1]:
+            joint_value = joint_limits[1]
+        return joint_value
+
+    def get_current_set_points(self, bound=False):
         """
         Reads from the set points
+        @bound - Boolean to choose if the set points should be bounded within the joint limits. Default False.
         @return - Dictionary which contains the set points of the joints that belong to the move group
         """
         with self._set_points_lock:
@@ -494,9 +543,15 @@ def get_current_set_points(self):
                 ratio_part = raw_set_points[joint]/(underactuation_ratio+1)
                 set_point_j1 = underactuation_ratio*ratio_part
                 set_point_j2 = ratio_part
+                if bound:
+                    set_point_j1 = self._bound_joint(set_point_j1, self._joint_limits[f"{joint[:-2]}J1"])
+                    set_point_j2 = self._bound_joint(set_point_j2, self._joint_limits[f"{joint[:-2]}J2"])
                 set_points.update({f"{joint[:-2]}J1": set_point_j1})
                 set_points.update({f"{joint[:-2]}J2": set_point_j2})
             elif joint[5:] not in ["J1", "J2"] or joint[3:-2] in ["WR", "TH"]:
+                set_point_value = raw_set_points[joint]
+                if bound:
+                    set_point_value = self._bound_joint(set_point_value, self._joint_limits[joint])
                 set_points.update({joint: raw_set_points[joint]})
 
         joint_state = JointState()
@@ -511,9 +566,6 @@ def get_current_set_points(self):
 
         return set_points, move_group_robot_state
 
-    def get_robot_state_bounded(self):
-        return self._move_group_commander._g.get_current_state_bounded()
-
     def move_to_named_target(self, name, wait=True):
         """
         Set target of the robot's links and moves to it
diff --git a/sr_robot_commander/test/test_robot_commander.py b/sr_robot_commander/test/test_robot_commander.py
index 8411bc229..566427559 100755
--- a/sr_robot_commander/test/test_robot_commander.py
+++ b/sr_robot_commander/test/test_robot_commander.py
@@ -224,7 +224,7 @@ def test_plan_to_joint_value_target(self):
                                                                custom_start_state=None)
         self.assertIsInstance(plan, RobotTrajectory)
 
-    def test_plan_joint_value_target_and_execute(self):
+    def test_plan_to_joint_value_target_and_execute_partial(self):
         self.reset_to_home()
         self.robot_commander._move_group_commander.set_joint_value_target([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
         plan = self.robot_commander.plan_to_joint_value_target(CONST_PARTIAL_TARGET, angle_degrees=False,
@@ -242,9 +242,6 @@ def test_move_to_joint_value_target_partial(self):
                                                      CONST_FULL_TARGET,
                                                      tolerance=TOLERANCE_SAFE))
 
-    def test_remove_joints_from_plan(self):
-        raise NotImplementedError
-
     def test_execute(self):
         self.reset_to_home()
         self.robot_commander.plan_to_joint_value_target(CONST_EXAMPLE_TARGET, angle_degrees=False,

From 0e50fa5b8cfa54fe0ce818003b7ab708c8d1e147 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 3 Oct 2022 09:47:33 +0000
Subject: [PATCH 11/35] removed todo

---
 sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index ce5d916b4..f02104303 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -324,7 +324,6 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
         joint_states_cpy = copy.deepcopy(joint_states)
         set_points, move_group_robot_state = self.get_current_set_points(bound=True)
 
-        #TODO: handle degrees argument
         if angle_degrees:
             joint_states_cpy.update((joint, radians(i))
                                     for joint, i in joint_states_cpy.items())

From a83b770b20386ec18534cd76b5be2dc8987fd37c Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Wed, 12 Oct 2022 09:53:39 +0000
Subject: [PATCH 12/35] created function to bound joint states

---
 .../sr_robot_commander/sr_robot_commander.py  | 41 +++++++++++++------
 1 file changed, 29 insertions(+), 12 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index f02104303..21a9ce12b 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -302,8 +302,9 @@ def move_to_joint_value_target(self, joint_states, wait=True,
         if angle_degrees:
             joint_states_cpy.update((joint, radians(i))
                                     for joint, i in joint_states_cpy.items())
-        set_points, move_group_robot_state = self.get_current_set_points(bound=True)
+        set_points, move_group_robot_state = self.get_current_set_points()
         self._move_group_commander.set_start_state(move_group_robot_state)
+        set_points = self._bound_state(set_points)
         self._move_group_commander.set_joint_value_target(set_points)
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
         self._move_group_commander.go(wait=wait)
@@ -322,17 +323,18 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
         @return - motion plan (RobotTrajectory msg) that contains the trajectory to the set goal state.
         """
         joint_states_cpy = copy.deepcopy(joint_states)
-        set_points, move_group_robot_state = self.get_current_set_points(bound=True)
+        set_points, robot_state = self.get_current_set_points()
 
         if angle_degrees:
             joint_states_cpy.update((joint, radians(i))
                                     for joint, i in joint_states_cpy.items())
         if custom_start_state is None:
-            self._move_group_commander.set_start_state(move_group_robot_state)
+            self._move_group_commander.set_start_state(robot_state)
         else:
             self._move_group_commander.set_start_state(custom_start_state)
 
-        self._move_group_commander.set_joint_value_target(set_points)
+        set_points_bounded = self._bound_state(set_points)
+        self._move_group_commander.set_joint_value_target(set_points_bounded)
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
         self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
         return self.__plan
@@ -509,6 +511,9 @@ def get_current_state_bounded(self):
 
         return output
 
+    def get_robot_state_bounded(self):
+        return self._move_group_commander._g.get_current_state_bounded()
+
     @staticmethod
     def _bound_joint(joint_value, joint_limits):
         """
@@ -523,10 +528,28 @@ def _bound_joint(joint_value, joint_limits):
             joint_value = joint_limits[1]
         return joint_value
 
-    def get_current_set_points(self, bound=False):
+    def _bound_state(self, joint_states):
+        """
+        Bounds the joint states within the limits of the joints
+        @param joint_states - It can be of type dict or RobotState. This param should contain the joints to be bounded
+        within their limits
+        @return - The joint states updated with the joint poisitions bounded
+        """
+        if type(joint_states) == dict:
+            for joint in joint_states:
+                joint_states[joint] = self._bound_joint(joint_states[joint],
+                                                        self._joint_limits[joint])
+
+        elif type(joint_states) == RobotState:
+            for i in range(0,len(joint_states.joint_state.name)):
+                joint_states.joint_state.position[i] = self._bound_joint(joint_states.joint_state.position[i],
+                                                                         self._joint_limits[joint_states.joint_state.name[i]])
+
+        return joint_states
+
+    def get_current_set_points(self):
         """
         Reads from the set points
-        @bound - Boolean to choose if the set points should be bounded within the joint limits. Default False.
         @return - Dictionary which contains the set points of the joints that belong to the move group
         """
         with self._set_points_lock:
@@ -542,15 +565,9 @@ def get_current_set_points(self, bound=False):
                 ratio_part = raw_set_points[joint]/(underactuation_ratio+1)
                 set_point_j1 = underactuation_ratio*ratio_part
                 set_point_j2 = ratio_part
-                if bound:
-                    set_point_j1 = self._bound_joint(set_point_j1, self._joint_limits[f"{joint[:-2]}J1"])
-                    set_point_j2 = self._bound_joint(set_point_j2, self._joint_limits[f"{joint[:-2]}J2"])
                 set_points.update({f"{joint[:-2]}J1": set_point_j1})
                 set_points.update({f"{joint[:-2]}J2": set_point_j2})
             elif joint[5:] not in ["J1", "J2"] or joint[3:-2] in ["WR", "TH"]:
-                set_point_value = raw_set_points[joint]
-                if bound:
-                    set_point_value = self._bound_joint(set_point_value, self._joint_limits[joint])
                 set_points.update({joint: raw_set_points[joint]})
 
         joint_state = JointState()

From 97ed92c297c03fa995996247ab29da6d44642821 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Wed, 12 Oct 2022 10:03:30 +0000
Subject: [PATCH 13/35] removed continue

---
 sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 21a9ce12b..73f11a275 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -15,7 +15,6 @@
 # with this program. If not, see <http://www.gnu.org/licenses/>.
 
 from __future__ import absolute_import
-from ntpath import join
 import threading
 
 import rospy
@@ -904,7 +903,6 @@ def _set_up_set_points_subscribers(self, controllers_list):
                     if topic_name not in topics_subscribed:
                         rospy.Subscriber(topic_name, JointTrajectoryControllerState, self._set_point_cb, queue_size=1)
                         topics_subscribed.append(topic_name)
-                        continue
 
         for joint_name in joint_names_group:
             joint_member = joint_name[3:-2]

From 0af6674ae263a74b1ab30f090adab69fcb516308 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Wed, 12 Oct 2022 10:52:19 +0000
Subject: [PATCH 14/35] linter errors

---
 .../sr_robot_commander/sr_robot_commander.py  | 22 ++++++++++++-------
 1 file changed, 14 insertions(+), 8 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 73f11a275..03b28d719 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -15,6 +15,7 @@
 # with this program. If not, see <http://www.gnu.org/licenses/>.
 
 from __future__ import absolute_import
+from __future__ import division
 import threading
 
 import rospy
@@ -126,7 +127,7 @@ def __init__(self, name):
         threading.Thread(None, rospy.spin)
 
         self._wait_for_set_points()
-    
+
     def _get_joint_limits(self, robot_description):
         robot_dom = minidom.parseString(robot_description)
         robot = robot_dom.getElementsByTagName('robot')[0]
@@ -148,8 +149,9 @@ def _get_joint_limits(self, robot_description):
                         limit = child.getElementsByTagName('limit')[0]
                         minval = float(limit.getAttribute('lower'))
                         maxval = float(limit.getAttribute('upper'))
-                    except:
-                        rospy.logwarn("%s is not fixed, nor continuous, but limits are not specified!" % name)
+                    except Exception as exception:
+                        rospy.logwarn(f"{exception}: {name} is not fixed, nor continuous, \
+                            but limits are not specified!")
                         continue
                 self._joint_limits.update({name: (minval, maxval)})
 
@@ -540,9 +542,10 @@ def _bound_state(self, joint_states):
                                                         self._joint_limits[joint])
 
         elif type(joint_states) == RobotState:
-            for i in range(0,len(joint_states.joint_state.name)):
-                joint_states.joint_state.position[i] = self._bound_joint(joint_states.joint_state.position[i],
-                                                                         self._joint_limits[joint_states.joint_state.name[i]])
+            for i in range(0, len(joint_states.joint_state.name)):
+                joint_states.joint_state.position[i] = \
+                    self._bound_joint(joint_states.joint_state.position[i],
+                                      self._joint_limits[joint_states.joint_state.name[i]])
 
         return joint_states
 
@@ -909,7 +912,10 @@ def _set_up_set_points_subscribers(self, controllers_list):
             joint_number = joint_name[5:]
             if joint_number in ["J1", "J2"] and joint_member not in ["WR", "TH"]:
                 topic_name = f"/sh_{joint_name.lower()[0:5]}j0_position_controller/state"
-                rospy.Subscriber(topic_name, JointControllerState, self._set_point_j0_cb, f"{joint_name[0:5]}J0", queue_size=1)
+                rospy.Subscriber(topic_name,
+                                 JointControllerState,
+                                 self._set_point_j0_cb, f"{joint_name[0:5]}J0",
+                                 queue_size=1)
                 joint_names_group.remove(f"{joint_name[0:5]}J1")
                 joint_names_group.remove(f"{joint_name[0:5]}J2")
 
@@ -1018,7 +1024,7 @@ def _set_point_cb(self, msg):
                     else:
                         self._are_set_points_ready = True
                         self._set_points_cv.notifyAll()
-    
+
     def _set_point_j0_cb(self, msg, joint_name):
         """
         Updates the dictionary mapping joint names with their desired position in the trajectory controllers

From 9a027002ce23ba3676837533c6d6169298e8f1c3 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Wed, 12 Oct 2022 13:16:04 +0000
Subject: [PATCH 15/35] added public getter to get joint limits

---
 .../src/sr_robot_commander/sr_robot_commander.py      | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 03b28d719..0898fda03 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -107,7 +107,7 @@ def __init__(self, name):
 
         robot_description = rospy.get_param("/robot_description")
         self._joint_limits = dict()
-        self._get_joint_limits(robot_description)
+        self._read_joint_limits(robot_description)
 
         # create dictionary with name of controllers and corresponding joints
         self._controllers = {item["name"]: item["joints"] for item in controller_list_param}
@@ -128,7 +128,7 @@ def __init__(self, name):
 
         self._wait_for_set_points()
 
-    def _get_joint_limits(self, robot_description):
+    def _read_joint_limits(self, robot_description):
         robot_dom = minidom.parseString(robot_description)
         robot = robot_dom.getElementsByTagName('robot')[0]
 
@@ -155,6 +155,13 @@ def _get_joint_limits(self, robot_description):
                         continue
                 self._joint_limits.update({name: (minval, maxval)})
 
+    def get_joint_limits(self):
+        """
+        @return - dictionary mapping joint names with a tuple containing the lower limit
+        and the upper limit of a joint
+        """
+        return self._joint_limits
+
     def _is_trajectory_valid(self, trajectory, required_keys):
         if type(trajectory) != list:
             rospy.logerr("Trajectory is not a list of waypoints")

From 480fc1eb0e7dac769c4ec1fd9fbdb9152b7b0963 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Tue, 18 Oct 2022 10:53:10 +0000
Subject: [PATCH 16/35] added years

---
 sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py | 2 +-
 sr_robot_commander/test/test_robot_commander.py                 | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 0898fda03..d5e35d259 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 
-# Copyright 2015 Shadow Robot Company Ltd.
+# Copyright 2015-2022 Shadow Robot Company Ltd.
 #
 # This program is free software: you can redistribute it and/or modify it
 # under the terms of the GNU General Public License as published by the Free
diff --git a/sr_robot_commander/test/test_robot_commander.py b/sr_robot_commander/test/test_robot_commander.py
index 566427559..a079fd578 100755
--- a/sr_robot_commander/test/test_robot_commander.py
+++ b/sr_robot_commander/test/test_robot_commander.py
@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 
-# Copyright 2021 Shadow Robot Company Ltd.
+# Copyright 2021, 2022 Shadow Robot Company Ltd.
 #
 # This program is free software: you can redistribute it and/or modify it
 # under the terms of the GNU General Public License as published by the Free

From f2987744aac3f70520ceb8ff4a3bfa89fd21d94a Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Wed, 19 Oct 2022 13:57:11 +0000
Subject: [PATCH 17/35] small style changes

---
 .../sr_robot_commander/sr_robot_commander.py  | 24 +++++++++++--------
 1 file changed, 14 insertions(+), 10 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index d5e35d259..b58ba6ba4 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -106,8 +106,8 @@ def __init__(self, name):
         controller_list_param = rospy.get_param("/move_group/controller_list")
 
         robot_description = rospy.get_param("/robot_description")
-        self._joint_limits = dict()
-        self._read_joint_limits(robot_description)
+        self._joint_limits = {}
+        self._initialize_joint_limits(robot_description)
 
         # create dictionary with name of controllers and corresponding joints
         self._controllers = {item["name"]: item["joints"] for item in controller_list_param}
@@ -128,7 +128,11 @@ def __init__(self, name):
 
         self._wait_for_set_points()
 
-    def _read_joint_limits(self, robot_description):
+    def _initialize_joint_limits(self, robot_description):
+        """
+        It reads a robot descritpion and updates the joint limits dictionary of the class
+        @param robot_description - Robot description from which the joint limits are going to be read
+        """
         robot_dom = minidom.parseString(robot_description)
         robot = robot_dom.getElementsByTagName('robot')[0]
 
@@ -136,12 +140,12 @@ def _read_joint_limits(self, robot_description):
             if child.nodeType is child.TEXT_NODE:
                 continue
             if child.localName == 'joint':
-                jtype = child.getAttribute('type')
-                if jtype in ['fixed', 'floating', 'planar']:
+                joint_type = child.getAttribute('type')
+                if joint_type in ['fixed', 'floating', 'planar']:
                     continue
                 name = child.getAttribute('name')
 
-                if jtype == 'continuous':
+                if joint_type == 'continuous':
                     minval = -pi
                     maxval = pi
                 else:
@@ -543,13 +547,13 @@ def _bound_state(self, joint_states):
         within their limits
         @return - The joint states updated with the joint poisitions bounded
         """
-        if type(joint_states) == dict:
+        if isinstance(joint_states, dict):
             for joint in joint_states:
                 joint_states[joint] = self._bound_joint(joint_states[joint],
                                                         self._joint_limits[joint])
 
-        elif type(joint_states) == RobotState:
-            for i in range(0, len(joint_states.joint_state.name)):
+        elif isinstance(joint_states, RobotState):
+            for i in range(len(joint_states.joint_state.name)):
                 joint_states.joint_state.position[i] = \
                     self._bound_joint(joint_states.joint_state.position[i],
                                       self._joint_limits[joint_states.joint_state.name[i]])
@@ -1015,7 +1019,7 @@ def _set_point_cb(self, msg):
         """
         joint_names_group = self._move_group_commander.get_active_joints()
         with self._set_points_lock:
-            for i in range(0, len(msg.joint_names)):
+            for i in range(len(msg.joint_names)):
                 if msg.joint_names[i] not in joint_names_group:
                     continue
                 self._set_points.update({msg.joint_names[i]: msg.desired.positions[i]})

From b17bb1a6bd08f9aa56660ba561990f5458893f87 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 31 Oct 2022 09:47:46 +0000
Subject: [PATCH 18/35] reformatted checking for underactuated joint

---
 .../sr_robot_commander/sr_robot_commander.py  | 20 +++++++++++++++----
 1 file changed, 16 insertions(+), 4 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index b58ba6ba4..2d660dbdb 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -17,6 +17,7 @@
 from __future__ import absolute_import
 from __future__ import division
 import threading
+import re
 
 import rospy
 from actionlib import SimpleActionClient
@@ -99,6 +100,8 @@ def __init__(self, name):
 
         self._controllers = {}
 
+        self._underactuated_joint_finder = re.compile('[r,l]h_[F,M,R,L]FJ[1,2]')
+
         rospy.wait_for_service('compute_ik')
         self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
         self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
@@ -137,13 +140,15 @@ def _initialize_joint_limits(self, robot_description):
         robot = robot_dom.getElementsByTagName('robot')[0]
 
         for child in robot.childNodes:
+            name = child.getAttribute('name')
+
             if child.nodeType is child.TEXT_NODE:
                 continue
+
             if child.localName == 'joint':
                 joint_type = child.getAttribute('type')
                 if joint_type in ['fixed', 'floating', 'planar']:
                     continue
-                name = child.getAttribute('name')
 
                 if joint_type == 'continuous':
                     minval = -pi
@@ -560,6 +565,13 @@ def _bound_state(self, joint_states):
 
         return joint_states
 
+    def _is_joint_underactuated(self, joint_name):
+        """
+        @param joint_name - Name of the joint to check if it is underactuated or not, example format rh_FFJ1
+        @return - boolean indicating if the joint is underactuated or not
+        """
+        return bool(self._underactuated_joint_finder.findall(joint_name))
+
     def get_current_set_points(self):
         """
         Reads from the set points
@@ -572,15 +584,15 @@ def get_current_set_points(self):
         set_points = {}
 
         for joint in raw_set_points:
-            if "J0" in joint:
-                # Get j1 j2 ratio from current state
+            if self._is_joint_underactuated(joint):
+                # Underactuated joint, get j1 j2 ratio from current state
                 underactuation_ratio = current_state[f"{joint[:-2]}J1"]/current_state[f"{joint[:-2]}J2"]
                 ratio_part = raw_set_points[joint]/(underactuation_ratio+1)
                 set_point_j1 = underactuation_ratio*ratio_part
                 set_point_j2 = ratio_part
                 set_points.update({f"{joint[:-2]}J1": set_point_j1})
                 set_points.update({f"{joint[:-2]}J2": set_point_j2})
-            elif joint[5:] not in ["J1", "J2"] or joint[3:-2] in ["WR", "TH"]:
+            else:
                 set_points.update({joint: raw_set_points[joint]})
 
         joint_state = JointState()

From c0e9d31a931921fb4633263aed885ae351ecc8c1 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 31 Oct 2022 09:56:36 +0000
Subject: [PATCH 19/35] refactored underactuated check

---
 .../src/sr_robot_commander/sr_robot_commander.py    | 13 +++++--------
 1 file changed, 5 insertions(+), 8 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 2d660dbdb..f7f0cdd9c 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -596,7 +596,6 @@ def get_current_set_points(self):
                 set_points.update({joint: raw_set_points[joint]})
 
         joint_state = JointState()
-        joint_state.header = Header()
         joint_state.header.stamp = rospy.Time.now()
         for joint in set_points:
             joint_state.name.append(joint)
@@ -931,9 +930,7 @@ def _set_up_set_points_subscribers(self, controllers_list):
                         topics_subscribed.append(topic_name)
 
         for joint_name in joint_names_group:
-            joint_member = joint_name[3:-2]
-            joint_number = joint_name[5:]
-            if joint_number in ["J1", "J2"] and joint_member not in ["WR", "TH"]:
+            if self._is_joint_underactuated(joint_name):
                 topic_name = f"/sh_{joint_name.lower()[0:5]}j0_position_controller/state"
                 rospy.Subscriber(topic_name,
                                  JointControllerState,
@@ -1031,15 +1028,15 @@ def _set_point_cb(self, msg):
         """
         joint_names_group = self._move_group_commander.get_active_joints()
         with self._set_points_lock:
-            for i in range(len(msg.joint_names)):
-                if msg.joint_names[i] not in joint_names_group:
+            for index, joint in enumerate(msg.joint_names):
+                if joint not in joint_names_group:
                     continue
-                self._set_points.update({msg.joint_names[i]: msg.desired.positions[i]})
+                self._set_points.update({joint: msg.desired.positions[index]})
             if not self._are_set_points_ready:
                 with self._set_points_cv:
                     joint_names_group = self._move_group_commander.get_active_joints()
                     for joint_name in joint_names_group:
-                        if joint_name[5:] in ["J1", "J2"] and joint_name[3:-2] not in ["WR", "TH"]:
+                        if self._is_joint_underactuated(joint):
                             joint_names_group.append(f"{joint_name[0:5]}J0")
                     for joint_name in joint_names_group:
                         if joint_name not in self._set_points.keys():

From fc05a18623dcebc99845442202a089b9f9e753d9 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 31 Oct 2022 10:51:17 +0000
Subject: [PATCH 20/35] fixed issue having j0 in get current set points

---
 .../src/sr_robot_commander/sr_robot_commander.py      | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index f7f0cdd9c..da2f28726 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -140,8 +140,6 @@ def _initialize_joint_limits(self, robot_description):
         robot = robot_dom.getElementsByTagName('robot')[0]
 
         for child in robot.childNodes:
-            name = child.getAttribute('name')
-
             if child.nodeType is child.TEXT_NODE:
                 continue
 
@@ -149,6 +147,7 @@ def _initialize_joint_limits(self, robot_description):
                 joint_type = child.getAttribute('type')
                 if joint_type in ['fixed', 'floating', 'planar']:
                     continue
+                name = child.getAttribute('name')
 
                 if joint_type == 'continuous':
                     minval = -pi
@@ -577,6 +576,7 @@ def get_current_set_points(self):
         Reads from the set points
         @return - Dictionary which contains the set points of the joints that belong to the move group
         """
+        raw_set_points = {}
         with self._set_points_lock:
             raw_set_points = copy.deepcopy(self._set_points)
 
@@ -592,7 +592,8 @@ def get_current_set_points(self):
                 set_point_j2 = ratio_part
                 set_points.update({f"{joint[:-2]}J1": set_point_j1})
                 set_points.update({f"{joint[:-2]}J2": set_point_j2})
-            else:
+            elif "J0" not in joint:
+                # Avoind adding set points of J0 to the output
                 set_points.update({joint: raw_set_points[joint]})
 
         joint_state = JointState()
@@ -1034,9 +1035,9 @@ def _set_point_cb(self, msg):
                 self._set_points.update({joint: msg.desired.positions[index]})
             if not self._are_set_points_ready:
                 with self._set_points_cv:
-                    joint_names_group = self._move_group_commander.get_active_joints()
                     for joint_name in joint_names_group:
-                        if self._is_joint_underactuated(joint):
+                        if self._is_joint_underactuated(joint_name) and \
+                        f"{joint_name[0:5]}J0" not in joint_names_group:
                             joint_names_group.append(f"{joint_name[0:5]}J0")
                     for joint_name in joint_names_group:
                         if joint_name not in self._set_points.keys():

From ee270c0df1b21f529015209a4eb9260af16ea343 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 31 Oct 2022 10:58:20 +0000
Subject: [PATCH 21/35] undid splitting line

---
 .../src/sr_robot_commander/sr_robot_commander.py               | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index da2f28726..5a45f9c9b 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -1036,8 +1036,7 @@ def _set_point_cb(self, msg):
             if not self._are_set_points_ready:
                 with self._set_points_cv:
                     for joint_name in joint_names_group:
-                        if self._is_joint_underactuated(joint_name) and \
-                        f"{joint_name[0:5]}J0" not in joint_names_group:
+                        if self._is_joint_underactuated(joint_name) and f"{joint_name[0:5]}J0" not in joint_names_group:
                             joint_names_group.append(f"{joint_name[0:5]}J0")
                     for joint_name in joint_names_group:
                         if joint_name not in self._set_points.keys():

From 230364810687b5f7f25347afda9cc2189e7b8805 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Wed, 2 Nov 2022 10:29:25 +0000
Subject: [PATCH 22/35] extended equations for set points j1, j2 to avoid
 division by 0

---
 .../src/sr_robot_commander/sr_robot_commander.py            | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 5a45f9c9b..f91541550 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -586,10 +586,8 @@ def get_current_set_points(self):
         for joint in raw_set_points:
             if self._is_joint_underactuated(joint):
                 # Underactuated joint, get j1 j2 ratio from current state
-                underactuation_ratio = current_state[f"{joint[:-2]}J1"]/current_state[f"{joint[:-2]}J2"]
-                ratio_part = raw_set_points[joint]/(underactuation_ratio+1)
-                set_point_j1 = underactuation_ratio*ratio_part
-                set_point_j2 = ratio_part
+                set_point_j1 = current_state[f"{joint[:-2]}J1"] * raw_set_points[joint] / (current_state[f"{joint[:-2]}J1"] + current_state[f"{joint[:-2]}J2"])
+                set_point_j2 = current_state[f"{joint[:-2]}J2"] * raw_set_points[joint] / (current_state[f"{joint[:-2]}J1"] + current_state[f"{joint[:-2]}J2"])
                 set_points.update({f"{joint[:-2]}J1": set_point_j1})
                 set_points.update({f"{joint[:-2]}J2": set_point_j2})
             elif "J0" not in joint:

From 90dc1aaf687a7fb9326ed286b5b506546b6e8dd6 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Wed, 2 Nov 2022 13:43:12 +0000
Subject: [PATCH 23/35] refactored j0, j1 and j2 equations

---
 .../sr_robot_commander/sr_robot_commander.py  | 33 +++++++++++++++----
 1 file changed, 27 insertions(+), 6 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index f91541550..8fe999592 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -582,14 +582,35 @@ def get_current_set_points(self):
 
         current_state = self.get_current_state()
         set_points = {}
+        joint_names = list(raw_set_points.keys())
 
-        for joint in raw_set_points:
+        for joint in joint_names:
             if self._is_joint_underactuated(joint):
-                # Underactuated joint, get j1 j2 ratio from current state
-                set_point_j1 = current_state[f"{joint[:-2]}J1"] * raw_set_points[joint] / (current_state[f"{joint[:-2]}J1"] + current_state[f"{joint[:-2]}J2"])
-                set_point_j2 = current_state[f"{joint[:-2]}J2"] * raw_set_points[joint] / (current_state[f"{joint[:-2]}J1"] + current_state[f"{joint[:-2]}J2"])
-                set_points.update({f"{joint[:-2]}J1": set_point_j1})
-                set_points.update({f"{joint[:-2]}J2": set_point_j2})
+                # Underactuated joint, split the set point of j0 given the state of j1 and j2
+                joint_0_name = f"{joint[:-2]}J0"
+                joint_1_name = f"{joint[:-2]}J1"
+                joint_2_name = f"{joint[:-2]}J2"
+
+                state_j1 = current_state[joint_1_name]
+                state_j2 = current_state[joint_2_name]
+                set_point_j0 = raw_set_points[joint_0_name]
+
+                if (state_j1 + state_j2) == 0:
+                    # Avoid division by 0, use directly the set point of J1 and J2
+                    # from the trajectory controller
+                    set_point_j1 = raw_set_points[joint_1_name]
+                    set_point_j2 = raw_set_points[joint_2_name]
+                else:
+                    # Split j0 given state of j1 and j2
+                    set_point_j1 = state_j1 * set_point_j0 / (state_j1 + state_j2)
+                    set_point_j2 = state_j2 * set_point_j0 / (state_j1 + state_j2)
+
+                set_points.update({joint_1_name: set_point_j1})
+                set_points.update({joint_2_name: set_point_j2})
+
+                # Avoid executing again this
+                joint_names.remove(joint_1_name)
+                joint_names.remove(joint_2_name)
             elif "J0" not in joint:
                 # Avoind adding set points of J0 to the output
                 set_points.update({joint: raw_set_points[joint]})

From 99e816e102d3ec8652313ae9d1e57c8ad4c15963 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Wed, 2 Nov 2022 13:51:01 +0000
Subject: [PATCH 24/35] renamed variables to be more readable

---
 .../src/sr_robot_commander/sr_robot_commander.py          | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 8fe999592..b3cd93a00 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -318,8 +318,8 @@ def move_to_joint_value_target(self, joint_states, wait=True,
         if angle_degrees:
             joint_states_cpy.update((joint, radians(i))
                                     for joint, i in joint_states_cpy.items())
-        set_points, move_group_robot_state = self.get_current_set_points()
-        self._move_group_commander.set_start_state(move_group_robot_state)
+        set_points, robot_state_set_points = self.get_current_set_points()
+        self._move_group_commander.set_start_state(robot_state_set_points)
         set_points = self._bound_state(set_points)
         self._move_group_commander.set_joint_value_target(set_points)
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
@@ -339,13 +339,13 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
         @return - motion plan (RobotTrajectory msg) that contains the trajectory to the set goal state.
         """
         joint_states_cpy = copy.deepcopy(joint_states)
-        set_points, robot_state = self.get_current_set_points()
+        set_points, robot_state_set_points = self.get_current_set_points()
 
         if angle_degrees:
             joint_states_cpy.update((joint, radians(i))
                                     for joint, i in joint_states_cpy.items())
         if custom_start_state is None:
-            self._move_group_commander.set_start_state(robot_state)
+            self._move_group_commander.set_start_state(robot_state_set_points)
         else:
             self._move_group_commander.set_start_state(custom_start_state)
 

From df2c826e3ee6be676369e52883436e28f9bbb226 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Wed, 2 Nov 2022 14:19:54 +0000
Subject: [PATCH 25/35] fixed comment

---
 .../src/sr_robot_commander/sr_robot_commander.py               | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index b3cd93a00..d4a745870 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -574,7 +574,8 @@ def _is_joint_underactuated(self, joint_name):
     def get_current_set_points(self):
         """
         Reads from the set points
-        @return - Dictionary which contains the set points of the joints that belong to the move group
+        @return - Tuple that contains a dictionary mapping joint names with their respective set point, and
+        a RobotState object which contains the same set points.
         """
         raw_set_points = {}
         with self._set_points_lock:

From d65af25eed274e3b792895082df6514e6862d31c Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Fri, 18 Nov 2022 12:56:32 +0000
Subject: [PATCH 26/35] modified set point with state if it is far

---
 .../sr_robot_commander/sr_robot_commander.py  | 24 ++++++++++++++++---
 1 file changed, 21 insertions(+), 3 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index d4a745870..6c124e728 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -349,6 +349,10 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
         else:
             self._move_group_commander.set_start_state(custom_start_state)
 
+        states = self.get_current_state()
+        for joint_name in set_points.keys():
+            print(f"Joint {joint_name}, state: {states[joint_name]} and set point: {set_points[joint_name]}")
+
         set_points_bounded = self._bound_state(set_points)
         self._move_group_commander.set_joint_value_target(set_points_bounded)
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
@@ -580,13 +584,16 @@ def get_current_set_points(self):
         raw_set_points = {}
         with self._set_points_lock:
             raw_set_points = copy.deepcopy(self._set_points)
+            print(f"Raw set points: {raw_set_points}")
 
         current_state = self.get_current_state()
         set_points = {}
         joint_names = list(raw_set_points.keys())
 
+        underactuated_done = {"FF": False, "MF": False, "RF": False, "LF": False}
+
         for joint in joint_names:
-            if self._is_joint_underactuated(joint):
+            if self._is_joint_underactuated(joint) and not underactuated_done[joint[3:5]]:
                 # Underactuated joint, split the set point of j0 given the state of j1 and j2
                 joint_0_name = f"{joint[:-2]}J0"
                 joint_1_name = f"{joint[:-2]}J1"
@@ -606,14 +613,25 @@ def get_current_set_points(self):
                     set_point_j1 = state_j1 * set_point_j0 / (state_j1 + state_j2)
                     set_point_j2 = state_j2 * set_point_j0 / (state_j1 + state_j2)
 
+                # Check if the set point is very far from the real state
+                if abs(set_point_j1 - current_state[joint_1_name]) > 0.1:
+                    print(f"Joint {joint_1_name} set point {set_point_j1} deviates more than 0.1 to {current_state[joint_1_name]}")
+                    set_point_j1 = current_state[joint_1_name]
+                if abs(set_point_j2 - current_state[joint_2_name]) > 0.1:
+                    print(f"Joint {joint_2_name} set point {set_point_j2} deviates more than 0.1 to {current_state[joint_2_name]}")
+                    set_point_j2 = current_state[joint_2_name]
+
                 set_points.update({joint_1_name: set_point_j1})
                 set_points.update({joint_2_name: set_point_j2})
 
                 # Avoid executing again this
-                joint_names.remove(joint_1_name)
-                joint_names.remove(joint_2_name)
+                underactuated_done[joint[3:5]] = True
             elif "J0" not in joint:
                 # Avoind adding set points of J0 to the output
+                set_point_value = raw_set_points[joint]
+                if abs(set_point_value - current_state[joint]) > 0.1:
+                    print(f"Joint {joint} set point {set_point_value} deviates more than 0.1 to {current_state[joint]}")
+                    set_point_value = current_state[joint]
                 set_points.update({joint: raw_set_points[joint]})
 
         joint_state = JointState()

From f8d1de54814a2579b1282f19dfc0ed5a9cb3f0f9 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 21 Nov 2022 10:42:00 +0000
Subject: [PATCH 27/35] update set points in case they are far from the current
 state

---
 .../sr_robot_commander/sr_robot_commander.py  | 77 +++++++++++--------
 1 file changed, 46 insertions(+), 31 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 6c124e728..c9c829334 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -107,7 +107,7 @@ def __init__(self, name):
         self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
 
         controller_list_param = rospy.get_param("/move_group/controller_list")
-
+        self._allowed_start_tolerance = rospy.get_param("/move_group/trajectory_execution/allowed_start_tolerance", 0.1)
         robot_description = rospy.get_param("/robot_description")
         self._joint_limits = {}
         self._initialize_joint_limits(robot_description)
@@ -319,6 +319,7 @@ def move_to_joint_value_target(self, joint_states, wait=True,
             joint_states_cpy.update((joint, radians(i))
                                     for joint, i in joint_states_cpy.items())
         set_points, robot_state_set_points = self.get_current_set_points()
+        set_points, robot_state_set_points = self.fix_set_points_if_far_from_state(set_points, robot_state_set_points)
         self._move_group_commander.set_start_state(robot_state_set_points)
         set_points = self._bound_state(set_points)
         self._move_group_commander.set_joint_value_target(set_points)
@@ -340,6 +341,7 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
         """
         joint_states_cpy = copy.deepcopy(joint_states)
         set_points, robot_state_set_points = self.get_current_set_points()
+        set_points, robot_state_set_points = self.fix_set_points_if_far_from_state(set_points, robot_state_set_points)
 
         if angle_degrees:
             joint_states_cpy.update((joint, radians(i))
@@ -349,10 +351,6 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
         else:
             self._move_group_commander.set_start_state(custom_start_state)
 
-        states = self.get_current_state()
-        for joint_name in set_points.keys():
-            print(f"Joint {joint_name}, state: {states[joint_name]} and set point: {set_points[joint_name]}")
-
         set_points_bounded = self._bound_state(set_points)
         self._move_group_commander.set_joint_value_target(set_points_bounded)
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
@@ -584,20 +582,20 @@ def get_current_set_points(self):
         raw_set_points = {}
         with self._set_points_lock:
             raw_set_points = copy.deepcopy(self._set_points)
-            print(f"Raw set points: {raw_set_points}")
 
         current_state = self.get_current_state()
         set_points = {}
         joint_names = list(raw_set_points.keys())
 
-        underactuated_done = {"FF": False, "MF": False, "RF": False, "LF": False}
+        # Lookup table to avoid processing the same underactuated joint group more than one time
+        underactuated_done = {"FF": False, "MF": False, "RF": False, "LF": False, "TH": True, "WR": True}
 
-        for joint in joint_names:
-            if self._is_joint_underactuated(joint) and not underactuated_done[joint[3:5]]:
+        for joint_name in joint_names:
+            if self._is_joint_underactuated(joint_name) and not underactuated_done[joint_name[3:5]]:
                 # Underactuated joint, split the set point of j0 given the state of j1 and j2
-                joint_0_name = f"{joint[:-2]}J0"
-                joint_1_name = f"{joint[:-2]}J1"
-                joint_2_name = f"{joint[:-2]}J2"
+                joint_0_name = f"{joint_name[:-2]}J0"
+                joint_1_name = f"{joint_name[:-2]}J1"
+                joint_2_name = f"{joint_name[:-2]}J2"
 
                 state_j1 = current_state[joint_1_name]
                 state_j2 = current_state[joint_2_name]
@@ -613,38 +611,55 @@ def get_current_set_points(self):
                     set_point_j1 = state_j1 * set_point_j0 / (state_j1 + state_j2)
                     set_point_j2 = state_j2 * set_point_j0 / (state_j1 + state_j2)
 
-                # Check if the set point is very far from the real state
-                if abs(set_point_j1 - current_state[joint_1_name]) > 0.1:
-                    print(f"Joint {joint_1_name} set point {set_point_j1} deviates more than 0.1 to {current_state[joint_1_name]}")
-                    set_point_j1 = current_state[joint_1_name]
-                if abs(set_point_j2 - current_state[joint_2_name]) > 0.1:
-                    print(f"Joint {joint_2_name} set point {set_point_j2} deviates more than 0.1 to {current_state[joint_2_name]}")
-                    set_point_j2 = current_state[joint_2_name]
-
                 set_points.update({joint_1_name: set_point_j1})
                 set_points.update({joint_2_name: set_point_j2})
 
                 # Avoid executing again this
-                underactuated_done[joint[3:5]] = True
-            elif "J0" not in joint:
-                # Avoind adding set points of J0 to the output
-                set_point_value = raw_set_points[joint]
-                if abs(set_point_value - current_state[joint]) > 0.1:
-                    print(f"Joint {joint} set point {set_point_value} deviates more than 0.1 to {current_state[joint]}")
-                    set_point_value = current_state[joint]
-                set_points.update({joint: raw_set_points[joint]})
+                underactuated_done[joint_name[3:5]] = True
+            else:
+                if "J0" not in joint_name:
+                    # Avoind adding set points of J0 to the output
+                    set_points.update({joint_name: raw_set_points[joint_name]})
 
+        # Create the RobotState since some moveit functions require this object instead of
+        # a dictionary. The JointState is filled first.
         joint_state = JointState()
         joint_state.header.stamp = rospy.Time.now()
-        for joint in set_points:
-            joint_state.name.append(joint)
-            joint_state.position.append(set_points[joint])
+        for joint_name in set_points:
+            joint_state.name.append(joint_name)
+            joint_state.position.append(set_points[joint_name])
 
         move_group_robot_state = RobotState()
         move_group_robot_state.joint_state = joint_state
 
         return set_points, move_group_robot_state
 
+    def fix_set_points_if_far_from_state(self, set_points, robot_state):
+        """
+        Updates the set points with the current stateof the robot if the set point
+        is far from the state given a tolerance. This can happen when the joint is not able
+        to achieve a certain set point of the controller such as when grasping an object.
+        @param set_points - Dictionary that maps the joint names with their set points.
+        @param robot_state - RobotState object that contains the joint names and their set points
+        as the position of the joints.
+        @param tolerance - Tolerance to decide if the set point is updated or not given the
+        current state of the joint.
+        @return - Tuple that contains the updated set_points and updated robot_state.
+        """
+        current_state = self.get_current_state()
+
+        # Update set points
+        for joint_name in list(set_points.keys()):
+            if abs(set_points[joint_name] - current_state[joint_name]) > self._allowed_start_tolerance:
+                set_points[joint_name] = current_state[joint_name]
+
+        # Update robot state
+        for index, joint_name in enumerate(robot_state.joint_state.name):
+            if abs(robot_state.joint_state.position[index] - current_state[joint_name]) > self._allowed_start_tolerance:
+                robot_state.joint_state.position[index] = current_state[joint_name]
+
+        return set_points, robot_state
+
     def move_to_named_target(self, name, wait=True):
         """
         Set target of the robot's links and moves to it

From f23062c272fffa37bd0354f874d9a9d2c4042ebe Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Mon, 21 Nov 2022 13:47:03 +0000
Subject: [PATCH 28/35] added comments and use execute instead of go

---
 .../src/sr_robot_commander/sr_robot_commander.py  | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index c9c829334..de53db0f9 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -318,13 +318,20 @@ def move_to_joint_value_target(self, joint_states, wait=True,
         if angle_degrees:
             joint_states_cpy.update((joint, radians(i))
                                     for joint, i in joint_states_cpy.items())
+        # Create the set points dictionary and robot state
         set_points, robot_state_set_points = self.get_current_set_points()
+        # In case that the set points are far from the state, use the state of the joints
         set_points, robot_state_set_points = self.fix_set_points_if_far_from_state(set_points, robot_state_set_points)
+        # Setting the start state of the plan
         self._move_group_commander.set_start_state(robot_state_set_points)
+        # Bound the set points to make sure they lay within the joint limits
         set_points = self._bound_state(set_points)
+        # Set the target of the plan
         self._move_group_commander.set_joint_value_target(set_points)
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
-        self._move_group_commander.go(wait=wait)
+
+        # Create and execute the plan
+        self._move_group_commander.execute(self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX], wait=wait)
 
     def set_start_state_to_current_state(self):
         return self._move_group_commander.set_start_state_to_current_state()
@@ -340,20 +347,26 @@ def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_s
         @return - motion plan (RobotTrajectory msg) that contains the trajectory to the set goal state.
         """
         joint_states_cpy = copy.deepcopy(joint_states)
+        # Create the set points dictionary and robot state
         set_points, robot_state_set_points = self.get_current_set_points()
+        # In case that the set points are far from the state, use the state of the joints
         set_points, robot_state_set_points = self.fix_set_points_if_far_from_state(set_points, robot_state_set_points)
 
         if angle_degrees:
             joint_states_cpy.update((joint, radians(i))
                                     for joint, i in joint_states_cpy.items())
+        # Set the first point of the plan
         if custom_start_state is None:
             self._move_group_commander.set_start_state(robot_state_set_points)
         else:
             self._move_group_commander.set_start_state(custom_start_state)
 
+        # Make sure that the set points lay within the joint limits
         set_points_bounded = self._bound_state(set_points)
+        # Set the target of the plan
         self._move_group_commander.set_joint_value_target(set_points_bounded)
         self._move_group_commander.set_joint_value_target(joint_states_cpy)
+        # Create the plan
         self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
         return self.__plan
 

From d83c4626471d6c7212c98fe8833abdef5b69ce81 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Tue, 22 Nov 2022 13:59:39 +0000
Subject: [PATCH 29/35] refactored checking topics subscribed

---
 .../sr_robot_commander/sr_robot_commander.py  | 22 ++++++++++---------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index de53db0f9..21377c5db 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -570,7 +570,6 @@ def _bound_state(self, joint_states):
             for joint in joint_states:
                 joint_states[joint] = self._bound_joint(joint_states[joint],
                                                         self._joint_limits[joint])
-
         elif isinstance(joint_states, RobotState):
             for i in range(len(joint_states.joint_state.name)):
                 joint_states.joint_state.position[i] = \
@@ -986,25 +985,28 @@ def _set_up_set_points_subscribers(self, controllers_list):
         """
         # Get joint names of the group
         joint_names_group = self._move_group_commander.get_active_joints()
-        topics_subscribed = []
+        trajectory_controllers_subscribed = []
+        j0_position_controllers_subscribed = []
 
+        # Subscribe to all the trajectory controllers that contain joints of this group
         for controller_name in controllers_list.keys():
             for joint_name in joint_names_group:
                 if joint_name in controllers_list[controller_name]:
                     topic_name = f"/{controller_name}/state"
-                    if topic_name not in topics_subscribed:
+                    if topic_name not in trajectory_controllers_subscribed:
                         rospy.Subscriber(topic_name, JointTrajectoryControllerState, self._set_point_cb, queue_size=1)
-                        topics_subscribed.append(topic_name)
+                        trajectory_controllers_subscribed.append(topic_name)
 
+        # Susbribe to the j0 position controllers of the joints contained in this group
         for joint_name in joint_names_group:
             if self._is_joint_underactuated(joint_name):
                 topic_name = f"/sh_{joint_name.lower()[0:5]}j0_position_controller/state"
-                rospy.Subscriber(topic_name,
-                                 JointControllerState,
-                                 self._set_point_j0_cb, f"{joint_name[0:5]}J0",
-                                 queue_size=1)
-                joint_names_group.remove(f"{joint_name[0:5]}J1")
-                joint_names_group.remove(f"{joint_name[0:5]}J2")
+                if topic_name not in j0_position_controllers_subscribed:
+                    rospy.Subscriber(topic_name,
+                                    JointControllerState,
+                                    self._set_point_j0_cb, f"{joint_name[0:5]}J0",
+                                    queue_size=1)
+                    j0_position_controllers_subscribed.append(topic_name)
 
     def _set_up_action_client(self, controller_list):
         """

From 6a09a298086fad2f3045ad1a8235bb378b2994cb Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Tue, 22 Nov 2022 14:00:28 +0000
Subject: [PATCH 30/35] fixed typo

---
 sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 21377c5db..1f22be7df 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -997,7 +997,7 @@ def _set_up_set_points_subscribers(self, controllers_list):
                         rospy.Subscriber(topic_name, JointTrajectoryControllerState, self._set_point_cb, queue_size=1)
                         trajectory_controllers_subscribed.append(topic_name)
 
-        # Susbribe to the j0 position controllers of the joints contained in this group
+        # Subscribe to the j0 position controllers of the joints contained in this group
         for joint_name in joint_names_group:
             if self._is_joint_underactuated(joint_name):
                 topic_name = f"/sh_{joint_name.lower()[0:5]}j0_position_controller/state"

From 0952bd758d73a5daa1327509dbc73bc4cd97b636 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Tue, 22 Nov 2022 14:11:42 +0000
Subject: [PATCH 31/35] lint error

---
 .../src/sr_robot_commander/sr_robot_commander.py            | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 1f22be7df..8df6829e3 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -1003,9 +1003,9 @@ def _set_up_set_points_subscribers(self, controllers_list):
                 topic_name = f"/sh_{joint_name.lower()[0:5]}j0_position_controller/state"
                 if topic_name not in j0_position_controllers_subscribed:
                     rospy.Subscriber(topic_name,
-                                    JointControllerState,
-                                    self._set_point_j0_cb, f"{joint_name[0:5]}J0",
-                                    queue_size=1)
+                                     JointControllerState,
+                                     self._set_point_j0_cb, f"{joint_name[0:5]}J0",
+                                     queue_size=1)
                     j0_position_controllers_subscribed.append(topic_name)
 
     def _set_up_action_client(self, controller_list):

From 5fb64ed198bbeca899878f57466f174a35cf9865 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Thu, 24 Nov 2022 13:42:09 +0000
Subject: [PATCH 32/35] used array instead of dictionary

---
 .../src/sr_robot_commander/sr_robot_commander.py          | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 8df6829e3..c11f15c90 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -599,11 +599,11 @@ def get_current_set_points(self):
         set_points = {}
         joint_names = list(raw_set_points.keys())
 
-        # Lookup table to avoid processing the same underactuated joint group more than one time
-        underactuated_done = {"FF": False, "MF": False, "RF": False, "LF": False, "TH": True, "WR": True}
+        # Store the fingers ids from which the underactuated joints have been processed
+        underactuation_fingers_processed = []
 
         for joint_name in joint_names:
-            if self._is_joint_underactuated(joint_name) and not underactuated_done[joint_name[3:5]]:
+            if self._is_joint_underactuated(joint_name) and joint_name[3:5] not in underactuation_fingers_processed:
                 # Underactuated joint, split the set point of j0 given the state of j1 and j2
                 joint_0_name = f"{joint_name[:-2]}J0"
                 joint_1_name = f"{joint_name[:-2]}J1"
@@ -627,7 +627,7 @@ def get_current_set_points(self):
                 set_points.update({joint_2_name: set_point_j2})
 
                 # Avoid executing again this
-                underactuated_done[joint_name[3:5]] = True
+                underactuation_fingers_processed.append(joint_name[3:5])
             else:
                 if "J0" not in joint_name:
                     # Avoind adding set points of J0 to the output

From 22177ad583f19570e38117e31a63449efd2d2989 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Tue, 29 Nov 2022 14:33:00 +0000
Subject: [PATCH 33/35] fixed mistake in documentation

---
 .../src/sr_robot_commander/sr_robot_commander.py              | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index c11f15c90..e8eb298b5 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -649,13 +649,11 @@ def get_current_set_points(self):
     def fix_set_points_if_far_from_state(self, set_points, robot_state):
         """
         Updates the set points with the current stateof the robot if the set point
-        is far from the state given a tolerance. This can happen when the joint is not able
+        is far from the state given the allowed_start_tolerance. This can happen when the joint is not able
         to achieve a certain set point of the controller such as when grasping an object.
         @param set_points - Dictionary that maps the joint names with their set points.
         @param robot_state - RobotState object that contains the joint names and their set points
         as the position of the joints.
-        @param tolerance - Tolerance to decide if the set point is updated or not given the
-        current state of the joint.
         @return - Tuple that contains the updated set_points and updated robot_state.
         """
         current_state = self.get_current_state()

From 12e6217962cf52a1bd7a5781fe1b183cd3da3797 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Tue, 13 Dec 2022 09:49:37 +0000
Subject: [PATCH 34/35] changed list name

---
 .../src/sr_robot_commander/sr_robot_commander.py     | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index e8eb298b5..25999ac3e 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -983,28 +983,28 @@ def _set_up_set_points_subscribers(self, controllers_list):
         """
         # Get joint names of the group
         joint_names_group = self._move_group_commander.get_active_joints()
-        trajectory_controllers_subscribed = []
-        j0_position_controllers_subscribed = []
+        trajectory_controllers_state_subscribed = []
+        j0_position_controllers_state_subscribed = []
 
         # Subscribe to all the trajectory controllers that contain joints of this group
         for controller_name in controllers_list.keys():
             for joint_name in joint_names_group:
                 if joint_name in controllers_list[controller_name]:
                     topic_name = f"/{controller_name}/state"
-                    if topic_name not in trajectory_controllers_subscribed:
+                    if topic_name not in trajectory_controllers_state_subscribed:
                         rospy.Subscriber(topic_name, JointTrajectoryControllerState, self._set_point_cb, queue_size=1)
-                        trajectory_controllers_subscribed.append(topic_name)
+                        trajectory_controllers_state_subscribed.append(topic_name)
 
         # Subscribe to the j0 position controllers of the joints contained in this group
         for joint_name in joint_names_group:
             if self._is_joint_underactuated(joint_name):
                 topic_name = f"/sh_{joint_name.lower()[0:5]}j0_position_controller/state"
-                if topic_name not in j0_position_controllers_subscribed:
+                if topic_name not in j0_position_controllers_state_subscribed:
                     rospy.Subscriber(topic_name,
                                      JointControllerState,
                                      self._set_point_j0_cb, f"{joint_name[0:5]}J0",
                                      queue_size=1)
-                    j0_position_controllers_subscribed.append(topic_name)
+                    j0_position_controllers_state_subscribed.append(topic_name)
 
     def _set_up_action_client(self, controller_list):
         """

From 543eb5221b70e3838e7b482b0f64ea9403860b41 Mon Sep 17 00:00:00 2001
From: Jesus Ferrandiz <jeferrandiz98@gmail.com>
Date: Tue, 13 Dec 2022 10:09:23 +0000
Subject: [PATCH 35/35] fixed typo

---
 sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 25999ac3e..531088304 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -648,7 +648,7 @@ def get_current_set_points(self):
 
     def fix_set_points_if_far_from_state(self, set_points, robot_state):
         """
-        Updates the set points with the current stateof the robot if the set point
+        Updates the set points with the current state of the robot if the set point
         is far from the state given the allowed_start_tolerance. This can happen when the joint is not able
         to achieve a certain set point of the controller such as when grasping an object.
         @param set_points - Dictionary that maps the joint names with their set points.