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 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 set_default_body_control(): """Set default body control params to current body position""" footprint_R_body = geometry.EulerZXY() position = geo.Vec3(x=0.0, y=0.0, z=0.0) rotation = footprint_R_body.to_quaternion() pose = geo.SE3Pose(position=position, rotation=rotation) point = trajectory_pb2.SE3TrajectoryPoint(pose=pose) traj = trajectory_pb2.SE3Trajectory(points=[point]) return spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj)
def 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 bodyPoseCallback(self, data): """Callback for cmd_vel command""" q = Quaternion() q.x = data.orientation.x q.y = data.orientation.y q.z = data.orientation.z q.w = data.orientation.w position = geometry_pb2.Vec3(z=data.position.z) pose = geometry_pb2.SE3Pose(position=position, rotation=q) point = trajectory_pb2.SE3TrajectoryPoint(pose=pose) traj = trajectory_pb2.SE3Trajectory(points=[point]) body_control = spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj) mobility_params = self.spot_wrapper.get_mobility_params() mobility_params.body_control.CopyFrom(body_control) self.spot_wrapper.set_mobility_params(mobility_params)
def move_along_trajectory(frame, velocity, se3_poses): """ Builds an ArmSE3PoseCommand the arm to a point at a specific speed. Builds a trajectory from the current location to a new location velocity is in m/s """ last_pose = None last_t = 0 points = [] # Create a trajectory from the points for pose in se3_poses: if last_pose is None: time_in_sec = 0 seconds = int(0) nanos = int(0) else: # Compute the distance from the current hand position to the new hand position dist = math.sqrt((last_pose.x - pose.x)**2 + (last_pose.y - pose.y)**2 + (last_pose.z - pose.z)**2) time_in_sec = dist / velocity seconds = int(time_in_sec + last_t) nanos = int((time_in_sec + last_t - seconds) * 1e9) position = geometry_pb2.Vec3(x=pose.x, y=pose.y, z=pose.z) rotation = geometry_pb2.Quaternion(w=pose.rot.w, x=pose.rot.x, y=pose.rot.y, z=pose.rot.z) this_se3_pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) points.append( trajectory_pb2.SE3TrajectoryPoint( pose=this_se3_pose, time_since_reference=duration_pb2.Duration(seconds=seconds, nanos=nanos))) last_pose = pose last_t = time_in_sec + last_t hand_trajectory = trajectory_pb2.SE3Trajectory(points=points) return hand_trajectory
def mobility_params(body_height=0.0, footprint_R_body=geometry.EulerZXY(), locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=False, external_force_params=None): """Helper to create Mobility params for spot mobility commands. This function is designed to help get started issuing commands, but lots of options are not exposed via this interface. See spot.robot_command_pb2 for more details. If unset, good defaults will be chosen by the robot. Returns: spot.MobilityParams, params for spot mobility commands. """ # Simplified body control params position = geometry_pb2.Vec3(z=body_height) rotation = footprint_R_body.to_quaternion() pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) point = trajectory_pb2.SE3TrajectoryPoint(pose=pose) frame = geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_BODY) traj = trajectory_pb2.SE3Trajectory(points=[point], frame=frame) body_control = spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj) return spot_command_pb2.MobilityParams(body_control=body_control, locomotion_hint=locomotion_hint, stair_hint=stair_hint, external_force_params=external_force_params)
def mobility_params(body_height=0.0, footprint_R_body=geometry.EulerZXY(), locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=False, external_force_params=None): """Helper to create Mobility params for spot mobility commands. This function is designed to help get started issuing commands, but lots of options are not exposed via this interface. See spot.robot_command_pb2 for more details. If unset, good defaults will be chosen by the robot. Args: body_height: Body height in meters. footprint_R_body(EulerZXY): The orientation of the body frame with respect to the footprint frame (gravity aligned framed with yaw computed from the stance feet) locomotion_hint: Locomotion hint to use for the command. stair_hint: Boolean to specify if stair mode should be used. external_force_params(spot.BodyExternalForceParams): Robot body external force parameters. Returns: spot.MobilityParams, params for spot mobility commands. """ # Simplified body control params position = geometry_pb2.Vec3(z=body_height) rotation = footprint_R_body.to_quaternion() pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) point = trajectory_pb2.SE3TrajectoryPoint(pose=pose) traj = trajectory_pb2.SE3Trajectory(points=[point]) body_control = spot_command_pb2.BodyControlParams( base_offset_rt_footprint=traj) return spot_command_pb2.MobilityParams( body_control=body_control, locomotion_hint=locomotion_hint, stair_hint=stair_hint, external_force_params=external_force_params)
def 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 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)