Introduce an ArmJointsPiece, fix Hero joints rename hack

robot_skills/src/robot_skills/arms.py

@@ -319,6 +319,28 @@ class GripperState(object):
     CLOSE = "close"
 
 
+class ArmJointsPiece:
+    """
+    :var arm_joint_names: Names of all available arm joints.
+    :var torso_joint_names: Names of all available torso joints.
+    """
+    def __init__(self, arm_joints_suffix: str=""):
+        """
+        Constructor.
+
+        :param arm_joints_suffix: Suffix to append to names of arm joints.
+        """
+        joint_names = self.load_param('skills/arm/joint_names')
+        self.arm_joint_names = [name + arm_joints_suffix for name in joint_names]
+        self.torso_joint_names = self.load_param('skills/torso/joint_names')
+
+    def get_arm_joint_names(self):
+        return self.arm_joint_names
+
+    def get_torso_joint_names(self):
+        return sef.torso_joint_names
+
+
 class Arm(RobotPart):
     """
     A single arm can be either left or right, extends Arms:
@@ -332,14 +354,15 @@ class Arm(RobotPart):
     #To open left gripper
     >>> left.send_gripper_goal_open(10)  # doctest: +SKIP
     """
-    def __init__(self, robot_name, tf_listener, get_joint_states, side):
+    def __init__(self, robot_name, tf_listener, get_joint_states, side: str, joints_piece: ArmJointsPiece=None):
         """
         constructor
 
         :param robot_name: robot_name
         :param tf_listener: tf_server.TFClient()
         :param get_joint_states: get_joint_states function for getting the last joint states
         :param side: left or right
+        :param joints_piece If specified, arm part containing the arm and torso joint names.
         """
         super(Arm, self).__init__(robot_name=robot_name, tf_listener=tf_listener)
         self.side = side
@@ -359,9 +382,10 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side):
         go = self.load_param('skills/arm/' + self.side + '/base_offset')
         self._base_offset = kdl.Vector(go["x"], go["y"], go["z"])
 
-        self.joint_names = self.load_param('skills/arm/joint_names')
-        self.joint_names = [name + "_" + self.side for name in self.joint_names]
-        self.torso_joint_names = self.load_param('skills/torso/joint_names')
+        if joints_piece:
+            self._joints_part = joints_piece
+        else:
+            self._joints_part = ArmJointsPiece(suffix="_" + side)
 
         self.default_configurations = self.load_param('skills/arm/default_configurations')
         self.default_trajectories   = self.load_param('skills/arm/default_trajectories')
@@ -734,10 +758,12 @@ def _send_joint_trajectory(self, joints_references, max_joint_vel=0.7, timeout=r
         if not joints_references:
             return False
 
-        if len(joints_references[0]) == len(self.joint_names) + len(self.torso_joint_names):
-            joint_names = self.torso_joint_names + self.joint_names
+        arm_joint_names = self._joints_part.get_arm_joint_names()
+        torso_joint_names = self._joints_part.get_torso_joint_names()
+        if len(joints_references[0]) == len(arm_joint_names) + len(torso_joint_names):
+            joint_names = torso_joint_names + arm_joint_names
         else:
-            joint_names = self.joint_names
+            joint_names = arm_joint_names
 
         if isinstance(max_joint_vel, (float, int)):
             max_joint_vel = [max_joint_vel]*len(joint_names)
@@ -763,8 +789,7 @@ def _send_joint_trajectory(self, joints_references, max_joint_vel=0.7, timeout=r
                 positions=joints_reference,
                 time_from_start=rospy.Duration.from_sec(time_from_start)))
 
-        joint_trajectory = JointTrajectory(joint_names=joint_names,
-                                           points=ps)
+        joint_trajectory = JointTrajectory(joint_names=joint_names, points=ps)
         goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory, goal_time_tolerance=timeout)
 
         rospy.logdebug("Send {0} arm to jointcoords \n{1}".format(self.side, ps))
@@ -894,7 +919,7 @@ def __repr__(self):
 
 
 class ForceSensingArm(Arm):
-    def __init__(self, robot_name, tf_listener, get_joint_states, side):
+    def __init__(self, robot_name, tf_listener, get_joint_states, side: str, joints_piece: ArmJointsPiece=None):
         """
         constructor
 
@@ -904,8 +929,10 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side):
         :param tf_listener: tf_server.TFClient()
         :param get_joint_states: get_joint_states function for getting the last joint states
         :param side: left or right
+        :param joints_piece: If specified, arm part containing the arm and torso joint names.
+
         """
-        super(ForceSensingArm, self).__init__(robot_name, tf_listener, get_joint_states, side)
+        super(ForceSensingArm, self).__init__(robot_name, tf_listener, get_joint_states, side, joints_piece)
 
         self.force_sensor = ForceSensor(robot_name, tf_listener, "/" + robot_name + "/wrist_wrench/raw")
 
@@ -931,16 +958,17 @@ def move_down_until_force_sensor_edge_up(self, timeout=10, retract_distance=0.01
         self._ac_joint_traj.send_goal(self._make_goal(current_joint_state, 0.5))
 
     def _make_goal(self, current_joint_state, timeout):
-        positions = [current_joint_state[n] for n in self.joint_names]
+        arm_joint_names = self._joints_part.get_arm_joint_names()
+        positions = [current_joint_state[n] for n in arm_joint_names]
         points = [JointTrajectoryPoint(positions=positions,
                                        time_from_start=rospy.Duration.from_sec(timeout))]
-        trajectory = JointTrajectory(joint_names=self.joint_names, points=points)
+        trajectory = JointTrajectory(joint_names=arm_joint_names, points=points)
         return FollowJointTrajectoryGoal(trajectory=trajectory)
 
 
 class FakeArm(RobotPart):
-    def __init__(self, robot_name, tf_listener, side):
-        super(FakeArm, self).__init__(robot_name=robot_name, tf_listener=tf_listener)
+    def __init__(self, robot_name, tf_listener, side: str, joints_piece: ArmJointsPiece=None):
+        super(FakeArm, self).__init__(robot_name=robot_name, tf_listener=tf_listener, joints_piece)
         self.side = side
         if (self.side is "left") or (self.side is "right"):
             pass

robot_skills/src/robot_skills/hero.py

@@ -14,9 +14,10 @@ def __init__(self, wait_services=False):
         self.add_body_part('base', base.Base(self.robot_name, self.tf_listener))
         self.add_body_part('torso', torso.Torso(self.robot_name, self.tf_listener, self.get_joint_states))
 
+        joints_piece = arms.ArmJointsPiece()
         self.add_arm_part(
             'leftArm',
-            arms.ForceSensingArm(self.robot_name, self.tf_listener, self.get_joint_states, "left")
+            arms.ForceSensingArm(self.robot_name, self.tf_listener, self.get_joint_states, "left", joints_piece=joints_piece)
         )
 
         self.add_body_part('head', head.Head(self.robot_name, self.tf_listener))
@@ -44,9 +45,6 @@ def __init__(self, wait_services=False):
         # Reasoning/world modeling
         self.add_body_part('ed', world_model_ed.ED(self.robot_name, self.tf_listener))
 
-        #rename joint names
-        self.parts['leftArm'].joint_names = self.parts['leftArm'].load_param('skills/arm/joint_names')
-
         # These don't work for HSR because (then) Toyota's diagnostics aggregator makes the robot go into error somehow
         for part in self.parts.itervalues():
             part.unsubscribe_hardware_status()