def update_frame(tf, position_change, rotation_change): return geom.SE3Pose( position=geom.Vec3( x=tf.position.x + position_change[0], y=tf.position.y + position_change[1], z=tf.position.z + position_change[2]), rotation=geom.Quaternion( x=tf.rotation.x + rotation_change[0], y=tf.rotation.y + rotation_change[1], z=tf.rotation.z + rotation_change[2], w=tf.rotation.w + rotation_change[3]))
def create_drawable_sphere_object(): """Create a drawable sphere world object that can be added through a mutation request.""" # Add an edge where the transform vision_tform_object is only a position transform with no rotation. vision_tform_drawable = geom.SE3Pose(position=geom.Vec3(x=2, y=3, z=2), rotation=geom.Quaternion(x=0, y=0, z=0, w=1)) # Create a map between the child frame name and the parent frame name/SE3Pose parent_tform_child edges = {} # Create an edge in the frame tree snapshot that includes vision_tform_drawable drawable_frame_name = "drawing_sphere" edges = add_edge_to_tree(edges, vision_tform_drawable, VISION_FRAME_NAME, drawable_frame_name) snapshot = geom.FrameTreeSnapshot(child_to_parent_edge_map=edges) # Set the acquisition time for the sphere using a function to get google.protobuf.Timestamp of the current system time. time_now = now_timestamp() # Create the sphere drawable object sphere = world_object_pb2.DrawableSphere(radius=1) red_color = world_object_pb2.DrawableProperties.Color(r=255, b=0, g=0, a=1) sphere_drawable_prop = world_object_pb2.DrawableProperties( color=red_color, label="red sphere", wireframe=False, sphere=sphere, frame_name_drawable=drawable_frame_name) # Create the complete world object with transform information, a unique name, and the drawable sphere properties. world_object_sphere = world_object_pb2.WorldObject(id=16, name="red_sphere_ball", transforms_snapshot=snapshot, acquisition_time=time_now, drawable_properties=[sphere_drawable_prop]) return world_object_sphere
def test_create_se3_pose(): # Test creating an SE3Pose from a proto with from_obj() proto_se3 = geometry_pb2.SE3Pose(position=geometry_pb2.Vec3(x=1, y=2, z=3), rotation=geometry_pb2.Quaternion(w=.1, x=.2, y=.2, z=.1)) se3 = SE3Pose.from_obj(proto_se3) assert type(se3) == SE3Pose assert se3.x == proto_se3.position.x assert se3.y == proto_se3.position.y assert se3.z == proto_se3.position.z assert se3.rot.w == proto_se3.rotation.w assert se3.rot.x == proto_se3.rotation.x assert se3.rot.y == proto_se3.rotation.y assert se3.rot.z == proto_se3.rotation.z # Test proto-like attribute access properties pos = se3.position assert type(pos) == geometry_pb2.Vec3 assert pos.x == proto_se3.position.x assert pos.y == proto_se3.position.y assert pos.z == proto_se3.position.z quat = se3.rotation assert type(quat) == Quat assert quat.w == proto_se3.rotation.w assert quat.x == proto_se3.rotation.x assert quat.y == proto_se3.rotation.y assert quat.z == proto_se3.rotation.z # Test going back to a proto message with to_proto() new_proto_se3 = se3.to_proto() assert type(new_proto_se3) == geometry_pb2.SE3Pose assert new_proto_se3.position.x == proto_se3.position.x assert new_proto_se3.position.y == proto_se3.position.y assert new_proto_se3.position.z == proto_se3.position.z assert new_proto_se3.rotation.w == proto_se3.rotation.w assert new_proto_se3.rotation.x == proto_se3.rotation.x assert new_proto_se3.rotation.y == proto_se3.rotation.y assert new_proto_se3.rotation.z == proto_se3.rotation.z # Test mutating an existing proto message to_obj() proto_mut_se3 = geometry_pb2.SE3Pose() se3.to_obj(proto_mut_se3) assert se3.x == proto_mut_se3.position.x assert se3.y == proto_mut_se3.position.y assert se3.z == proto_mut_se3.position.z assert se3.rot.w == proto_mut_se3.rotation.w assert se3.rot.x == proto_mut_se3.rotation.x assert se3.rot.y == proto_mut_se3.rotation.y assert se3.rot.z == proto_mut_se3.rotation.z # Test identity SE3Pose identity = SE3Pose.from_identity() assert identity.x == 0 assert identity.y == 0 assert identity.z == 0 assert identity.rot.w == 1 assert identity.rot.x == 0 assert identity.rot.y == 0 assert identity.rot.z == 0
def to_proto(self): """Converts the math_helpers.SE3Pose into an output of the protobuf geometry_pb2.SE3Pose.""" return geometry_pb2.SE3Pose(position=geometry_pb2.Vec3(x=self.x, y=self.y, z=self.z), rotation=geometry_pb2.Quaternion( w=self.rot.w, x=self.rot.x, y=self.rot.y, z=self.rot.z))
def create_apriltag_object(): """Create an apriltag world object that can be added through a mutation request.""" # Set the acquisition time for the additional april tag in robot time using a function to # get google.protobuf.Timestamp of the current system time. time_now = now_timestamp() # The apriltag id for the object we want to add. tag_id = 308 # Set the frame names used for the two variants of the apriltag (filtered, raw) frame_name_fiducial = "fiducial_" + str(tag_id) frame_name_fiducial_filtered = "filtered_fiducial_" + str(tag_id) # Make the april tag (slightly offset from the first tag detection) as a world object. Create the # different edges necessary to create an expressive tree. The root node will be the world frame. default_a_tform_b = geom.SE3Pose(position=geom.Vec3(x=.2, y=.2, z=.2), rotation=geom.Quaternion(x=.1, y=.1, z=.1, w=.1)) # Create a map between the child frame name and the parent frame name/SE3Pose parent_tform_child edges = {} # Create an edge for the raw fiducial detection in the world. vision_tform_fiducial = update_frame(tf=default_a_tform_b, position_change=(0, 0, -.2), rotation_change=(0, 0, 0, 0)) edges = add_edge_to_tree(edges, vision_tform_fiducial, VISION_FRAME_NAME, frame_name_fiducial) # Create a edge for the filtered version of the fiducial in the world. vision_tform_filtered_fiducial = update_frame(tf=default_a_tform_b, position_change=(0, 0, -.2), rotation_change=(0, 0, 0, 0)) edges = add_edge_to_tree(edges, vision_tform_filtered_fiducial, VISION_FRAME_NAME, frame_name_fiducial_filtered) # Create the transform to express vision_tform_odom vision_tform_odom = update_frame(tf=default_a_tform_b, position_change=(0, 0, -.2), rotation_change=(0, 0, 0, 0)) edges = add_edge_to_tree(edges, vision_tform_odom, VISION_FRAME_NAME, ODOM_FRAME_NAME) # Can also add custom frames into the frame tree snapshot as long as they keep the tree structure, # so the parent_frame must also be in the tree. vision_tform_special_frame = update_frame(tf=default_a_tform_b, position_change=(0, 0, -.2), rotation_change=(0, 0, 0, 0)) edges = add_edge_to_tree(edges, vision_tform_special_frame, VISION_FRAME_NAME, "my_special_frame") snapshot = geom.FrameTreeSnapshot(child_to_parent_edge_map=edges) # Create the specific properties for the apriltag including the frame names for the transforms # describing the apriltag's position. tag_prop = world_object_pb2.AprilTagProperties( tag_id=tag_id, dimensions=geom.Vec2(x=.2, y=.2), frame_name_fiducial=frame_name_fiducial, frame_name_fiducial_filtered=frame_name_fiducial_filtered) #Create the complete world object with transform information and the apriltag properties. wo_obj_to_add = world_object_pb2.WorldObject(id=21, transforms_snapshot=snapshot, acquisition_time=time_now, apriltag_properties=tag_prop) return wo_obj_to_add
def annotate_spot(config): """A simple example of using the Boston Dynamics API to communicate log annotations to spot.""" bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('AnnotateSpotClient') sdk.load_app_token(config.app_token) robot = sdk.create_robot(config.hostname) # Authenticate robot before being able to use it robot.authenticate(config.username, config.password) # Establish time sync with the robot robot.time_sync.wait_for_sync() # Create a log annotation client log_annotation_client = robot.ensure_client( LogAnnotationClient.default_service_name) # Log a text message. text_message = "This is a text message." txt_msg_proto = log_annotation_protos.LogAnnotationTextMessage( message=text_message, timestamp=None) log_annotation_client.add_text_messages([txt_msg_proto]) robot.logger.info('Added comment "%s" to robot log.', text_message) time.sleep(0.1) # Log an operator comment. op_comment = "This is an operator comment." log_annotation_client.add_operator_comment(op_comment) robot.logger.info('Added comment "%s" to robot log.', op_comment) time.sleep(0.1) # Log two blobs of data (serialized protobuf messages) geo_proto = geometry_protos.Quaternion(x=.91, y=.5, z=.9, w=.2) log_annotation_client.add_log_protobuf(geo_proto) robot.logger.info('Added geometry log blob to robot log.') time.sleep(0.1) cmd_proto = robot_command_protos.SelfRightCommand() log_annotation_client.add_log_protobuf(cmd_proto) robot.logger.info('Added robot command log blob to robot log.') time.sleep(0.1)
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 log_spot_data(config): """A simple example of using the Boston Dynamics API to send data to spot to be logged.""" bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('AnnotateSpotClient') robot = sdk.create_robot(config.hostname) # Authenticate robot before being able to use it robot.authenticate(config.username, config.password) # Establish time sync with the robot robot.time_sync.wait_for_sync() # Create a log annotation client data_buffer_client = robot.ensure_client( DataBufferClient.default_service_name) # Log a text message. text_message = "This is a text message." txt_msg_proto = data_buffer_protos.TextMessage(message=text_message, timestamp=None) data_buffer_client.add_text_messages([txt_msg_proto]) robot.logger.info('Added comment "%s" to robot log.', text_message) time.sleep(0.1) # Log an operator comment. op_comment = "This is an operator comment." data_buffer_client.add_operator_comment(op_comment) robot.logger.info('Added comment "%s" to robot log.', op_comment) time.sleep(0.1) data_buffer_client.add_operator_comment(op_comment) robot.logger.info('Added comment "%s" to robot log.', op_comment) time.sleep(0.1) # Log an event. event_time = robot.time_sync.robot_timestamp_from_local_secs(time.time()) event = data_buffer_protos.Event(type='bosdyn:test', description='test event', start_time=event_time, end_time=event_time, level=data_buffer_protos.Event.LEVEL_LOW) data_buffer_client.add_events([event]) robot.logger.info("Added event") time.sleep(0.1) # Log two blobs of data (serialized protobuf messages) geo_proto = geometry_protos.Quaternion(x=.91, y=.5, z=.9, w=.2) data_buffer_client.add_protobuf(geo_proto) robot.logger.info('Added geometry log blob to robot log.') time.sleep(0.1) cmd_proto = basic_command_pb2.SelfRightCommand() data_buffer_client.add_protobuf(cmd_proto) robot.logger.info('Added robot command log blob to robot log.') time.sleep(0.1) # Log two blobs of data of different types to the same 'channel' data_buffer_client.add_protobuf(geo_proto, channel='multi-proto-channel') data_buffer_client.add_protobuf(cmd_proto, channel='mulit-proto-channel') robot.logger.info('Added protos of different types to the same channel.') time.sleep(0.1)
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 run_gcode_program(config): """A simple example of using the Boston Dynamics API to command a Spot robot.""" config_parser = configparser.ConfigParser() config_parser.read_file(open('gcode.cfg')) gcode_file = config_parser.get("General", "gcode_file") scale = config_parser.getfloat("General", "scale") min_dist_to_goal = config_parser.getfloat("General", "min_dist_to_goal") allow_walking = config_parser.getboolean("General", "allow_walking") velocity = config_parser.getfloat("General", "velocity") press_force_percent = config_parser.getfloat("General", "press_force_percent") below_z_is_admittance = config_parser.getfloat("General", "below_z_is_admittance") travel_z = config_parser.getfloat("General", "travel_z") gcode_start_x = config_parser.getfloat("General", "gcode_start_x") gcode_start_y = config_parser.getfloat("General", "gcode_start_y") draw_on_wall = config_parser.getboolean("General", "draw_on_wall") use_vision_frame = config_parser.getboolean("General", "use_vision_frame") use_xy_to_z_cross_term = config_parser.getboolean("General", "use_xy_to_z_cross_term") bias_force_x = config_parser.getfloat("General", "bias_force_x") if config_parser.has_option("General", "walk_to_at_end_rt_gcode_origin_x") and config_parser.has_option( "General", "walk_to_at_end_rt_gcode_origin_y"): walk_to_at_end_rt_gcode_origin_x = config_parser.getfloat( "General", "walk_to_at_end_rt_gcode_origin_x") walk_to_at_end_rt_gcode_origin_y = config_parser.getfloat( "General", "walk_to_at_end_rt_gcode_origin_y") else: walk_to_at_end_rt_gcode_origin_x = None walk_to_at_end_rt_gcode_origin_y = None if velocity <= 0: print('Velocity must be greater than 0. Currently is: ', velocity) return if use_vision_frame: api_send_frame = VISION_FRAME_NAME else: api_send_frame = ODOM_FRAME_NAME # The Boston Dynamics Python library uses Python's logging module to # generate output. Applications using the library can specify how # the logging information should be output. bosdyn.client.util.setup_logging(config.verbose) # The SDK object is the primary entry point to the Boston Dynamics API. # create_standard_sdk will initialize an SDK object with typical default # parameters. The argument passed in is a string identifying the client. sdk = bosdyn.client.create_standard_sdk('GcodeClient') # A Robot object represents a single robot. Clients using the Boston # Dynamics API can manage multiple robots, but this tutorial limits # access to just one. The network address of the robot needs to be # specified to reach it. This can be done with a DNS name # (e.g. spot.intranet.example.com) or an IP literal (e.g. 10.0.63.1) robot = sdk.create_robot(config.hostname) # Clients need to authenticate to a robot before being able to use it. robot.authenticate(config.username, config.password) # Establish time sync with the robot. This kicks off a background thread to establish time sync. # Time sync is required to issue commands to the robot. After starting time sync thread, block # until sync is established. robot.time_sync.wait_for_sync() # Verify the robot has an arm. assert robot.has_arm(), "Robot requires an arm to run the gcode 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." arm_surface_contact_client = robot.ensure_client(ArmSurfaceContactClient.default_service_name) # Only one client at a time can operate a robot. Clients acquire a lease to # indicate that they want to control a robot. Acquiring may fail if another # client is currently controlling the robot. When the client is done # controlling the robot, it should return the lease so other clients can # control it. Note that the lease is returned as the "finally" condition in this # try-catch-finally block. 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.") robot_state_client = robot.ensure_client(RobotStateClient.default_service_name) # Update state robot_state = robot_state_client.get_robot_state() gcode = GcodeReader(gcode_file, scale, robot.logger, below_z_is_admittance, travel_z, draw_on_wall, gcode_start_x, gcode_start_y) # Prep arm # Build a position to move the arm to (in meters, relative to the body frame's origin) x = 0.75 y = 0 if not draw_on_wall: z = -0.35 qw = .707 qx = 0 qy = .707 qz = 0 else: z = -0.25 qw = 1 qx = 0 qy = 0 qz = 0 flat_body_T_hand = math_helpers.SE3Pose(x, y, z, math_helpers.Quat(w=qw, x=qx, y=qy, z=qz)) 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 * flat_body_T_hand robot.logger.info('Moving arm to starting position.') # Send the request odom_T_hand_obj = odom_T_hand.to_proto() move_time = 0.000001 # move as fast as possible because we will use (default) velocity/accel limiting. arm_command = RobotCommandBuilder.arm_pose_command( odom_T_hand_obj.position.x, odom_T_hand_obj.position.y, odom_T_hand_obj.position.z, odom_T_hand_obj.rotation.w, odom_T_hand_obj.rotation.x, odom_T_hand_obj.rotation.y, odom_T_hand_obj.rotation.z, ODOM_FRAME_NAME, move_time) command = RobotCommandBuilder.build_synchro_command(arm_command) cmd_id = command_client.robot_command(command) # Wait for the move to complete block_until_arm_arrives(command_client, cmd_id) # Update state and Get the hand position robot_state = robot_state_client.get_robot_state() (world_T_body, body_T_hand, world_T_hand, odom_T_body) = get_transforms( use_vision_frame, robot_state) world_T_admittance_frame = geometry_pb2.SE3Pose( position=geometry_pb2.Vec3(x=0, y=0, z=0), rotation=geometry_pb2.Quaternion(w=1, x=0, y=0, z=0)) if draw_on_wall: # Create an admittance frame that has Z- along the robot's X axis xhat_ewrt_robot = [0, 0, 1] xhat_ewrt_vo = [0, 0, 0] (xhat_ewrt_vo[0], xhat_ewrt_vo[1], xhat_ewrt_vo[2]) = world_T_body.rot.transform_point( xhat_ewrt_robot[0], xhat_ewrt_robot[1], xhat_ewrt_robot[2]) (z1, z2, z3) = world_T_body.rot.transform_point(-1, 0, 0) zhat_temp = [z1, z2, z3] zhat = make_orthogonal(xhat_ewrt_vo, zhat_temp) yhat = np.cross(zhat, xhat_ewrt_vo) mat = np.array([xhat_ewrt_vo, yhat, zhat]).transpose() q_wall = Quat.from_matrix(mat) zero_vec3 = geometry_pb2.Vec3(x=0, y=0, z=0) q_wall_proto = geometry_pb2.Quaternion(w=q_wall.w, x=q_wall.x, y=q_wall.y, z=q_wall.z) world_T_admittance_frame = geometry_pb2.SE3Pose(position=zero_vec3, rotation=q_wall_proto) # Touch the ground/wall move_arm(robot_state, True, [world_T_hand], arm_surface_contact_client, velocity, allow_walking, world_T_admittance_frame, press_force_percent, api_send_frame, use_xy_to_z_cross_term, bias_force_x) time.sleep(4.0) last_admittance = True # Update state robot_state = robot_state_client.get_robot_state() # Get the hand position (world_T_body, body_T_hand, world_T_hand, odom_T_body) = get_transforms( use_vision_frame, robot_state) odom_T_ground_plane = get_a_tform_b(robot_state.kinematic_state.transforms_snapshot, "odom", "gpe") world_T_odom = world_T_body * odom_T_body.inverse() (gx, gy, gz) = world_T_odom.transform_point(odom_T_ground_plane.x, odom_T_ground_plane.y, odom_T_ground_plane.z) ground_plane_rt_vo = [gx, gy, gz] # Compute the robot's position on the ground plane. #ground_plane_T_robot = odom_T_ground_plane.inverse() * # Compute an origin. if not draw_on_wall: # For on the ground: # xhat = body x # zhat = (0,0,1) # Ensure the origin is gravity aligned, otherwise we get some height drift. zhat = [0.0, 0.0, 1.0] (x1, x2, x3) = world_T_body.rot.transform_point(1.0, 0.0, 0.0) xhat_temp = [x1, x2, x3] xhat = make_orthogonal(zhat, xhat_temp) yhat = np.cross(zhat, xhat) mat = np.array([xhat, yhat, zhat]).transpose() vo_Q_origin = Quat.from_matrix(mat) world_T_origin = SE3Pose(world_T_hand.x, world_T_hand.y, world_T_hand.z, vo_Q_origin) else: # todo should I use the same one? world_T_origin = world_T_hand gcode.set_origin(world_T_origin, world_T_admittance_frame) robot.logger.info('Origin set') (is_admittance, world_T_goals, is_pause) = gcode.get_next_world_T_goals(ground_plane_rt_vo) while is_pause: do_pause() (is_admittance, world_T_goals, is_pause) = gcode.get_next_world_T_goals(ground_plane_rt_vo) if world_T_goals is None: # we're done! done = True move_arm(robot_state, is_admittance, world_T_goals, arm_surface_contact_client, velocity, allow_walking, world_T_admittance_frame, press_force_percent, api_send_frame, use_xy_to_z_cross_term, bias_force_x) odom_T_hand_goal = world_T_odom.inverse() * world_T_goals[-1] last_admittance = is_admittance done = False while not done: # Update state robot_state = robot_state_client.get_robot_state() # Determine if we are at the goal point (world_T_body, body_T_hand, world_T_hand, odom_T_body) = get_transforms( use_vision_frame, robot_state) (gx, gy, gz) = world_T_odom.transform_point(odom_T_ground_plane.x, odom_T_ground_plane.y, odom_T_ground_plane.z) ground_plane_rt_vo = [gx, gy, gz] world_T_odom = world_T_body * odom_T_body.inverse() odom_T_hand = odom_T_body * body_T_hand admittance_frame_T_world = math_helpers.SE3Pose.from_obj( world_T_admittance_frame).inverse() admit_frame_T_hand = admittance_frame_T_world * world_T_odom * odom_T_body * body_T_hand admit_frame_T_hand_goal = admittance_frame_T_world * world_T_odom * odom_T_hand_goal if is_admittance: dist = math.sqrt((admit_frame_T_hand.x - admit_frame_T_hand_goal.x)**2 + (admit_frame_T_hand.y - admit_frame_T_hand_goal.y)**2) #+ (admit_frame_T_hand.z - admit_frame_T_hand_goal.z)**2 ) else: dist = math.sqrt((admit_frame_T_hand.x - admit_frame_T_hand_goal.x)**2 + (admit_frame_T_hand.y - admit_frame_T_hand_goal.y)**2 + (admit_frame_T_hand.z - admit_frame_T_hand_goal.z)**2) arm_near_goal = dist < min_dist_to_goal if arm_near_goal: # Compute where to go. (is_admittance, world_T_goals, is_pause) = gcode.get_next_world_T_goals(ground_plane_rt_vo) while is_pause: do_pause() (is_admittance, world_T_goals, is_pause) = gcode.get_next_world_T_goals(ground_plane_rt_vo) if world_T_goals is None: # we're done! done = True robot.logger.info("Gcode program finished.") break move_arm(robot_state, is_admittance, world_T_goals, arm_surface_contact_client, velocity, allow_walking, world_T_admittance_frame, press_force_percent, api_send_frame, use_xy_to_z_cross_term, bias_force_x) odom_T_hand_goal = world_T_odom.inverse() * world_T_goals[-1] if is_admittance != last_admittance: if is_admittance: print('Waiting for touchdown...') time.sleep(3.0) # pause to wait for touchdown else: time.sleep(1.0) last_admittance = is_admittance elif not is_admittance: # We are in a travel move, so we'll keep updating to account for a changing # ground plane. (is_admittance, world_T_goals, is_pause) = gcode.get_next_world_T_goals( ground_plane_rt_vo, read_new_line=False) # At the end, walk back to the start. robot.logger.info('Done with gcode, going to stand...') blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing") # Compute walking location if walk_to_at_end_rt_gcode_origin_x is not None and walk_to_at_end_rt_gcode_origin_y is not None: robot.logger.info("Walking to end position...") gcode_origin_T_walk = SE3Pose(walk_to_at_end_rt_gcode_origin_x * scale, walk_to_at_end_rt_gcode_origin_y * scale, 0, Quat(1, 0, 0, 0)) odom_T_walk = world_T_odom.inverse() * gcode.world_T_origin * gcode_origin_T_walk odom_T_walk_se2 = SE2Pose.flatten(odom_T_walk) # Command the robot to go to the end point. walk_cmd = RobotCommandBuilder.trajectory_command( goal_x=odom_T_walk_se2.x, goal_y=odom_T_walk_se2.y, goal_heading=odom_T_walk_se2.angle, frame_name="odom") end_time = 15.0 #Issue the command to the robot command_client.robot_command(command=walk_cmd, end_time_secs=time.time() + end_time) time.sleep(end_time) 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 to_proto(self): """Converts the math_helpers.Quat into an output of the protobuf geometry_pb2.Quaternion.""" return geometry_pb2.Quaternion(w=self.w, x=self.x, y=self.y, z=self.z)
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 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 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_with_body_follow(config): """A simple example that moves the arm and asks the body to move to a good position based on the arm's location.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('ArmWithBodyFollowClient') 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 command the body to follow the hand. # Build a position to move the arm to (in meters, relative to the body frame origin.) x = 1.25 y = 0 z = 0.25 hand_pos_rt_body = geometry_pb2.Vec3(x=x, y=y, z=z) # Rotation as a quaternion. qw = 1 qx = 0 qy = 0 qz = 0 body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz) # Build the SE(3) pose of the desired hand position in the moving body frame. body_T_hand = geometry_pb2.SE3Pose(position=hand_pos_rt_body, rotation=body_Q_hand) # Transform the desired from the moving body frame to the odom frame. robot_state = robot_state_client.get_robot_state() odom_T_body = get_a_tform_b( robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) odom_T_hand = odom_T_body * math_helpers.SE3Pose.from_obj( body_T_hand) # duration in seconds seconds = 5 # Create the arm command. 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) # Tell the robot's body to follow the arm follow_arm_command = RobotCommandBuilder.follow_arm_command() # Combine the arm and mobility commands into one synchronized command. command = RobotCommandBuilder.build_synchro_command( follow_arm_command, arm_command) # Send the request command_client.robot_command(command) robot.logger.info('Moving arm to position.') time.sleep(6.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)