From 42a680058b769a30c0391a950f34aa9f02ca616a Mon Sep 17 00:00:00 2001 From: Nikhil Sridhar Date: Thu, 6 Apr 2023 02:16:41 -0400 Subject: [PATCH] support joint trajectories --- docs/arm_usage.rst | 2 + docs/html/_sources/arm_usage.rst.txt | 2 + docs/html/arm_usage.html | 19 +++-- spot_driver/src/spot_driver/spot_ros.py | 11 +++ spot_driver/src/spot_driver/spot_wrapper.py | 88 ++++++++++++++++----- spot_msgs/CMakeLists.txt | 2 + spot_msgs/msg/JointTarget.msg | 1 + spot_msgs/srv/ArmJointTrajectory.srv | 4 + 8 files changed, 102 insertions(+), 27 deletions(-) create mode 100644 spot_msgs/msg/JointTarget.msg create mode 100644 spot_msgs/srv/ArmJointTrajectory.srv diff --git a/docs/arm_usage.rst b/docs/arm_usage.rst index c5e0a0b1..05c3cb64 100644 --- a/docs/arm_usage.rst +++ b/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 | | diff --git a/docs/html/_sources/arm_usage.rst.txt b/docs/html/_sources/arm_usage.rst.txt index c5e0a0b1..05c3cb64 100644 --- a/docs/html/_sources/arm_usage.rst.txt +++ b/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 | | diff --git a/docs/html/arm_usage.html b/docs/html/arm_usage.html index a1f57b80..935ce9ef 100644 --- a/docs/html/arm_usage.html +++ b/docs/html/arm_usage.html @@ -91,28 +91,31 @@

Arm control/spot/arm_joint_move   -/spot/arm_stow +/spot/joint_trajectory   -/spot/arm_unstow +/spot/arm_stow   -/spot/force_trajectory +/spot/arm_unstow   -/spot/grasp_3d +/spot/force_trajectory   -/spot/gripper_angle_open +/spot/grasp_3d   -/spot/gripper_close +/spot/gripper_angle_open   -/spot/gripper_open +/spot/gripper_close   -/spot/gripper_pose +/spot/gripper_open +  + +/spot/gripper_pose   diff --git a/spot_driver/src/spot_driver/spot_ros.py b/spot_driver/src/spot_driver/spot_ros.py index 1bf37443..7aedb653 100644 --- a/spot_driver/src/spot_driver/spot_ros.py +++ b/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 ) diff --git a/spot_driver/src/spot_driver/spot_wrapper.py b/spot_driver/src/spot_driver/spot_wrapper.py index 329fd030..415cf464 100644 --- a/spot_driver/src/spot_driver/spot_wrapper.py +++ b/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: diff --git a/spot_msgs/CMakeLists.txt b/spot_msgs/CMakeLists.txt index 9a17c51b..eb4eb7ff 100644 --- a/spot_msgs/CMakeLists.txt +++ b/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 diff --git a/spot_msgs/msg/JointTarget.msg b/spot_msgs/msg/JointTarget.msg new file mode 100644 index 00000000..6beca5ea --- /dev/null +++ b/spot_msgs/msg/JointTarget.msg @@ -0,0 +1 @@ +float64[6] positions \ No newline at end of file diff --git a/spot_msgs/srv/ArmJointTrajectory.srv b/spot_msgs/srv/ArmJointTrajectory.srv new file mode 100644 index 00000000..ecb59d64 --- /dev/null +++ b/spot_msgs/srv/ArmJointTrajectory.srv @@ -0,0 +1,4 @@ +JointTarget[] joint_targets +--- +bool success +string message \ No newline at end of file