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)
