def set_default_body_control(): """Set default body control params to current body position""" footprint_R_body = geometry.EulerZXY() position = geo.Vec3(x=0.0, y=0.0, z=0.0) rotation = footprint_R_body.to_quaternion() pose = geo.SE3Pose(position=position, rotation=rotation) point = trajectory_pb2.SE3TrajectoryPoint(pose=pose) traj = trajectory_pb2.SE3Trajectory(points=[point]) return spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj)
def bodyPoseCallback(self, data): """Callback for cmd_vel command""" q = Quaternion() q.x = data.orientation.x q.y = data.orientation.y q.z = data.orientation.z q.w = data.orientation.w position = geometry_pb2.Vec3(z=data.position.z) pose = geometry_pb2.SE3Pose(position=position, rotation=q) point = trajectory_pb2.SE3TrajectoryPoint(pose=pose) traj = trajectory_pb2.SE3Trajectory(points=[point]) body_control = spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj) mobility_params = self.spot_wrapper.get_mobility_params() mobility_params.body_control.CopyFrom(body_control) self.spot_wrapper.set_mobility_params(mobility_params)
def move_along_trajectory(frame, velocity, se3_poses): """ Builds an ArmSE3PoseCommand the arm to a point at a specific speed. Builds a trajectory from the current location to a new location velocity is in m/s """ last_pose = None last_t = 0 points = [] # Create a trajectory from the points for pose in se3_poses: if last_pose is None: time_in_sec = 0 seconds = int(0) nanos = int(0) else: # Compute the distance from the current hand position to the new hand position dist = math.sqrt((last_pose.x - pose.x)**2 + (last_pose.y - pose.y)**2 + (last_pose.z - pose.z)**2) time_in_sec = dist / velocity seconds = int(time_in_sec + last_t) nanos = int((time_in_sec + last_t - seconds) * 1e9) position = geometry_pb2.Vec3(x=pose.x, y=pose.y, z=pose.z) rotation = geometry_pb2.Quaternion(w=pose.rot.w, x=pose.rot.x, y=pose.rot.y, z=pose.rot.z) this_se3_pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) points.append( trajectory_pb2.SE3TrajectoryPoint( pose=this_se3_pose, time_since_reference=duration_pb2.Duration(seconds=seconds, nanos=nanos))) last_pose = pose last_t = time_in_sec + last_t hand_trajectory = trajectory_pb2.SE3Trajectory(points=points) return hand_trajectory
def mobility_params(body_height=0.0, footprint_R_body=geometry.EulerZXY(), locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=False, external_force_params=None): """Helper to create Mobility params for spot mobility commands. This function is designed to help get started issuing commands, but lots of options are not exposed via this interface. See spot.robot_command_pb2 for more details. If unset, good defaults will be chosen by the robot. Returns: spot.MobilityParams, params for spot mobility commands. """ # Simplified body control params position = geometry_pb2.Vec3(z=body_height) rotation = footprint_R_body.to_quaternion() pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) point = trajectory_pb2.SE3TrajectoryPoint(pose=pose) frame = geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_BODY) traj = trajectory_pb2.SE3Trajectory(points=[point], frame=frame) body_control = spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj) return spot_command_pb2.MobilityParams(body_control=body_control, locomotion_hint=locomotion_hint, stair_hint=stair_hint, external_force_params=external_force_params)
def mobility_params(body_height=0.0, footprint_R_body=geometry.EulerZXY(), locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=False, external_force_params=None): """Helper to create Mobility params for spot mobility commands. This function is designed to help get started issuing commands, but lots of options are not exposed via this interface. See spot.robot_command_pb2 for more details. If unset, good defaults will be chosen by the robot. Args: body_height: Body height in meters. footprint_R_body(EulerZXY): The orientation of the body frame with respect to the footprint frame (gravity aligned framed with yaw computed from the stance feet) locomotion_hint: Locomotion hint to use for the command. stair_hint: Boolean to specify if stair mode should be used. external_force_params(spot.BodyExternalForceParams): Robot body external force parameters. Returns: spot.MobilityParams, params for spot mobility commands. """ # Simplified body control params position = geometry_pb2.Vec3(z=body_height) rotation = footprint_R_body.to_quaternion() pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) point = trajectory_pb2.SE3TrajectoryPoint(pose=pose) traj = trajectory_pb2.SE3Trajectory(points=[point]) body_control = spot_command_pb2.BodyControlParams( base_offset_rt_footprint=traj) return spot_command_pb2.MobilityParams( body_control=body_control, locomotion_hint=locomotion_hint, stair_hint=stair_hint, external_force_params=external_force_params)
def arm_surface_contact(config): # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('ArmSurfaceContactExample') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." arm_surface_contact_client = robot.ensure_client( ArmSurfaceContactClient.default_service_name) # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper 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 = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(2.0) # Unstow the arm unstow = RobotCommandBuilder.arm_ready_command() # Issue the command via the RobotCommandClient command_client.robot_command(unstow) robot.logger.info("Unstow command issued.") time.sleep(3.0) # ---------- # # Now we'll use the arm_surface_contact service to do an accurate position move with # some amount of force. # # Position of the hand: hand_x = 0.75 # in front of the robot. hand_y_start = 0 # centered hand_y_end = -0.5 # to the right hand_z = 0 # will be ignored since we'll have a force in the Z axis. f_z = -0.05 # percentage of maximum press force, negative to press down # be careful setting this too large, you can knock the robot over percentage_press = geometry_pb2.Vec3(x=0, y=0, z=f_z) hand_vec3_start_rt_body = geometry_pb2.Vec3(x=hand_x, y=hand_y_start, z=hand_z) hand_vec3_end_rt_body = geometry_pb2.Vec3(x=hand_x, y=hand_y_end, z=hand_z) # We want to point the hand straight down the entire time. qw = 0.707 qx = 0 qy = 0.707 qz = 0 body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) # Build a position trajectory body_T_hand1 = geometry_pb2.SE3Pose( position=hand_vec3_start_rt_body, rotation=body_Q_hand) body_T_hand2 = geometry_pb2.SE3Pose(position=hand_vec3_end_rt_body, rotation=body_Q_hand) robot_state = robot_state_client.get_robot_state() odom_T_flat_body = get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) odom_T_hand1 = odom_T_flat_body * math_helpers.SE3Pose.from_obj( body_T_hand1) odom_T_hand2 = odom_T_flat_body * math_helpers.SE3Pose.from_obj( body_T_hand2) # Trajectory length traj_time = 5.0 # in seconds time_since_reference = seconds_to_duration(traj_time) traj_point1 = trajectory_pb2.SE3TrajectoryPoint( pose=odom_T_hand1.to_proto(), time_since_reference=seconds_to_duration(0)) traj_point2 = trajectory_pb2.SE3TrajectoryPoint( pose=odom_T_hand2.to_proto(), time_since_reference=time_since_reference) hand_traj = trajectory_pb2.SE3Trajectory( points=[traj_point1, traj_point2]) # Open the gripper gripper_cmd_packed = RobotCommandBuilder.claw_gripper_open_fraction_command( 0) gripper_command = gripper_cmd_packed.synchronized_command.gripper_command.claw_gripper_command cmd = arm_surface_contact_pb2.ArmSurfaceContact.Request( pose_trajectory_in_task=hand_traj, root_frame_name=ODOM_FRAME_NAME, press_force_percentage=percentage_press, x_axis=arm_surface_contact_pb2.ArmSurfaceContact.Request. AXIS_MODE_POSITION, y_axis=arm_surface_contact_pb2.ArmSurfaceContact.Request. AXIS_MODE_POSITION, z_axis=arm_surface_contact_pb2.ArmSurfaceContact.Request. AXIS_MODE_FORCE, z_admittance=arm_surface_contact_pb2.ArmSurfaceContact.Request. ADMITTANCE_SETTING_LOOSE, # Enable the cross term so that if the arm gets stuck in a rut, it will retract # upwards slightly, preventing excessive lateral forces. xy_to_z_cross_term_admittance=arm_surface_contact_pb2. ArmSurfaceContact.Request.ADMITTANCE_SETTING_VERY_STIFF, gripper_command=gripper_command) # Enable walking cmd.is_robot_following_hand = True # A bias force (in this case, leaning forward) can help improve stability. bias_force_x = -25 cmd.bias_force_ewrt_body.CopyFrom( geometry_pb2.Vec3(x=bias_force_x, y=0, z=0)) proto = arm_surface_contact_service_pb2.ArmSurfaceContactCommand( request=cmd) # Send the request robot.logger.info('Running arm surface contact...') arm_surface_contact_client.arm_surface_contact_command(proto) time.sleep(traj_time + 5.0) robot.logger.info('Turning off...') # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def force_wrench(config): """Commanding a force / wrench with Spot's arm.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('ForceWrenchClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper 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 = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") # Send a second stand command with a lowered body height to allow the arm to reach the ground. stand_command = RobotCommandBuilder.synchro_stand_command( body_height=-0.15) command_id = command_client.robot_command(stand_command, timeout=2) time.sleep(2.0) # Unstow the arm unstow = RobotCommandBuilder.arm_ready_command() # Issue the command via the RobotCommandClient command_client.robot_command(unstow) robot.logger.info("Unstow command issued.") time.sleep(3.0) # Start in gravity-compensation mode (but zero force) f_x = 0 # Newtons f_y = 0 f_z = 0 # We won't have any rotational torques torque_x = 0 torque_y = 0 torque_z = 0 # Duration in seconds. seconds = 1000 # Use the helper function to build a single wrench command = RobotCommandBuilder.arm_wrench_command( f_x, f_y, f_z, torque_x, torque_y, torque_z, BODY_FRAME_NAME, seconds) # Send the request command_client.robot_command(command) robot.logger.info('Zero force commanded...') time.sleep(5.0) # Drive the arm into the ground with a specified force: f_z = -5 # Newtons command = RobotCommandBuilder.arm_wrench_command( f_x, f_y, f_z, torque_x, torque_y, torque_z, BODY_FRAME_NAME, seconds) # Send the request command_client.robot_command(command) time.sleep(5.0) # --------------- # # Hybrid position-force mode and trajectories. # # We'll move the arm in an X/Y trajectory while maintaining some force on the ground. # Hand will start to the left and move to the right. hand_x = 0.75 # in front of the robot. hand_y_start = 0.25 # to the left hand_y_end = -0.25 # to the right hand_z = 0 # will be ignored since we'll have a force in the Z axis. f_z = -5 # Newtons hand_vec3_start = geometry_pb2.Vec3(x=hand_x, y=hand_y_start, z=hand_z) hand_vec3_end = geometry_pb2.Vec3(x=hand_x, y=hand_y_end, z=hand_z) qw = 1 qx = 0 qy = 0 qz = 0 quat = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) # Build a position trajectory hand_pose1_in_flat_body = geometry_pb2.SE3Pose( position=hand_vec3_start, rotation=quat) hand_pose2_in_flat_body = geometry_pb2.SE3Pose( position=hand_vec3_end, rotation=quat) # Convert the poses to the odometry frame. robot_state = robot_state_client.get_robot_state() odom_T_flat_body = get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) hand_pose1 = odom_T_flat_body * math_helpers.SE3Pose.from_obj( hand_pose1_in_flat_body) hand_pose2 = odom_T_flat_body * math_helpers.SE3Pose.from_obj( hand_pose2_in_flat_body) traj_time = 5.0 time_since_reference = seconds_to_duration(traj_time) traj_point1 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose1.to_proto(), time_since_reference=seconds_to_duration(0)) traj_point2 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose2.to_proto(), time_since_reference=time_since_reference) hand_traj = trajectory_pb2.SE3Trajectory( points=[traj_point1, traj_point2]) # We'll use a constant wrench here. Build a wrench trajectory with a single point. # Note that we only need to fill out Z-force because that is the only axis that will # be in force mode. force_tuple = odom_T_flat_body.rotation.transform_point(x=0, y=0, z=f_z) force = geometry_pb2.Vec3(x=force_tuple[0], y=force_tuple[1], z=force_tuple[2]) torque = geometry_pb2.Vec3(x=0, y=0, z=0) wrench = geometry_pb2.Wrench(force=force, torque=torque) # We set this point to happen at 0 seconds. The robot will hold the wrench past that # time, so we'll end up with a constant value. duration = seconds_to_duration(0) traj_point = trajectory_pb2.WrenchTrajectoryPoint( wrench=wrench, time_since_reference=duration) trajectory = trajectory_pb2.WrenchTrajectory(points=[traj_point]) arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( pose_trajectory_in_task=hand_traj, root_frame_name=ODOM_FRAME_NAME, wrench_trajectory_in_task=trajectory, x_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_POSITION, y_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_POSITION, z_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_FORCE, rx_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_POSITION, ry_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_POSITION, rz_axis=arm_command_pb2.ArmCartesianCommand.Request. AXIS_MODE_POSITION) arm_command = arm_command_pb2.ArmCommand.Request( arm_cartesian_command=arm_cartesian_command) synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( arm_command=arm_command) robot_command = robot_command_pb2.RobotCommand( synchronized_command=synchronized_command) # Send the request command_client.robot_command(robot_command) robot.logger.info('Running mixed position and force mode.') time.sleep(traj_time + 5.0) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def gaze_control(config): """Commanding a gaze with Spot's arm.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('GazeDemoClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper 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 = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(2.0) # Unstow the arm unstow = RobotCommandBuilder.arm_ready_command() # Issue the command via the RobotCommandClient command_client.robot_command(unstow) robot.logger.info("Unstow command issued.") time.sleep(3.0) # Convert the location from the moving base frame to the world frame. robot_state = robot_state_client.get_robot_state() odom_T_flat_body = get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) # Look at a point 3 meters in front and 4 meters to the left. # We are not specifying a hand location, the robot will pick one. gaze_target_in_odom = odom_T_flat_body.transform_point(x=3.0, y=4.0, z=0) gaze_command = RobotCommandBuilder.arm_gaze_command( gaze_target_in_odom[0], gaze_target_in_odom[1], gaze_target_in_odom[2], ODOM_FRAME_NAME) # Make the open gripper RobotCommand gripper_command = RobotCommandBuilder.claw_gripper_open_command() # Combine the arm and gripper commands into one RobotCommand synchro_command = RobotCommandBuilder.build_synchro_command( gripper_command, gaze_command) # Send the request robot.logger.info("Requesting gaze.") command_client.robot_command(synchro_command) time.sleep(4.0) # Look to the left and the right with the hand. # Robot's frame is X+ forward, Z+ up, so left and right is +/- in Y. x = 4.0 # look 4 meters ahead start_y = 2.0 end_y = -2.0 z = 0 # Look ahead, not up or down traj_time = 5.5 # take 5.5 seconds to look from left to right. start_pos_in_odom_tuple = odom_T_flat_body.transform_point( x=x, y=start_y, z=z) start_pos_in_odom = geometry_pb2.Vec3(x=start_pos_in_odom_tuple[0], y=start_pos_in_odom_tuple[1], z=start_pos_in_odom_tuple[2]) end_pos_in_odom_tuple = odom_T_flat_body.transform_point(x=x, y=end_y, z=z) end_pos_in_odom = geometry_pb2.Vec3(x=end_pos_in_odom_tuple[0], y=end_pos_in_odom_tuple[1], z=end_pos_in_odom_tuple[2]) # Create the trajectory points point1 = trajectory_pb2.Vec3TrajectoryPoint( point=start_pos_in_odom) duration_seconds = int(traj_time) duration_nanos = int((traj_time - duration_seconds) * 1e9) point2 = trajectory_pb2.Vec3TrajectoryPoint( point=end_pos_in_odom, time_since_reference=duration_pb2.Duration( seconds=duration_seconds, nanos=duration_nanos)) # Build the trajectory proto traj_proto = trajectory_pb2.Vec3Trajectory(points=[point1, point2]) # Build the proto gaze_cmd = arm_command_pb2.GazeCommand.Request( target_trajectory_in_frame1=traj_proto, frame1_name=ODOM_FRAME_NAME, frame2_name=ODOM_FRAME_NAME) arm_command = arm_command_pb2.ArmCommand.Request( arm_gaze_command=gaze_cmd) synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( arm_command=arm_command) command = robot_command_pb2.RobotCommand( synchronized_command=synchronized_command) # Make the open gripper RobotCommand gripper_command = RobotCommandBuilder.claw_gripper_open_command() # Combine the arm and gripper commands into one RobotCommand synchro_command = RobotCommandBuilder.build_synchro_command( gripper_command, command) # Send the request gaze_cmd_id = command_client.robot_command(command) robot.logger.info('Sending gaze trajectory.') # Wait until the robot completes the gaze before issuing the next command. block_until_arm_arrives(command_client, gaze_cmd_id, timeout_sec=traj_time + 3.0) # ------------- # # Now make a gaze trajectory that moves the hand around while maintaining the gaze. # We'll use the same trajectory as before, but add a trajectory for the hand to move to. # Hand will start to the left (while looking left) and move to the right. hand_x = 0.75 # in front of the robot. hand_y = 0 # centered hand_z_start = 0 # body height hand_z_end = 0.25 # above body height hand_vec3_start = geometry_pb2.Vec3(x=hand_x, y=hand_y, z=hand_z_start) hand_vec3_end = geometry_pb2.Vec3(x=hand_x, y=hand_y, z=hand_z_end) # We specify an orientation for the hand, which the robot will use its remaining degree # of freedom to achieve. Most of it will be ignored in favor of the gaze direction. qw = 1 qx = 0 qy = 0 qz = 0 quat = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) # Build a trajectory hand_pose1_in_flat_body = geometry_pb2.SE3Pose( position=hand_vec3_start, rotation=quat) hand_pose2_in_flat_body = geometry_pb2.SE3Pose( position=hand_vec3_end, rotation=quat) hand_pose1_in_odom = odom_T_flat_body * math_helpers.SE3Pose.from_obj( hand_pose1_in_flat_body) hand_pose2_in_odom = odom_T_flat_body * math_helpers.SE3Pose.from_obj( hand_pose2_in_flat_body) traj_point1 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose1_in_odom.to_proto()) # We'll make this trajectory the same length as the one above. traj_point2 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose2_in_odom.to_proto(), time_since_reference=duration_pb2.Duration( seconds=duration_seconds, nanos=duration_nanos)) hand_traj = trajectory_pb2.SE3Trajectory( points=[traj_point1, traj_point2]) # Build the proto gaze_cmd = arm_command_pb2.GazeCommand.Request( target_trajectory_in_frame1=traj_proto, frame1_name=ODOM_FRAME_NAME, tool_trajectory_in_frame2=hand_traj, frame2_name=ODOM_FRAME_NAME) arm_command = arm_command_pb2.ArmCommand.Request( arm_gaze_command=gaze_cmd) synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( arm_command=arm_command) command = robot_command_pb2.RobotCommand( synchronized_command=synchronized_command) # Make the open gripper RobotCommand gripper_command = RobotCommandBuilder.claw_gripper_open_command() # Combine the arm and gripper commands into one RobotCommand synchro_command = RobotCommandBuilder.build_synchro_command( gripper_command, command) # Send the request gaze_cmd_id = command_client.robot_command(synchro_command) robot.logger.info('Sending gaze trajectory with hand movement.') # Wait until the robot completes the gaze before powering off. block_until_arm_arrives(command_client, gaze_cmd_id, timeout_sec=traj_time + 3.0) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def arm_trajectory(config): # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('ArmTrajectory') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. # The set of valid commands for a robot depends on hardware configuration. See # SpotCommandHelper 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 = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(2.0) # Move the arm along a simple trajectory vo_T_flat_body = get_a_tform_b( robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot, VISION_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) x = 0.75 # a reasonable position in front of the robot y1 = 0 # centered y2 = 0.4 # 0.4 meters to the robot's left y3 = -0.4 # 0.4 meters to the robot's right z = 0 # at the body's height # Use the same rotation as the robot's body. rotation = math_helpers.Quat() t_first_point = 0 # first point starts at t = 0 for the trajectory. t_second_point = 4.0 # trajectory will last 1.0 seconds t_third_point = 8.0 # trajectory will last 1.0 seconds # Build the two points in the trajectory hand_pose1 = math_helpers.SE3Pose(x=x, y=y1, z=z, rot=rotation) hand_pose2 = math_helpers.SE3Pose(x=x, y=y2, z=z, rot=rotation) hand_pose3 = math_helpers.SE3Pose(x=x, y=y3, z=z, rot=rotation) # Build the points by combining the pose and times into protos. traj_point1 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose1.to_proto(), time_since_reference=seconds_to_duration(t_first_point)) traj_point2 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose2.to_proto(), time_since_reference=seconds_to_duration(t_second_point)) traj_point3 = trajectory_pb2.SE3TrajectoryPoint( pose=hand_pose3.to_proto(), time_since_reference=seconds_to_duration(t_third_point)) # Build the trajectory proto by combining the two points hand_traj = trajectory_pb2.SE3Trajectory( points=[traj_point1, traj_point2, traj_point3]) # Build the command by taking the trajectory and specifying the frame it is expressed # in. # # In this case, we want to specify the trajectory in the body's frame, so we set the # root frame name to the the flat body frame. arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( pose_trajectory_in_task=hand_traj, root_frame_name=GRAV_ALIGNED_BODY_FRAME_NAME) # Pack everything up in protos. arm_command = arm_command_pb2.ArmCommand.Request( arm_cartesian_command=arm_cartesian_command) synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( arm_command=arm_command) robot_command = robot_command_pb2.RobotCommand( synchronized_command=synchronized_command) # Keep the gripper closed the whole time. robot_command = RobotCommandBuilder.claw_gripper_open_fraction_command( 0, build_on_command=robot_command) robot.logger.info("Sending trajectory command...") # Send the trajectory to the robot. cmd_id = command_client.robot_command(robot_command) # Wait until the arm arrives at the goal. while True: feedback_resp = command_client.robot_command_feedback(cmd_id) robot.logger.info( 'Distance to final point: ' + '{:.2f} meters'.format( feedback_resp.feedback.synchronized_feedback. arm_command_feedback.arm_cartesian_feedback. measured_pos_distance_to_goal) + ', {:.2f} radians'.format( feedback_resp.feedback.synchronized_feedback. arm_command_feedback.arm_cartesian_feedback. measured_rot_distance_to_goal)) if feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.status == arm_command_pb2.ArmCartesianCommand.Feedback.STATUS_TRAJECTORY_COMPLETE: robot.logger.info('Move complete.') break time.sleep(0.1) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)