support joint trajectories

docs/arm_usage.rst

@@ -11,6 +11,8 @@ parts of the arm.
 +------------------------+------------------------+
 |/spot/arm_joint_move    |                        |
 +------------------------+------------------------+
+|/spot/joint_trajectory  |                        |
++------------------------+------------------------+
 |/spot/arm_stow          |                        |
 +------------------------+------------------------+
 |/spot/arm_unstow        |                        |

docs/html/_sources/arm_usage.rst.txt

@@ -11,6 +11,8 @@ parts of the arm.
 +------------------------+------------------------+
 |/spot/arm_joint_move    |                        |
 +------------------------+------------------------+
+|/spot/joint_trajectory  |                        |
++------------------------+------------------------+
 |/spot/arm_stow          |                        |
 +------------------------+------------------------+
 |/spot/arm_unstow        |                        |

docs/html/arm_usage.html

@@ -91,28 +91,31 @@ <h1>Arm control<a class="headerlink" href="#arm-control" title="Permalink to thi
 <tr class="row-odd"><td>/spot/arm_joint_move</td>
 <td>&#160;</td>
 </tr>
-<tr class="row-even"><td>/spot/arm_stow</td>
+<tr class="row-even"><td>/spot/joint_trajectory</td>
 <td>&#160;</td>
 </tr>
-<tr class="row-odd"><td>/spot/arm_unstow</td>
+<tr class="row-odd"><td>/spot/arm_stow</td>
 <td>&#160;</td>
 </tr>
-<tr class="row-even"><td>/spot/force_trajectory</td>
+<tr class="row-even"><td>/spot/arm_unstow</td>
 <td>&#160;</td>
 </tr>
-<tr class="row-odd"><td>/spot/grasp_3d</td>
+<tr class="row-odd"><td>/spot/force_trajectory</td>
 <td>&#160;</td>
 </tr>
-<tr class="row-even"><td>/spot/gripper_angle_open</td>
+<tr class="row-even"><td>/spot/grasp_3d</td>
 <td>&#160;</td>
 </tr>
-<tr class="row-odd"><td>/spot/gripper_close</td>
+<tr class="row-odd"><td>/spot/gripper_angle_open</td>
 <td>&#160;</td>
 </tr>
-<tr class="row-even"><td>/spot/gripper_open</td>
+<tr class="row-even"><td>/spot/gripper_close</td>
 <td>&#160;</td>
 </tr>
-<tr class="row-odd"><td>/spot/gripper_pose</td>
+<tr class="row-odd"><td>/spot/gripper_open</td>
+<td>&#160;</td>
+</tr>
+<tr class="row-even"><td>/spot/gripper_pose</td>
 <td>&#160;</td>
 </tr>
 </tbody>

spot_driver/src/spot_driver/spot_ros.py

@@ -54,6 +54,11 @@
     ArmJointMovementResponse,
     ArmJointMovementRequest,
 )
+from spot_msgs.srv import (
+    ArmJointTrajectory,
+    ArmJointTrajectoryResponse,
+    ArmJointTrajectoryRequest,
+)
 from spot_msgs.srv import (
     GripperAngleMove,
     GripperAngleMoveResponse,
@@ -1267,6 +1272,11 @@ def handle_arm_joint_move(self, srv_data: ArmJointMovementRequest):
         """ROS service handler to send joint movement to the arm to execute"""
         resp = self.spot_wrapper.arm_joint_move(joint_targets=srv_data.joint_target)
         return ArmJointMovementResponse(resp[0], resp[1])
+
+    def handle_joint_trajectory(self, srv_data: ArmJointTrajectoryRequest):
+        """ROS service handler to send joint trajectory to the arm to execute"""
+        resp = self.spot_wrapper.joint_trajectory(joint_targets=srv_data.joint_targets)
+        return ArmJointTrajectoryResponse(resp[0], resp[1])
 
     def handle_force_trajectory(self, srv_data: ArmForceTrajectoryRequest):
         """ROS service handler to send a force trajectory up or down a vertical force"""
@@ -1707,6 +1717,7 @@ def main(self):
             "gripper_angle_open", GripperAngleMove, self.handle_gripper_angle_open
         )
         rospy.Service("arm_joint_move", ArmJointMovement, self.handle_arm_joint_move)
+        rospy.Service("joint_trajectory", ArmJointTrajectory, self.handle_joint_trajectory)
         rospy.Service(
             "force_trajectory", ArmForceTrajectory, self.handle_force_trajectory
         )

spot_driver/src/spot_driver/spot_wrapper.py

@@ -1191,8 +1191,8 @@ def make_arm_trajectory_command(self, arm_joint_trajectory):
             synchronized_command=sync_arm
         )
         return RobotCommandBuilder.build_synchro_command(arm_sync_robot_cmd)
-
-    def arm_joint_move(self, joint_targets):
+    
+    def validate_joint_target(self, joint_target):
         # All perspectives are given when looking at the robot from behind after the unstow service is called
         # Joint1: 0.0 arm points to the front. positive: turn left, negative: turn right)
         # RANGE: -3.14 -> 3.14
@@ -1207,28 +1207,28 @@ def arm_joint_move(self, joint_targets):
         # Joint6: 0.0 Gripper is not rolled, positive is ccw
         # RANGE: -2.87 -> 2.87
         # Values after unstow are: [0.0, -0.9, 1.8, 0.0, -0.9, 0.0]
-        if abs(joint_targets[0]) > 3.14:
+        if abs(joint_target[0]) > 3.14:
             msg = "Joint 1 has to be between -3.14 and 3.14"
-            self._logger.warn(msg)
             return False, msg
-        elif joint_targets[1] > 0.4 or joint_targets[1] < -3.13:
+        elif joint_target[1] > 0.4 or joint_target[1] < -3.13:
             msg = "Joint 2 has to be between -3.13 and 0.4"
-            self._logger.warn(msg)
             return False, msg
-        elif joint_targets[2] > 3.14 or joint_targets[2] < 0.0:
+        elif joint_target[2] > 3.14 or joint_target[2] < 0.0:
             msg = "Joint 3 has to be between 0.0 and 3.14"
-            self._logger.warn(msg)
             return False, msg
-        elif abs(joint_targets[3]) > 2.79253:
+        elif abs(joint_target[3]) > 2.79253:
             msg = "Joint 4 has to be between -2.79253 and 2.79253"
-            self._logger.warn(msg)
             return False, msg
-        elif abs(joint_targets[4]) > 1.8326:
+        elif abs(joint_target[4]) > 1.8326:
             msg = "Joint 5 has to be between -1.8326 and 1.8326"
-            self._logger.warn(msg)
             return False, msg
-        elif abs(joint_targets[5]) > 2.87:
+        elif abs(joint_target[5]) > 2.87:
             msg = "Joint 6 has to be between -2.87 and 2.87"
+            return False, msg
+
+    def arm_joint_move(self, joint_target):
+        success, msg = self.validate_joint_target(joint_target)
+        if not success:
             self._logger.warn(msg)
             return False, msg
         try:
@@ -1239,12 +1239,12 @@ def arm_joint_move(self, joint_targets):
             else:
                 trajectory_point = (
                     RobotCommandBuilder.create_arm_joint_trajectory_point(
-                        joint_targets[0],
-                        joint_targets[1],
-                        joint_targets[2],
-                        joint_targets[3],
-                        joint_targets[4],
-                        joint_targets[5],
+                        joint_target[0],
+                        joint_target[1],
+                        joint_target[2],
+                        joint_target[3],
+                        joint_target[4],
+                        joint_target[5],
                     )
                 )
                 arm_joint_trajectory = arm_command_pb2.ArmJointTrajectory(
@@ -1271,6 +1271,56 @@ def arm_joint_move(self, joint_targets):
 
         except Exception as e:
             return False, "Exception occured during arm movement: " + str(e)
+
+    def joint_trajectory(self, joint_targets):
+        for joint_target in joint_targets:
+            success, msg = self.validate_joint_target(joint_target.positions)
+            if not success:
+                self._logger.warn(msg)
+                return False, msg
+        try:
+            success, msg = self.ensure_arm_power_and_stand()
+            if not success:
+                self._logger.info(msg)
+                return False, msg
+            else:
+                points = []
+                for joint_target in joint_targets:
+                    trajectory_point = (
+                        RobotCommandBuilder.create_arm_joint_trajectory_point(
+                            joint_target.positions[0],
+                            joint_target.positions[1],
+                            joint_target.positions[2],
+                            joint_target.positions[3],
+                            joint_target.positions[4],
+                            joint_target.positions[5],
+                        )
+                    )
+                    points.append(trajectory_point)
+                arm_joint_trajectory = arm_command_pb2.ArmJointTrajectory(
+                    points=points
+                )
+                arm_command = self.make_arm_trajectory_command(arm_joint_trajectory)
+
+                # Send the request
+                cmd_id = self._robot_command_client.robot_command(arm_command)
+
+                # Query for feedback to determine how long it will take
+                feedback_resp = self._robot_command_client.robot_command_feedback(
+                    cmd_id
+                )
+                joint_move_feedback = (
+                    feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_joint_move_feedback
+                )
+                time_to_goal: Duration = joint_move_feedback.time_to_goal
+                time_to_goal_in_seconds: float = time_to_goal.seconds + (
+                    float(time_to_goal.nanos) / float(10**9)
+                )
+                time.sleep(time_to_goal_in_seconds)
+                return True, "Spot Arm moved successfully"
+
+        except Exception as e:
+            return False, "Exception occured during arm movement: " + str(e)
 
     def force_trajectory(self, data):
         try:

spot_msgs/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
 
 add_message_files(
   FILES
+  JointTarget.msg
   BatteryStateArray.msg
   BehaviorFault.msg
   EStopStateArray.msg
@@ -40,6 +41,7 @@ add_message_files(
 
 add_service_files(
   FILES
+  ArmJointTrajectory.srv
   ListGraph.srv
   SetLocomotion.srv
   SetSwingHeight.srv

spot_msgs/msg/JointTarget.msg

@@ -0,0 +1 @@
+float64[6] positions

spot_msgs/srv/ArmJointTrajectory.srv

@@ -0,0 +1,4 @@
+JointTarget[] joint_targets
+---
+bool success
+string message