def velocity_command(v_x, v_y, v_rot, params=None, body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_AUTO):
"""Command robot to move along 2D plane. Velocity should be specified in the robot body
frame. Other frames are currently not supported. The arguments body_height and
locomotion_hint are ignored if params argument is passed.
A velocity command requires an end time. End time is not set in this function, but rather
is set externally before call to RobotCommandService.
Returns:
RobotCommand, which can be issued to the robot command service.
"""
if not params:
params = RobotCommandBuilder.mobility_params(body_height=body_height,
locomotion_hint=locomotion_hint)
any_params = RobotCommandBuilder._to_any(params)
linear = geometry_pb2.Vec2(x=v_x, y=v_y)
vel = geometry_pb2.SE2Velocity(linear=linear, angular=v_rot)
slew_rate_limit = geometry_pb2.SE2Velocity(linear=geometry_pb2.Vec2(x=4, y=4), angular=2.0)
frame = geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_BODY)
vel_command = robot_command_pb2.SE2VelocityCommand.Request(velocity=vel, frame=frame,
slew_rate_limit=slew_rate_limit)
mobility_command = robot_command_pb2.MobilityCommand.Request(
se2_velocity_request=vel_command, params=any_params)
command = robot_command_pb2.RobotCommand(mobility_command=mobility_command)
return command
def trajectory_command(goal_x, goal_y, goal_heading, frame, params=None, body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_AUTO):
"""Command robot to move to pose along a 2D plane. Pose can specified in the world
(kinematic odometry) frame or the robot body frame. The arguments body_height and
locomotion_hint are ignored if params argument is passed.
A trajectory command requires an end time. End time is not set in this function, but rather
is set externally before call to RobotCommandService.
Returns:
RobotCommand, which can be issued to the robot command service.
"""
if not params:
params = RobotCommandBuilder.mobility_params(body_height=body_height,
locomotion_hint=locomotion_hint)
any_params = RobotCommandBuilder._to_any(params)
position = geometry_pb2.Vec2(x=goal_x, y=goal_y)
pose = geometry_pb2.SE2Pose(position=position, angle=goal_heading)
point = trajectory_pb2.SE2TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE2Trajectory(points=[point], frame=frame)
traj_command = robot_command_pb2.SE2TrajectoryCommand.Request(trajectory=traj)
mobility_command = robot_command_pb2.MobilityCommand.Request(
se2_trajectory_request=traj_command, params=any_params)
command = robot_command_pb2.RobotCommand(mobility_command=mobility_command)
return command
def test_create_se2_pose():
# Test creating an SE2Pose from a proto with from_obj()
proto_se2 = geometry_pb2.SE2Pose(position=geometry_pb2.Vec2(x=1, y=2),
angle=.2)
se2 = SE2Pose.from_obj(proto_se2)
assert type(se2) == SE2Pose
assert se2.x == proto_se2.position.x
assert se2.y == proto_se2.position.y
assert se2.angle == proto_se2.angle
# Test proto-like attribute access properties
pos = se2.position
assert type(pos) == geometry_pb2.Vec2
assert pos.x == proto_se2.position.x
assert pos.y == proto_se2.position.y
# Test going back to a proto message with to_proto()
new_proto_se2 = se2.to_proto()
assert type(new_proto_se2) == geometry_pb2.SE2Pose
assert new_proto_se2.position.x == proto_se2.position.x
assert new_proto_se2.position.y == proto_se2.position.y
assert new_proto_se2.angle == proto_se2.angle
# Test mutating an existing proto message to_obj()
proto_mut_se2 = geometry_pb2.SE2Pose()
se2.to_obj(proto_mut_se2)
assert se2.x == proto_mut_se2.position.x
assert se2.y == proto_mut_se2.position.y
assert se2.angle == proto_mut_se2.angle
def get_walking_params(max_linear_vel, max_rotation_vel):
max_vel_linear = geometry_pb2.Vec2(x=max_linear_vel, y=max_linear_vel)
max_vel_se2 = geometry_pb2.SE2Velocity(linear=max_vel_linear,
angular=max_rotation_vel)
vel_limit = geometry_pb2.SE2VelocityLimit(max_vel=max_vel_se2)
params = RobotCommandBuilder.mobility_params()
params.vel_limit.CopyFrom(vel_limit)
return params
def velocity_command(v_x,
v_y,
v_rot,
params=None,
body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_AUTO,
frame_name=BODY_FRAME_NAME):
"""Command robot to move along 2D plane. Velocity should be specified in the robot body
frame. Other frames are currently not supported. The arguments body_height and
locomotion_hint are ignored if params argument is passed.
A velocity command requires an end time. End time is not set in this function, but rather
is set externally before call to RobotCommandService.
Args:
v_x: Velocity in X direction.
v_y: Velocity in Y direction.
v_rot: Velocity heading in radians.
params(spot.MobilityParams): Spot specific parameters for mobility commands. If not set,
this will be constructed using other args.
body_height: Body height in meters.
locomotion_hint: Locomotion hint to use for the velocity command.
frame_name: Name of the frame to use.
Returns:
RobotCommand, which can be issued to the robot command service.
"""
if not params:
params = RobotCommandBuilder.mobility_params(
body_height=body_height, locomotion_hint=locomotion_hint)
any_params = RobotCommandBuilder._to_any(params)
linear = geometry_pb2.Vec2(x=v_x, y=v_y)
vel = geometry_pb2.SE2Velocity(linear=linear, angular=v_rot)
slew_rate_limit = geometry_pb2.SE2Velocity(linear=geometry_pb2.Vec2(
x=4, y=4),
angular=2.0)
vel_command = basic_command_pb2.SE2VelocityCommand.Request(
velocity=vel,
se2_frame_name=frame_name,
slew_rate_limit=slew_rate_limit)
mobility_command = mobility_command_pb2.MobilityCommand.Request(
se2_velocity_request=vel_command, params=any_params)
command = robot_command_pb2.RobotCommand(
mobility_command=mobility_command)
return command
def test_synchro_se2_trajectory_command():
goal_x = 1.0
goal_y = 2.0
goal_heading = 3.0
frame = ODOM_FRAME_NAME
position = geometry_pb2.Vec2(x=goal_x, y=goal_y)
goal_se2 = geometry_pb2.SE2Pose(position=position, angle=goal_heading)
command = RobotCommandBuilder.synchro_se2_trajectory_command(
goal_se2, frame)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
def get_mobility_params():
"""Gets mobility parameters for following"""
vel_desired = .75
speed_limit = geo.SE2VelocityLimit(
max_vel=geo.SE2Velocity(linear=geo.Vec2(x=vel_desired, y=vel_desired), angular=.25))
body_control = set_default_body_control()
mobility_params = spot_command_pb2.MobilityParams(vel_limit=speed_limit, obstacle_params=None,
body_control=body_control,
locomotion_hint=spot_command_pb2.HINT_TROT)
return mobility_params
def test_create_se2_vel():
# Test creating an SE2Velocity from a proto with from_obj()
proto_se2 = geometry_pb2.SE2Velocity(linear=geometry_pb2.Vec2(x=1, y=2),
angular=.2)
se2 = SE2Velocity.from_obj(proto_se2)
assert type(se2) == SE2Velocity
assert se2.linear_velocity_x == proto_se2.linear.x
assert se2.linear_velocity_y == proto_se2.linear.y
assert se2.angular_velocity == proto_se2.angular
# Test proto-like attribute access properties
lin = se2.linear
assert type(lin) == geometry_pb2.Vec2
assert lin.x == proto_se2.linear.x
assert lin.y == proto_se2.linear.y
ang = se2.angular
assert ang == proto_se2.angular
# Test going back to a proto message with to_proto()
new_proto_se2 = se2.to_proto()
assert type(new_proto_se2) == geometry_pb2.SE2Velocity
assert new_proto_se2.linear.x == proto_se2.linear.x
assert new_proto_se2.linear.y == proto_se2.linear.y
assert new_proto_se2.angular == proto_se2.angular
# Test mutating an existing proto message to_obj()
proto_mut_se2 = geometry_pb2.SE2Velocity()
se2.to_obj(proto_mut_se2)
assert se2.linear_velocity_x == proto_mut_se2.linear.x
assert se2.linear_velocity_y == proto_mut_se2.linear.y
assert se2.angular_velocity == proto_mut_se2.angular
# Test creating the velocity vector
vec = se2.to_vector()
assert type(vec) == numpy.ndarray
assert vec[0] == proto_se2.linear.x
assert vec[1] == proto_se2.linear.y
assert vec[2] == proto_se2.angular
# Test creating the SE2Velocity from a array
vel_arr = numpy.array([1, 2, 3]).reshape((3, 1))
se2 = SE2Velocity.from_vector(vel_arr)
assert type(se2) == SE2Velocity
assert se2.linear_velocity_x == 1
assert se2.linear_velocity_y == 2
assert se2.angular_velocity == 3
# Test creating the SE2Velocity from a list
vel_list = [1, 2, 3]
se2 = SE2Velocity.from_vector(vel_list)
assert type(se2) == SE2Velocity
assert se2.linear_velocity_x == 1
assert se2.linear_velocity_y == 2
assert se2.angular_velocity == 3
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 main(argv):
"""An example using the API demonstrating adding image coordinates to the world object service."""
parser = argparse.ArgumentParser()
bosdyn.client.util.add_common_arguments(parser)
options = parser.parse_args(argv)
# Create robot object with a world object client.
sdk = bosdyn.client.create_standard_sdk('WorldObjectClient')
sdk.load_app_token(options.app_token)
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()
# Create the world object client.
world_object_client = robot.ensure_client(
WorldObjectClient.default_service_name)
# List all world objects in the scene before any mutation.
world_objects = world_object_client.list_world_objects().world_objects
print("Current World objects before mutations: " +
str([obj for obj in world_objects]))
# Set the detection time for the additional april tag. The client library will convert the time into robot time.
# Uses a function to get google.protobuf.Timestamp of the current system time.
timestamp = now_timestamp()
# Create the image coordinate object. This type of object does not require a base frame for the world object.
# Since we are not providing a transform to the object expressed by the image coordinates, than it is not necessary
# to set the frame_name_image_properties, as this describes the frame used in a transform (such as world_tform_image_coords)
img_coord = wo.ImageProperties(
camera_source="back",
coordinates=geom.Polygon(vertexes=[geom.Vec2(x=100, y=100)]))
wo_obj = wo.WorldObject(id=2,
name="img_coord_tester",
acquisition_time=timestamp,
image_properties=img_coord)
# Request to add the image coordinates detection to the world object service.
add_coords = make_add_world_object_req(wo_obj)
resp = world_object_client.mutate_world_objects(mutation_req=add_coords)
# List all world objects in the scene after the mutation was applied.
world_objects = world_object_client.list_world_objects().world_objects
print("Current World objects after adding coordinates: " +
str([obj for obj in world_objects]))
return True
def test_synchro_se2_trajectory_command():
goal_x = 1.0
goal_y = 2.0
goal_heading = 3.0
frame = ODOM_FRAME_NAME
position = geometry_pb2.Vec2(x=goal_x, y=goal_y)
goal_se2 = geometry_pb2.SE2Pose(position=position, angle=goal_heading)
command = RobotCommandBuilder.synchro_se2_trajectory_command(goal_se2, frame)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
# with a build_on_command
arm_command = RobotCommandBuilder.arm_stow_command()
command = RobotCommandBuilder.synchro_se2_trajectory_command(goal_se2, frame,
build_on_command=arm_command)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
_test_has_arm(command.synchronized_command)
def trajectory_command(goal_x,
goal_y,
goal_heading,
frame_name,
params=None,
body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_AUTO):
"""Command robot to move to pose along a 2D plane. Pose can specified in the world
(kinematic odometry) frame or the robot body frame. The arguments body_height and
locomotion_hint are ignored if params argument is passed.
A trajectory command requires an end time. End time is not set in this function, but rather
is set externally before call to RobotCommandService.
Args:
goal_x: Position X coordinate.
goal_y: Position Y coordinate.
goal_heading: Pose heading in radians.
frame_name: Name of the frame to use.
params(spot.MobilityParams): Spot specific parameters for mobility commands. If not set,
this will be constructed using other args.
body_height: Body height in meters.
locomotion_hint: Locomotion hint to use for the trajectory command.
Returns:
RobotCommand, which can be issued to the robot command service.
"""
if not params:
params = RobotCommandBuilder.mobility_params(
body_height=body_height, locomotion_hint=locomotion_hint)
any_params = RobotCommandBuilder._to_any(params)
position = geometry_pb2.Vec2(x=goal_x, y=goal_y)
pose = geometry_pb2.SE2Pose(position=position, angle=goal_heading)
point = trajectory_pb2.SE2TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE2Trajectory(points=[point])
traj_command = basic_command_pb2.SE2TrajectoryCommand.Request(
trajectory=traj, se2_frame_name=frame_name)
mobility_command = mobility_command_pb2.MobilityCommand.Request(
se2_trajectory_request=traj_command, params=any_params)
command = robot_command_pb2.RobotCommand(
mobility_command=mobility_command)
return command
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 main(argv):
"""An example using the API to apply mutations to world objects."""
parser = argparse.ArgumentParser()
bosdyn.client.util.add_common_arguments(parser)
options = parser.parse_args(argv)
# Create robot object with a world object client.
sdk = bosdyn.client.create_standard_sdk('WorldObjectClient')
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()
# Create the world object client.
world_object_client = robot.ensure_client(
WorldObjectClient.default_service_name)
# List all world objects in the scene.
world_objects = world_object_client.list_world_objects().world_objects
print("Current World objects' ids " +
str([obj.id for obj in world_objects]))
# If there are any world objects in Spot's perception scene, then attempt to mutate one.
# This should fail and return a STATUS_NO_PERMISSION since a client cannot mutate
# objects that they did not add into the scene.
if len(world_objects) > 0:
obj_to_mutate = world_objects[0]
# Attempt to delete the object.
delete_req = make_delete_world_object_req(obj_to_mutate)
status = world_object_client.mutate_world_objects(delete_req).status
assert (status == world_object_pb2.MutateWorldObjectResponse.
STATUS_NO_PERMISSION)
# Attempt to change the object.
for edge in obj_to_mutate.transforms_snapshot.child_to_parent_edge_map:
obj_to_mutate.transforms_snapshot.child_to_parent_edge_map[
edge].parent_tform_child.position.x = 1.0
change_req = make_change_world_object_req(obj_to_mutate)
status = world_object_client.mutate_world_objects(change_req).status
assert (status == world_object_pb2.MutateWorldObjectResponse.
STATUS_NO_PERMISSION)
# Request to add the new april tag detection to the world object service.
wo_obj_to_add = create_apriltag_object()
add_apriltag = make_add_world_object_req(wo_obj_to_add)
resp = world_object_client.mutate_world_objects(mutation_req=add_apriltag)
# Get the world object ID set by the service, so that we can make additional changes to this object.
added_apriltag_world_obj_id = resp.mutated_object_id
# List all world objects in the scene after the mutation was applied.
world_objects = world_object_client.list_world_objects().world_objects
print("World object IDs after object addition: " +
str([obj.apriltag_properties.tag_id for obj in world_objects]))
for world_obj in world_objects:
if world_obj.id == added_apriltag_world_obj_id:
# Look for the custom frame that was included in the add-request, where the child frame name was "my_special_frame"
full_snapshot = world_obj.transforms_snapshot
for edge in full_snapshot.child_to_parent_edge_map:
if edge == "my_special_frame":
print(
"The world object includes the custom frame vision_tform_my_special_frame!"
)
# Request to change an existing apriltag's dimensions. This will succeed because it is changing
# an object that was added by a client program. We are using the ID returned by the service to
# change the correct apriltag.
time_now = now_timestamp()
tag_prop_modified = world_object_pb2.AprilTagProperties(
tag_id=308, dimensions=geom.Vec2(x=.35, y=.35))
wo_obj_to_change = world_object_pb2.WorldObject(
id=added_apriltag_world_obj_id,
name="world_obj_apriltag",
transforms_snapshot=wo_obj_to_add.transforms_snapshot,
acquisition_time=time_now,
apriltag_properties=tag_prop_modified)
print("World object X dimension of apriltag size before change: " +
str([obj.apriltag_properties.dimensions.x for obj in world_objects]))
change_apriltag = make_change_world_object_req(wo_obj_to_change)
resp = world_object_client.mutate_world_objects(
mutation_req=change_apriltag)
assert (
resp.status == world_object_pb2.MutateWorldObjectResponse.STATUS_OK)
# List all world objects in the scene after the mutation was applied.
world_objects = world_object_client.list_world_objects().world_objects
print("World object X dimension of apriltag size after change: " +
str([obj.apriltag_properties.dimensions.x for obj in world_objects]))
# Add a apriltag and then delete it. This will succeed because it is deleting an object added by
# a client program and not specific to Spot's perception
add_apriltag = make_add_world_object_req(wo_obj_to_add)
resp = world_object_client.mutate_world_objects(mutation_req=add_apriltag)
assert (
resp.status == world_object_pb2.MutateWorldObjectResponse.STATUS_OK)
apriltag_to_delete_id = resp.mutated_object_id
# Update the list of world object's after adding a apriltag
world_objects = world_object_client.list_world_objects().world_objects
# Delete the april tag that was just added. This will succeed because it is changing an object that was
# just added by a client program (and not an object Spot's perception system detected). The world object
# can be identified by the ID returned from the service after the mutation request succeeded.
wo_obj_to_delete = world_object_pb2.WorldObject(id=apriltag_to_delete_id)
delete_apriltag = make_delete_world_object_req(wo_obj_to_delete)
resp = world_object_client.mutate_world_objects(
mutation_req=delete_apriltag)
assert (
resp.status == world_object_pb2.MutateWorldObjectResponse.STATUS_OK)
# List all world objects in the scene after the deletion was applied.
world_objects = world_object_client.list_world_objects().world_objects
print("World object IDs after object deletion: " +
str([obj.apriltag_properties.tag_id for obj in world_objects]))
# Add a drawable object into the perception scene with a custom frame and unique name.
sphere_to_add = create_drawable_sphere_object()
add_sphere = make_add_world_object_req(sphere_to_add)
resp = world_object_client.mutate_world_objects(mutation_req=add_sphere)
# Get the world object ID set by the service.
sphere_id = resp.mutated_object_id
# List all world objects in the scene after the mutation was applied. Find the sphere in the list
# and see the transforms added into the frame tree snapshot by Spot in addition to the custom frame.
world_objects = world_object_client.list_world_objects().world_objects
for world_obj in world_objects:
if world_obj.id == sphere_id:
print("Found sphere named " + world_obj.name)
full_snapshot = world_obj.transforms_snapshot
for edge in full_snapshot.child_to_parent_edge_map:
print("Child frame name: " + edge + ". Parent frame name: " +
full_snapshot.child_to_parent_edge_map[edge].
parent_frame_name)
return True
def to_proto(self):
"""Converts the math_helpers.SE2Pose into an output of the protobuf geometry_pb2.SE2Pose."""
return geometry_pb2.SE2Pose(position=geometry_pb2.Vec2(x=self.x,
y=self.y),
angle=self.angle)
def linear(self):
"""Property to allow attribute access of the protobuf message field 'linear' similar to the geometry_pb2.SE2Velocity
for the math_helper.SE2Velocity."""
return geometry_pb2.Vec2(x=self.linear_velocity_x,
y=self.linear_velocity_y)
def to_proto(self):
"""Converts the math_helpers.SE2Velocity into an output of the protobuf geometry_pb2.SE2Velocity."""
return geometry_pb2.SE2Velocity(linear=geometry_pb2.Vec2(
x=self.linear_velocity_x, y=self.linear_velocity_y),
angular=self.angular_velocity)
def position(self):
"""Property to allow attribute access of the protobuf message field 'position' similar to
the geometry_pb2.SE2Pose for the math_helper.SE2Pose."""
return geometry_pb2.Vec2(x=self.x, y=self.y)
from bosdyn.client.frame_helpers import ODOM_FRAME_NAME
from bosdyn.client.world_object import WorldObjectClient
LOGGER = logging.getLogger()
VELOCITY_BASE_SPEED = 0.5 # m/s
VELOCITY_BASE_ANGULAR = 0.8 # rad/sec
VELOCITY_CMD_DURATION = 0.6 # seconds
COMMAND_INPUT_RATE = 0.1
# Velocity limits for navigation (optional)
NAV_VELOCITY_MAX_YAW = 1.2 # rad/s
NAV_VELOCITY_MAX_X = 1.0 # m/s
NAV_VELOCITY_MAX_Y = 0.5 # m/s
NAV_VELOCITY_LIMITS = geometry_pb2.SE2VelocityLimit(
max_vel=geometry_pb2.SE2Velocity(linear=geometry_pb2.Vec2(
x=NAV_VELOCITY_MAX_X, y=NAV_VELOCITY_MAX_Y),
angular=NAV_VELOCITY_MAX_YAW),
min_vel=geometry_pb2.SE2Velocity(linear=geometry_pb2.Vec2(
x=-NAV_VELOCITY_MAX_X, y=-NAV_VELOCITY_MAX_Y),
angular=-NAV_VELOCITY_MAX_YAW))
def _grpc_or_log(desc, thunk):
try:
return thunk()
except (ResponseError, RpcError) as err:
LOGGER.error("Failed %s: %s" % (desc, err))
class ExitCheck(object):
"""A class to help exiting a loop, also capturing SIGTERM to exit the loop."""
def walk_to_object(config):
"""Get an image and command the robot to walk up to a selected object.
We'll walk "up to" the object, not on top of it. The idea is that you
want to interact or manipulate the object."""
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('WalkToObjectClient')
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."
lease_client = robot.ensure_client(
bosdyn.client.lease.LeaseClient.default_service_name)
image_client = robot.ensure_client(ImageClient.default_service_name)
manipulation_api_client = robot.ensure_client(
ManipulationApiClient.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)
# Take a picture with a camera
robot.logger.info('Getting an image from: ' + config.image_source)
image_responses = image_client.get_image_from_sources(
[config.image_source])
if len(image_responses) != 1:
print('Got invalid number of images: ' +
str(len(image_responses)))
print(image_responses)
assert False
image = image_responses[0]
if image.shot.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_DEPTH_U16:
dtype = np.uint16
else:
dtype = np.uint8
img = np.fromstring(image.shot.image.data, dtype=dtype)
if image.shot.image.format == image_pb2.Image.FORMAT_RAW:
img = img.reshape(image.shot.image.rows, image.shot.image.cols)
else:
img = cv2.imdecode(img, -1)
# Show the image to the user and wait for them to click on a pixel
robot.logger.info('Click on an object to walk up to...')
image_title = 'Click to walk up to something'
cv2.namedWindow(image_title)
cv2.setMouseCallback(image_title, cv_mouse_callback)
global g_image_click, g_image_display
g_image_display = img
cv2.imshow(image_title, g_image_display)
while g_image_click is None:
key = cv2.waitKey(1) & 0xFF
if key == ord('q') or key == ord('Q'):
# Quit
print('"q" pressed, exiting.')
exit(0)
robot.logger.info('Walking to object at image location (' +
str(g_image_click[0]) + ', ' +
str(g_image_click[1]) + ')')
walk_vec = geometry_pb2.Vec2(x=g_image_click[0],
y=g_image_click[1])
# Optionally populate the offset distance parameter.
if config.distance is None:
offset_distance = None
else:
offset_distance = wrappers_pb2.FloatValue(
value=config.distance)
# Build the proto
walk_to = manipulation_api_pb2.WalkToObjectInImage(
pixel_xy=walk_vec,
transforms_snapshot_for_camera=image.shot.transforms_snapshot,
frame_name_image_sensor=image.shot.frame_name_image_sensor,
camera_model=image.source.pinhole,
offset_distance=offset_distance)
# Ask the robot to pick up the object
walk_to_request = manipulation_api_pb2.ManipulationApiRequest(
walk_to_object_in_image=walk_to)
# Send the request
cmd_response = manipulation_api_client.manipulation_api_command(
manipulation_api_request=walk_to_request)
# Get feedback from the robot
while True:
time.sleep(0.25)
feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest(
manipulation_cmd_id=cmd_response.manipulation_cmd_id)
# Send the request
response = manipulation_api_client.manipulation_api_feedback_command(
manipulation_api_feedback_request=feedback_request)
print(
'Current state: ',
manipulation_api_pb2.ManipulationFeedbackState.Name(
response.current_state))
if response.current_state == manipulation_api_pb2.MANIP_STATE_DONE:
break
robot.logger.info('Finished.')
time.sleep(4.0)
robot.logger.info('Sitting down and 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)
