def move_arm(robot_state, is_admittance, world_T_goals, arm_surface_contact_client, velocity, allow_walking, world_T_admittance, press_force_percentage, api_send_frame, use_xy_to_z_cross_term, bias_force_x): traj = move_along_trajectory(api_send_frame, velocity, world_T_goals) press_force = geometry_pb2.Vec3(x=0, y=0, z=press_force_percentage) max_vel = wrappers_pb2.DoubleValue(value=velocity) cmd = arm_surface_contact_pb2.ArmSurfaceContact.Request( pose_trajectory_in_task=traj, root_frame_name=api_send_frame, root_tform_task=world_T_admittance, press_force_percentage=press_force, 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_POSITION, max_linear_velocity=max_vel) if is_admittance: # Add admittance options cmd.z_axis = arm_surface_contact_pb2.ArmSurfaceContact.Request.AXIS_MODE_FORCE cmd.press_force_percentage.z = press_force_percentage #if admittance_frame is not None: # Set the robot to be really stiff in x/y and really sensitive to admittance in z. cmd.xy_admittance = arm_surface_contact_pb2.ArmSurfaceContact.Request.ADMITTANCE_SETTING_OFF cmd.z_admittance = arm_surface_contact_pb2.ArmSurfaceContact.Request.ADMITTANCE_SETTING_LOOSE if use_xy_to_z_cross_term: cmd.xy_to_z_cross_term_admittance = arm_surface_contact_pb2.ArmSurfaceContact.Request.ADMITTANCE_SETTING_VERY_STIFF else: cmd.xy_to_z_cross_term_admittance = arm_surface_contact_pb2.ArmSurfaceContact.Request.ADMITTANCE_SETTING_OFF # Set a bias force cmd.bias_force_ewrt_body.CopyFrom(geometry_pb2.Vec3(x=bias_force_x, y=0, z=0)) else: cmd.bias_force_ewrt_body.CopyFrom(geometry_pb2.Vec3(x=0, y=0, z=0)) gripper_cmd_packed = RobotCommandBuilder.claw_gripper_open_fraction_command(0) cmd.gripper_command.CopyFrom( gripper_cmd_packed.synchronized_command.gripper_command.claw_gripper_command) cmd.is_robot_following_hand = allow_walking # Build the request proto proto = arm_surface_contact_service_pb2.ArmSurfaceContactCommand(request=cmd) # Send the request arm_surface_contact_client.arm_surface_contact_command(proto)
def main(argv): parser = argparse.ArgumentParser() bosdyn.client.util.add_common_arguments(parser) parser.add_argument( '-s', '--ml-service', help='Service name of external machine learning server.', required=True) parser.add_argument('-m', '--model', help='Model name running on the external server.', required=True) parser.add_argument( '-p', '--person-model', help='Person detection model name running on the external server.') parser.add_argument( '-c', '--confidence-dogtoy', help= 'Minimum confidence to return an object for the dogoy (0.0 to 1.0)', default=0.5, type=float) parser.add_argument( '-e', '--confidence-person', help='Minimum confidence for person detection (0.0 to 1.0)', default=0.6, type=float) options = parser.parse_args(argv) cv2.namedWindow("Fetch") cv2.waitKey(500) sdk = bosdyn.client.create_standard_sdk('SpotFetchClient') sdk.register_service_client(NetworkComputeBridgeClient) robot = sdk.create_robot(options.hostname) robot.authenticate(options.username, options.password) # Time sync is necessary so that time-based filter requests can be converted robot.time_sync.wait_for_sync() network_compute_client = robot.ensure_client( NetworkComputeBridgeClient.default_service_name) robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) command_client = robot.ensure_client( RobotCommandClient.default_service_name) lease_client = robot.ensure_client(LeaseClient.default_service_name) manipulation_api_client = robot.ensure_client( ManipulationApiClient.default_service_name) # This script assumes the robot is already standing via the tablet. We'll take over from the # tablet. lease = lease_client.take() lk = bosdyn.client.lease.LeaseKeepAlive(lease_client) # Store the position of the hand at the last toy drop point. vision_tform_hand_at_drop = None while True: holding_toy = False while not holding_toy: # Capture an image and run ML on it. dogtoy, image, vision_tform_dogtoy = get_obj_and_img( network_compute_client, options.ml_service, options.model, options.confidence_dogtoy, kImageSources, 'dogtoy') if dogtoy is None: # Didn't find anything, keep searching. continue # If we have already dropped the toy off, make sure it has moved a sufficient amount before # picking it up again if vision_tform_hand_at_drop is not None and pose_dist( vision_tform_hand_at_drop, vision_tform_dogtoy) < 0.5: print('Found dogtoy, but it hasn\'t moved. Waiting...') time.sleep(1) continue print('Found dogtoy...') # Got a dogtoy. Request pick up. # Stow the arm in case it is deployed stow_cmd = RobotCommandBuilder.arm_stow_command() command_client.robot_command(stow_cmd) # Walk to the object. walk_rt_vision, heading_rt_vision = compute_stand_location_and_yaw( vision_tform_dogtoy, robot_state_client, distance_margin=1.0) move_cmd = RobotCommandBuilder.trajectory_command( goal_x=walk_rt_vision[0], goal_y=walk_rt_vision[1], goal_heading=heading_rt_vision, frame_name=frame_helpers.VISION_FRAME_NAME, params=get_walking_params(0.5, 0.5)) end_time = 5.0 cmd_id = command_client.robot_command(command=move_cmd, end_time_secs=time.time() + end_time) # Wait until the robot reports that it is at the goal. block_for_trajectory_cmd(command_client, cmd_id, timeout_sec=5, verbose=True) # The ML result is a bounding box. Find the center. (center_px_x, center_px_y) = find_center_px(dogtoy.image_properties.coordinates) # Request Pick Up on that pixel. pick_vec = geometry_pb2.Vec2(x=center_px_x, y=center_px_y) grasp = manipulation_api_pb2.PickObjectInImage( pixel_xy=pick_vec, transforms_snapshot_for_camera=image.shot.transforms_snapshot, frame_name_image_sensor=image.shot.frame_name_image_sensor, camera_model=image.source.pinhole) # We can specify where in the gripper we want to grasp. About halfway is generally good for # small objects like this. For a bigger object like a shoe, 0 is better (use the entire # gripper) grasp.grasp_params.grasp_palm_to_fingertip = 0.6 # Tell the grasping system that we want a top-down grasp. # Add a constraint that requests that the x-axis of the gripper is pointing in the # negative-z direction in the vision frame. # The axis on the gripper is the x-axis. axis_on_gripper_ewrt_gripper = geometry_pb2.Vec3(x=1, y=0, z=0) # The axis in the vision frame is the negative z-axis axis_to_align_with_ewrt_vision = geometry_pb2.Vec3(x=0, y=0, z=-1) # Add the vector constraint to our proto. constraint = grasp.grasp_params.allowable_orientation.add() constraint.vector_alignment_with_tolerance.axis_on_gripper_ewrt_gripper.CopyFrom( axis_on_gripper_ewrt_gripper) constraint.vector_alignment_with_tolerance.axis_to_align_with_ewrt_frame.CopyFrom( axis_to_align_with_ewrt_vision) # We'll take anything within about 15 degrees for top-down or horizontal grasps. constraint.vector_alignment_with_tolerance.threshold_radians = 0.25 # Specify the frame we're using. grasp.grasp_params.grasp_params_frame_name = frame_helpers.VISION_FRAME_NAME # Build the proto grasp_request = manipulation_api_pb2.ManipulationApiRequest( pick_object_in_image=grasp) # Send the request print('Sending grasp request...') cmd_response = manipulation_api_client.manipulation_api_command( manipulation_api_request=grasp_request) # Wait for the grasp to finish grasp_done = False failed = False time_start = time.time() while not grasp_done: feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( manipulation_cmd_id=cmd_response.manipulation_cmd_id) # Send a request for feedback response = manipulation_api_client.manipulation_api_feedback_command( manipulation_api_feedback_request=feedback_request) current_state = response.current_state current_time = time.time() - time_start print('Current state ({time:.1f} sec): {state}'.format( time=current_time, state=manipulation_api_pb2.ManipulationFeedbackState.Name( current_state)), end=' \r') sys.stdout.flush() failed_states = [ manipulation_api_pb2.MANIP_STATE_GRASP_FAILED, manipulation_api_pb2. MANIP_STATE_GRASP_PLANNING_NO_SOLUTION, manipulation_api_pb2. MANIP_STATE_GRASP_FAILED_TO_RAYCAST_INTO_MAP, manipulation_api_pb2. MANIP_STATE_GRASP_PLANNING_WAITING_DATA_AT_EDGE ] failed = current_state in failed_states grasp_done = current_state == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED or failed time.sleep(0.1) holding_toy = not failed # Move the arm to a carry position. print('') print('Grasp finished, search for a person...') carry_cmd = RobotCommandBuilder.arm_carry_command() command_client.robot_command(carry_cmd) # Wait for the carry command to finish time.sleep(0.75) person = None while person is None: # Find a person to deliver the toy to person, image, vision_tform_person = get_obj_and_img( network_compute_client, options.ml_service, options.person_model, options.confidence_person, kImageSources, 'person') # We now have found a person to drop the toy off near. drop_position_rt_vision, heading_rt_vision = compute_stand_location_and_yaw( vision_tform_person, robot_state_client, distance_margin=2.0) wait_position_rt_vision, wait_heading_rt_vision = compute_stand_location_and_yaw( vision_tform_person, robot_state_client, distance_margin=3.0) # Tell the robot to go there # Limit the speed so we don't charge at the person. move_cmd = RobotCommandBuilder.trajectory_command( goal_x=drop_position_rt_vision[0], goal_y=drop_position_rt_vision[1], goal_heading=heading_rt_vision, frame_name=frame_helpers.VISION_FRAME_NAME, params=get_walking_params(0.5, 0.5)) end_time = 5.0 cmd_id = command_client.robot_command(command=move_cmd, end_time_secs=time.time() + end_time) # Wait until the robot reports that it is at the goal. block_for_trajectory_cmd(command_client, cmd_id, timeout_sec=5, verbose=True) print('Arrived at goal, dropping object...') # Do an arm-move to gently put the object down. # Build a position to move the arm to (in meters, relative to and expressed in the gravity aligned body frame). x = 0.75 y = 0 z = -0.25 hand_ewrt_flat_body = geometry_pb2.Vec3(x=x, y=y, z=z) # Point the hand straight down with a quaternion. qw = 0.707 qx = 0 qy = 0.707 qz = 0 flat_body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) flat_body_tform_hand = geometry_pb2.SE3Pose( position=hand_ewrt_flat_body, rotation=flat_body_Q_hand) robot_state = robot_state_client.get_robot_state() vision_tform_flat_body = frame_helpers.get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, frame_helpers.VISION_FRAME_NAME, frame_helpers.GRAV_ALIGNED_BODY_FRAME_NAME) vision_tform_hand_at_drop = vision_tform_flat_body * math_helpers.SE3Pose.from_obj( flat_body_tform_hand) # duration in seconds seconds = 1 arm_command = RobotCommandBuilder.arm_pose_command( vision_tform_hand_at_drop.x, vision_tform_hand_at_drop.y, vision_tform_hand_at_drop.z, vision_tform_hand_at_drop.rot.w, vision_tform_hand_at_drop.rot.x, vision_tform_hand_at_drop.rot.y, vision_tform_hand_at_drop.rot.z, frame_helpers.VISION_FRAME_NAME, seconds) # Keep the gripper closed. gripper_command = RobotCommandBuilder.claw_gripper_open_fraction_command( 0.0) # Combine the arm and gripper commands into one RobotCommand command = RobotCommandBuilder.build_synchro_command( gripper_command, arm_command) # Send the request cmd_id = command_client.robot_command(command) # Wait until the arm arrives at the goal. block_until_arm_arrives(command_client, cmd_id) # Open the gripper gripper_command = RobotCommandBuilder.claw_gripper_open_fraction_command( 1.0) command = RobotCommandBuilder.build_synchro_command(gripper_command) cmd_id = command_client.robot_command(command) # Wait for the dogtoy to fall out time.sleep(1.5) # Stow the arm. stow_cmd = RobotCommandBuilder.arm_stow_command() command_client.robot_command(stow_cmd) time.sleep(1) print('Backing up and waiting...') # Back up one meter and wait for the person to throw the object again. move_cmd = RobotCommandBuilder.trajectory_command( goal_x=wait_position_rt_vision[0], goal_y=wait_position_rt_vision[1], goal_heading=wait_heading_rt_vision, frame_name=frame_helpers.VISION_FRAME_NAME, params=get_walking_params(0.5, 0.5)) end_time = 5.0 cmd_id = command_client.robot_command(command=move_cmd, end_time_secs=time.time() + end_time) # Wait until the robot reports that it is at the goal. block_for_trajectory_cmd(command_client, cmd_id, timeout_sec=5, verbose=True) lease_client.return_lease(lease)
def hello_arm(config): """A simple example of using the Boston Dynamics API to command 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('HelloSpotClient') 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 to a spot in front of the robot, and open the gripper. # Make the arm pose RobotCommand # Build a position to move the arm to (in meters, relative to and expressed in the gravity aligned body frame). x = 0.75 y = 0 z = 0.25 hand_ewrt_flat_body = geometry_pb2.Vec3(x=x, y=y, z=z) # Rotation as a quaternion qw = 1 qx = 0 qy = 0 qz = 0 flat_body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) flat_body_T_hand = geometry_pb2.SE3Pose( position=hand_ewrt_flat_body, rotation=flat_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_hand = odom_T_flat_body * math_helpers.SE3Pose.from_obj( flat_body_T_hand) # duration in seconds seconds = 2 arm_command = RobotCommandBuilder.arm_pose_command( odom_T_hand.x, odom_T_hand.y, odom_T_hand.z, odom_T_hand.rot.w, odom_T_hand.rot.x, odom_T_hand.rot.y, odom_T_hand.rot.z, ODOM_FRAME_NAME, seconds) # Make the open gripper RobotCommand gripper_command = RobotCommandBuilder.claw_gripper_open_fraction_command( 1.0) # Combine the arm and gripper commands into one RobotCommand command = RobotCommandBuilder.build_synchro_command( gripper_command, arm_command) # Send the request cmd_id = command_client.robot_command(command) robot.logger.info('Moving arm to position 1.') # Wait until the arm arrives at the goal. block_until_arm_arrives_with_prints(robot, command_client, cmd_id) # Move the arm to a different position hand_ewrt_flat_body.z = 0 flat_body_Q_hand.w = 0.707 flat_body_Q_hand.x = 0.707 flat_body_Q_hand.y = 0 flat_body_Q_hand.z = 0 flat_body_T_hand2 = geometry_pb2.SE3Pose( position=hand_ewrt_flat_body, rotation=flat_body_Q_hand) odom_T_hand = odom_T_flat_body * math_helpers.SE3Pose.from_obj( flat_body_T_hand2) arm_command = RobotCommandBuilder.arm_pose_command( odom_T_hand.x, odom_T_hand.y, odom_T_hand.z, odom_T_hand.rot.w, odom_T_hand.rot.x, odom_T_hand.rot.y, odom_T_hand.rot.z, ODOM_FRAME_NAME, seconds) # Close the gripper gripper_command = RobotCommandBuilder.claw_gripper_open_fraction_command( 0.0) # Build the proto command = RobotCommandBuilder.build_synchro_command( gripper_command, arm_command) # Send the request cmd_id = command_client.robot_command(command) robot.logger.info('Moving arm to position 2.') # Wait until the arm arrives at the goal. # Note: here we use the helper function provided by robot_command. block_until_arm_arrives(command_client, cmd_id) robot.logger.info('Done.') # 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_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 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)