diff --git a/spot_driver/src/spot_driver/arm/arm_wrapper.py b/spot_driver/src/spot_driver/arm/arm_wrapper.py index f243415..e53938f 100644 --- a/spot_driver/src/spot_driver/arm/arm_wrapper.py +++ b/spot_driver/src/spot_driver/arm/arm_wrapper.py @@ -1,3 +1,5 @@ +from bosdyn.api import arm_command_pb2, estop_pb2, geometry_pb2, robot_command_pb2, synchronized_command_pb2 +from bosdyn.api.spot import robot_command_pb2 as spot_command_pb2 from bosdyn.client.robot import RobotCommandClient from bosdyn.client.robot_command import RobotCommandBuilder, block_until_arm_arrives import rospy @@ -5,11 +7,20 @@ from std_srvs.srv import Trigger, TriggerResponse from spot_msgs.msg import OpenDoorAction -from spot_msgs.srv import OpenDoor +from spot_msgs.srv import OpenDoor, SetArmImpedanceParams, SetArmImpedanceParamsResponse from vision_msgs.msg import Detection2D +from control_msgs.msg import FollowJointTrajectoryAction +from actionlib import SimpleActionServer from spot_driver.arm.arm_utilities.door_opener import open_door_main from bosdyn.client.manipulation_api_client import ManipulationApiClient +from bosdyn.client.frame_helpers import (BODY_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME, + GROUND_PLANE_FRAME_NAME, HAND_FRAME_NAME, ODOM_FRAME_NAME, get_a_tform_b) +from bosdyn.client.math_helpers import Quat, SE3Pose +from bosdyn.client.robot_state import RobotStateClient +import math +import time +from bosdyn.util import seconds_to_timestamp, seconds_to_duration class ArmWrapper: @@ -52,6 +63,17 @@ def __init__(self, robot, wrapper, logger): self.handle_gripper_close, ) + self.arm_impedance_parameters = rospy.Service( + "arm_impedance_parameters", + SetArmImpedanceParams, + self.handle_arm_impedance_matrix, + ) + self.arm_joint_trajectory_server = SimpleActionServer( + "arm_controller/follow_joint_trajectory", + FollowJointTrajectoryAction, + execute_cb=self.handle_arm_joint_trajectory) + self.arm_joint_trajectory_server.start() + dds = "door_detection_service" self.door_detection_service_proxy = None if rospy.has_param(dds): @@ -100,3 +122,109 @@ def handle_open_door(self, _): return open_door_main( self._robot, self._spot_wrapper, self.door_detection_service_proxy ) + + def handle_arm_impedance_matrix(self, params): + robot_state_client = self._robot.ensure_client(RobotStateClient.default_service_name) + + # Tell the robot to stand up, parameterized to enable the body to adjust its height to + # assist manipulation. For this demo, that means the robot's base will descend, enabling + # the hand to reach the ground. + # The command service is used to issue commands to a robot. + # The set of valid commands for a robot depends on hardware configuration. See + # RobotCommandBuilder for more detailed examples on command building. The robot + # command service requires timesync between the robot and the client. + ## robot.logger.info("Commanding robot to stand...") + command_client = self._robot.ensure_client(RobotCommandClient.default_service_name) + + body_control = spot_command_pb2.BodyControlParams( + body_assist_for_manipulation=spot_command_pb2.BodyControlParams. + BodyAssistForManipulation(enable_hip_height_assist=True, enable_body_yaw_assist=False)) + # Define a stand command that we'll send with the rest of our arm commands so we keep + # adjusting the body for the arm + stand_command = RobotCommandBuilder.synchro_stand_command( + params=spot_command_pb2.MobilityParams(body_control=body_control)) + + # First, let's pick a task frame that is in front of the robot on the ground. + robot_state = robot_state_client.get_robot_state() + odom_T_task = get_a_tform_b(robot_state.kinematic_state.transforms_snapshot, + ODOM_FRAME_NAME, HAND_FRAME_NAME) + + # Now, let's set our tool frame to be the tip of the robot's bottom jaw. Flip the + # orientation so that when the hand is pointed downwards, the tool's z-axis is + # pointed upward. + wr1_T_tool = SE3Pose(0.23589, 0, -0.03943, Quat.from_pitch(-math.pi / 2)) + + # Execute the Cartesian command. + #### cmd_id = command_client.robot_command(robot_cmd) + #### block_until_arm_arrives(command_client, cmd_id, 3.0) + + # First, let's do an impedance command where we set all of our stiffnesses high and + # move around. This will act similar to a position command, but be slightly less stiff. + robot_cmd = robot_command_pb2.RobotCommand() + robot_cmd.CopyFrom(stand_command) # Make sure we keep adjusting the body for the arm + impedance_cmd = robot_cmd.synchronized_command.arm_command.arm_impedance_command + + # Set up our root frame, task frame, and tool frame. + impedance_cmd.root_frame_name = ODOM_FRAME_NAME + impedance_cmd.root_tform_task.CopyFrom(odom_T_task.to_proto()) + impedance_cmd.wrist_tform_tool.CopyFrom(wr1_T_tool.to_proto()) + + # Set up stiffness and damping matrices. Note: if these values are set too high, + # the arm can become unstable. Currently, these are the max stiffness and + # damping values that can be set. + impedance_cmd.diagonal_stiffness_matrix.CopyFrom( + geometry_pb2.Vector(values=[params.linear_stiffness.x, params.linear_stiffness.y, params.linear_stiffness.z, + params.rotational_stiffness.x, params.rotational_stiffness.y, params.rotational_stiffness.z])) + impedance_cmd.diagonal_damping_matrix.CopyFrom( + geometry_pb2.Vector(values=[params.linear_damping.x, params.linear_damping.y, params.linear_damping.z, + params.rotational_damping.x, params.rotational_damping.y, params.rotational_damping.z])) + # Set up our `desired_tool` trajectory. This is where we want the tool to be with respect + # to the task frame. The stiffness we set will drag the tool towards `desired_tool`. + traj = impedance_cmd.task_tform_desired_tool + pt1 = traj.points.add() + pt1.time_since_reference.CopyFrom(seconds_to_duration(2.0)) + pt1.pose.CopyFrom(SE3Pose(0, 0, 0, Quat.from_pitch(-math.pi / 2)).to_proto()) + + # Execute the impedance command. + cmd_id = command_client.robot_command(robot_cmd) + time.sleep(2.0) + return SetArmImpedanceParamsResponse(success=True) + + def handle_arm_joint_trajectory(self, goal): + joint_names = ['arm0.sh0', 'arm0.sh1', 'arm0.el0', 'arm0.el1', 'arm0.wr0', 'arm0.wr1'] + joint_positions = [] + for name in joint_names: + if name in goal.trajectory.joint_names: + joint_positions.append(goal.trajectory.joint_names.index(name)) + else: + msg = "Unsupported joint name {}. It must be {}".format(name, joint_names) + rospy.logerr(msg) + return self.arm_joint_trajectory_server.set_aborted(text=msg) + command_client = self._robot.ensure_client(RobotCommandClient.default_service_name) + # initialize data + start_time = time.time() + ref_time = seconds_to_timestamp(start_time) + times = [] + positions = [] + velocities = [] + # start sending commands + for point in goal.trajectory.points: + print([point.positions[i] for i in joint_positions]) + print([point.velocities[i] for i in joint_positions]) + print(point.time_from_start.to_sec()) + positions.append([point.positions[i] for i in joint_positions]) + velocities.append([point.velocities[i] for i in joint_positions]) + times.append(point.time_from_start.to_sec()) + total_time = point.time_from_start.to_sec() + print(len(times), len(goal.trajectory.points)) + if len(times) >= 10 or len(times) == len(goal.trajectory.points): + robot_cmd = RobotCommandBuilder.arm_joint_move_helper(joint_positions=positions, + joint_velocities=velocities, + times=times, ref_time=ref_time, + max_acc=10000, max_vel=10000) + cmd_id = command_client.robot_command(robot_cmd) + times = [] + positions = [] + velocities = [] + block_until_arm_arrives(command_client, cmd_id, total_time + 3) # 3[sec] is buffer + return self.arm_joint_trajectory_server.set_succeeded() diff --git a/spot_msgs/CMakeLists.txt b/spot_msgs/CMakeLists.txt index 1ca653e..287f6f6 100644 --- a/spot_msgs/CMakeLists.txt +++ b/spot_msgs/CMakeLists.txt @@ -46,6 +46,7 @@ add_service_files( ClearBehaviorFault.srv Dock.srv GetDockState.srv + SetArmImpedanceParams.srv ) add_action_files( diff --git a/spot_msgs/srv/SetArmImpedanceParams.srv b/spot_msgs/srv/SetArmImpedanceParams.srv new file mode 100644 index 0000000..28fa4bc --- /dev/null +++ b/spot_msgs/srv/SetArmImpedanceParams.srv @@ -0,0 +1,6 @@ +geometry_msgs/Vector3 linear_stiffness # Linear stiffness must be less than or equal to 500.000000 N/m +geometry_msgs/Vector3 rotational_stiffness # Rotational stiffness must be less than or equal to 60.000000 Nm/rad +geometry_msgs/Vector3 linear_damping # Linear damping must be less than or equal to 2.500000 Ns/m +geometry_msgs/Vector3 rotational_damping # Rotational damping must be less than or equal to 1.000000 Nms/rad +--- +bool success