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)
