def construct_hold_pose_task():
""" Helper function for holding the pose of the hand
Use this if you want to hold the position of the hand,
without leaving constrained manipulation.
Output:
+ command: api command object
"""
frame_name = "hand"
force_lim = 80
torque_lim = 10
force_direction = geometry_pb2.Vec3(x=1.0, y=0.0, z=0.0)
torque_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0)
init_wrench_dir = geometry_pb2.Wrench(force=force_direction,
torque=torque_direction)
task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_HOLD_POSE
command = RobotCommandBuilder.constrained_manipulation_command(
task_type=task_type,
init_wrench_direction_in_frame_name=init_wrench_dir,
force_limit=force_lim,
torque_limit=torque_lim,
tangential_speed=0.0,
frame_name=frame_name)
return command
def to_proto(self):
"""Converts the math_helpers.SE3Velocity into an output of the protobuf geometry_pb2.SE3Velocity."""
return geometry_pb2.SE3Velocity(
linear=geometry_pb2.Vec3(x=self.linear_velocity_x, y=self.linear_velocity_y,
z=self.linear_velocity_z),
angular=geometry_pb2.Vec3(x=self.angular_velocity_x, y=self.angular_velocity_y,
z=self.angular_velocity_z))
def move_arm(robot_state, is_admittance, world_T_goals, arm_surface_contact_client, velocity,
allow_walking, world_T_admittance, press_force_percentage, api_send_frame,
use_xy_to_z_cross_term, bias_force_x):
traj = move_along_trajectory(api_send_frame, velocity, world_T_goals)
press_force = geometry_pb2.Vec3(x=0, y=0, z=press_force_percentage)
max_vel = wrappers_pb2.DoubleValue(value=velocity)
cmd = arm_surface_contact_pb2.ArmSurfaceContact.Request(
pose_trajectory_in_task=traj, root_frame_name=api_send_frame,
root_tform_task=world_T_admittance, press_force_percentage=press_force,
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_POSITION,
max_linear_velocity=max_vel)
if is_admittance:
# Add admittance options
cmd.z_axis = arm_surface_contact_pb2.ArmSurfaceContact.Request.AXIS_MODE_FORCE
cmd.press_force_percentage.z = press_force_percentage
#if admittance_frame is not None:
# Set the robot to be really stiff in x/y and really sensitive to admittance in z.
cmd.xy_admittance = arm_surface_contact_pb2.ArmSurfaceContact.Request.ADMITTANCE_SETTING_OFF
cmd.z_admittance = arm_surface_contact_pb2.ArmSurfaceContact.Request.ADMITTANCE_SETTING_LOOSE
if use_xy_to_z_cross_term:
cmd.xy_to_z_cross_term_admittance = arm_surface_contact_pb2.ArmSurfaceContact.Request.ADMITTANCE_SETTING_VERY_STIFF
else:
cmd.xy_to_z_cross_term_admittance = arm_surface_contact_pb2.ArmSurfaceContact.Request.ADMITTANCE_SETTING_OFF
# Set a bias force
cmd.bias_force_ewrt_body.CopyFrom(geometry_pb2.Vec3(x=bias_force_x, y=0, z=0))
else:
cmd.bias_force_ewrt_body.CopyFrom(geometry_pb2.Vec3(x=0, y=0, z=0))
gripper_cmd_packed = RobotCommandBuilder.claw_gripper_open_fraction_command(0)
cmd.gripper_command.CopyFrom(
gripper_cmd_packed.synchronized_command.gripper_command.claw_gripper_command)
cmd.is_robot_following_hand = allow_walking
# Build the request proto
proto = arm_surface_contact_service_pb2.ArmSurfaceContactCommand(request=cmd)
# Send the request
arm_surface_contact_client.arm_surface_contact_command(proto)
def build_body_external_forces(external_force_indicator=spot_command_pb2.BodyExternalForceParams.EXTERNAL_FORCE_NONE,
override_external_force_vec=None):
"""Helper to create Mobility params. This function allows the user to enable an external
force estimator, or set a vector of forces (in the body frame) which override the estimator
with constant external forces.
Args:
external_force_indicator: Indicates if the external force estimator should be enabled/disabled
or an override force should be used. Can be specified as one of three values:
spot_command_pb2.BodyExternalForceParams.{ EXTERNAL_FORCE_NONE,
EXTERNAL_FORCE_USE_ESTIMATE,
EXTERNAL_FORCE_USE_OVERRIDE }
override_external_force_vec: x/y/z list of forces in the body frame. Only used when the indicator
specifies EXTERNAL_FORCE_USE_OVERRIDE
Returns:
spot.MobilityParams, params for spot mobility commands.
"""
if external_force_indicator == spot_command_pb2.BodyExternalForceParams.EXTERNAL_FORCE_USE_OVERRIDE:
if override_external_force_vec is None:
#Default the override forces to all zeros if none are specified
override_external_force_vec = (0.0, 0.0, 0.0)
body_frame = geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_BODY)
ext_forces = geometry_pb2.Vec3(x=override_external_force_vec[0], y=override_external_force_vec[1], z=override_external_force_vec[2])
return spot_command_pb2.BodyExternalForceParams(external_force_indicator=external_force_indicator,
frame=body_frame,
external_force_override=ext_forces)
elif (external_force_indicator == spot_command_pb2.BodyExternalForceParams.EXTERNAL_FORCE_NONE or
external_force_indicator == spot_command_pb2.BodyExternalForceParams.EXTERNAL_FORCE_USE_ESTIMATE):
return spot_command_pb2.BodyExternalForceParams(external_force_indicator=external_force_indicator)
else:
return None
def update_frame(tf, position_change, rotation_change):
return geom.SE3Pose(
position=geom.Vec3(
x=tf.position.x + position_change[0], y=tf.position.y + position_change[1],
z=tf.position.z + position_change[2]), rotation=geom.Quaternion(
x=tf.rotation.x + rotation_change[0], y=tf.rotation.y + rotation_change[1],
z=tf.rotation.z + rotation_change[2], w=tf.rotation.w + rotation_change[3]))
def create_drawable_sphere_object():
"""Create a drawable sphere world object that can be added through a mutation request."""
# Add an edge where the transform vision_tform_object is only a position transform with no rotation.
vision_tform_drawable = geom.SE3Pose(position=geom.Vec3(x=2, y=3, z=2),
rotation=geom.Quaternion(x=0, y=0, z=0, w=1))
# Create a map between the child frame name and the parent frame name/SE3Pose parent_tform_child
edges = {}
# Create an edge in the frame tree snapshot that includes vision_tform_drawable
drawable_frame_name = "drawing_sphere"
edges = add_edge_to_tree(edges, vision_tform_drawable, VISION_FRAME_NAME, drawable_frame_name)
snapshot = geom.FrameTreeSnapshot(child_to_parent_edge_map=edges)
# Set the acquisition time for the sphere using a function to get google.protobuf.Timestamp of the current system time.
time_now = now_timestamp()
# Create the sphere drawable object
sphere = world_object_pb2.DrawableSphere(radius=1)
red_color = world_object_pb2.DrawableProperties.Color(r=255, b=0, g=0, a=1)
sphere_drawable_prop = world_object_pb2.DrawableProperties(
color=red_color, label="red sphere", wireframe=False, sphere=sphere,
frame_name_drawable=drawable_frame_name)
# Create the complete world object with transform information, a unique name, and the drawable sphere properties.
world_object_sphere = world_object_pb2.WorldObject(id=16, name="red_sphere_ball",
transforms_snapshot=snapshot,
acquisition_time=time_now,
drawable_properties=[sphere_drawable_prop])
return world_object_sphere
def test_create_se3_pose():
# Test creating an SE3Pose from a proto with from_obj()
proto_se3 = geometry_pb2.SE3Pose(position=geometry_pb2.Vec3(x=1, y=2, z=3),
rotation=geometry_pb2.Quaternion(w=.1, x=.2, y=.2, z=.1))
se3 = SE3Pose.from_obj(proto_se3)
assert type(se3) == SE3Pose
assert se3.x == proto_se3.position.x
assert se3.y == proto_se3.position.y
assert se3.z == proto_se3.position.z
assert se3.rot.w == proto_se3.rotation.w
assert se3.rot.x == proto_se3.rotation.x
assert se3.rot.y == proto_se3.rotation.y
assert se3.rot.z == proto_se3.rotation.z
# Test proto-like attribute access properties
pos = se3.position
assert type(pos) == geometry_pb2.Vec3
assert pos.x == proto_se3.position.x
assert pos.y == proto_se3.position.y
assert pos.z == proto_se3.position.z
quat = se3.rotation
assert type(quat) == Quat
assert quat.w == proto_se3.rotation.w
assert quat.x == proto_se3.rotation.x
assert quat.y == proto_se3.rotation.y
assert quat.z == proto_se3.rotation.z
# Test going back to a proto message with to_proto()
new_proto_se3 = se3.to_proto()
assert type(new_proto_se3) == geometry_pb2.SE3Pose
assert new_proto_se3.position.x == proto_se3.position.x
assert new_proto_se3.position.y == proto_se3.position.y
assert new_proto_se3.position.z == proto_se3.position.z
assert new_proto_se3.rotation.w == proto_se3.rotation.w
assert new_proto_se3.rotation.x == proto_se3.rotation.x
assert new_proto_se3.rotation.y == proto_se3.rotation.y
assert new_proto_se3.rotation.z == proto_se3.rotation.z
# Test mutating an existing proto message to_obj()
proto_mut_se3 = geometry_pb2.SE3Pose()
se3.to_obj(proto_mut_se3)
assert se3.x == proto_mut_se3.position.x
assert se3.y == proto_mut_se3.position.y
assert se3.z == proto_mut_se3.position.z
assert se3.rot.w == proto_mut_se3.rotation.w
assert se3.rot.x == proto_mut_se3.rotation.x
assert se3.rot.y == proto_mut_se3.rotation.y
assert se3.rot.z == proto_mut_se3.rotation.z
# Test identity SE3Pose
identity = SE3Pose.from_identity()
assert identity.x == 0
assert identity.y == 0
assert identity.z == 0
assert identity.rot.w == 1
assert identity.rot.x == 0
assert identity.rot.y == 0
assert identity.rot.z == 0
def 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 construct_right_handed_ballvalve_task(velocity_normalized,
force_limit=40,
torque_limit=5):
""" Helper function for manipulating right-handed ball valves
Use this when the hand is to the right of the pivot of the ball valve
And when hand x axis is roughly parallel to the axis of rotation of
the ball valve
params:
+ velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0]
+ force_limit (optional): positive value denoting max force robot will exert along task dimension
+ torque_limit (optional): positive value denoting max torque robot will exert along
the axis of rotation of the task
Output:
+ command: api command object
Notes:
If the grasp is such that the hand x axis is not parallel to the axis
of rotation of the ball valve, then use the lever task.
"""
velocity_normalized = max(min(velocity_normalized, 1.0), -1.0)
velocity_limit = scale_velocity_lim_given_force_lim(force_limit)
tangential_velocity = velocity_normalized * velocity_limit
frame_name = "hand"
force_lim = force_limit
torque_lim = torque_limit
# Force/torque signs are opposite for right-handed ball valve
force_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=1.0)
# The torque vector is provided as additional information denoting the
# axis of rotation of the task.
torque_direction = geometry_pb2.Vec3(x=-1.0, y=0.0, z=0.0)
init_wrench_dir = geometry_pb2.Wrench(force=force_direction,
torque=torque_direction)
task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_SE3_CIRCLE_FORCE_TORQUE
command = RobotCommandBuilder.constrained_manipulation_command(
task_type=task_type,
init_wrench_direction_in_frame_name=init_wrench_dir,
force_limit=force_lim,
torque_limit=torque_lim,
tangential_speed=tangential_velocity,
frame_name=frame_name)
return command
def start(self):
"""Claim lease of robot and start the fiducial follower."""
self._lease = self._lease_client.acquire()
self._robot.time_sync.wait_for_sync()
self._lease_keepalive = bosdyn.client.lease.LeaseKeepAlive(
self._lease_client)
# Stand the robot up.
if self._standup:
self.power_on()
blocking_stand(self._robot_command_client)
# Delay grabbing image until spot is standing (or close enough to upright).
time.sleep(.35)
while self._attempts <= self._max_attempts:
detected_fiducial = False
fiducial_rt_world = None
if self._use_world_object_service:
# Get the first fiducial object Spot detects with the world object service.
fiducial = self.get_fiducial_objects()
if fiducial is not None:
vision_tform_fiducial = get_a_tform_b(
fiducial.transforms_snapshot, VISION_FRAME_NAME,
fiducial.apriltag_properties.frame_name_fiducial
).to_proto()
if vision_tform_fiducial is not None:
detected_fiducial = True
fiducial_rt_world = vision_tform_fiducial.position
else:
# Detect the april tag in the images from Spot using the apriltag library.
bboxes, source_name = self.image_to_bounding_box()
if bboxes:
self._previous_source = source_name
(tvec, _,
source_name) = self.pixel_coords_to_camera_coords(
bboxes, self._intrinsics, source_name)
vision_tform_fiducial_position = self.compute_fiducial_in_world_frame(
tvec)
fiducial_rt_world = geometry_pb2.Vec3(
x=vision_tform_fiducial_position[0],
y=vision_tform_fiducial_position[1],
z=vision_tform_fiducial_position[2])
detected_fiducial = True
if detected_fiducial:
# Go to the tag and stop within a certain distance
self.go_to_tag(fiducial_rt_world)
else:
print("No fiducials found")
self._attempts += 1 #increment attempts at finding a fiducial
# Power off at the conclusion of the example.
if self._powered_on:
self.power_off()
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 transform_vec3(self, vec3):
"""
Compute the transformation (translation and rotation) of a (x,y,z) vector using the
current SE(3) pose.
Returns:
Vec3 representing the transformed coordinate.
"""
(out_x, out_y, out_z) = self.rot.transform_point(vec3.x, vec3.y, vec3.z)
return geometry_pb2.Vec3(x=out_x + self.x, y=out_y + self.y, z=out_z + self.z)
def construct_lever_task(velocity_normalized, force_limit=40, torque_limit=5):
""" Helper function for manipulating levers
params:
+ velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0]
+ force_limit (optional): positive value denoting max force robot will exert along task dimension
+ torque_limit (optional): positive value denoting max torque robot will exert along
the axis of rotation of the task
Output:
+ command: api command object
Notes:
In this function, we assume the initial motion of the lever is
along the z axis of the hand (up and down). If the initial
grasp is such that the initial motion needs to be something else,
change the force direction.
This function assumes we don't know the plane_normal (i.e. torque_direction)
of the lever. If we do know that, we can specify it as torque_direction
or use the ball valve task types, which assume a specific grasp and specify
what the initial torque_direction is.
"""
velocity_normalized = max(min(velocity_normalized, 1.0), -1.0)
velocity_limit = scale_velocity_lim_given_force_lim(force_limit)
tangential_velocity = velocity_normalized * velocity_limit
frame_name = "hand"
force_lim = force_limit
torque_lim = torque_limit
force_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=1.0)
torque_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0)
init_wrench_dir = geometry_pb2.Wrench(force=force_direction,
torque=torque_direction)
task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_SE3_CIRCLE_FORCE_TORQUE
command = RobotCommandBuilder.constrained_manipulation_command(
task_type=task_type,
init_wrench_direction_in_frame_name=init_wrench_dir,
force_limit=force_lim,
torque_limit=torque_lim,
tangential_speed=tangential_velocity,
frame_name=frame_name)
return command
def construct_crank_task(velocity_normalized, force_limit=40):
""" Helper function for manipulating cranks with a free to rotate handle
params:
+ velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0]
+ force_limit (optional): positive value denoting max force robot will exert along task dimension
Output:
+ command: api command object
Notes:
In this function, we assume the initial motion of the crank is
along the y axis of the hand (left and right). If the initial
grasp is such that the initial motion needs to be something else,
change the force direction.
"""
velocity_normalized = max(min(velocity_normalized, 1.0), -1.0)
velocity_limit = scale_velocity_lim_given_force_lim(force_limit)
tangential_velocity = velocity_normalized * velocity_limit
frame_name = "hand"
force_lim = force_limit
# Setting a placeholder value that doesn't matter, since we don't
# apply a pure torque in this task.
torque_lim = 5.0
# This assumes the grasp of crank is such that the crank will initially
# move along the hand y axis. Change if that is not the case.
force_direction = geometry_pb2.Vec3(x=0.0, y=1.0, z=0.0)
torque_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0)
init_wrench_dir = geometry_pb2.Wrench(force=force_direction,
torque=torque_direction)
task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_R3_CIRCLE_EXTRADOF_FORCE
command = RobotCommandBuilder.constrained_manipulation_command(
task_type=task_type,
init_wrench_direction_in_frame_name=init_wrench_dir,
force_limit=force_lim,
torque_limit=torque_lim,
tangential_speed=tangential_velocity,
frame_name=frame_name)
return command
def construct_wheel_task(velocity_normalized, force_limit=40):
""" Helper function for turning wheels while grasping the rim
Use this when the wheel is grasped on the rim. If the grasp
is on a handle that is free to rotate, use the crank task type.
If the handle is not free to rotate, use this task type.
params:
+ velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0]
+ force_limit (optional): positive value denoting max force robot will exert along task dimension
Output:
+ command: api command object
Notes:
This assumes initial motion will be along the y axis of the hand,
which is often the case. Change force_direction if that is not true.
"""
velocity_normalized = max(min(velocity_normalized, 1.0), -1.0)
velocity_limit = scale_velocity_lim_given_force_lim(force_limit)
tangential_velocity = velocity_normalized * velocity_limit
frame_name = "hand"
force_lim = force_limit
# Setting a placeholder value that doesn't matter, since we don't
# apply a pure torque in this task.
torque_lim = 5.0
force_direction = geometry_pb2.Vec3(x=0.0, y=1.0, z=0.0)
torque_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0)
init_wrench_dir = geometry_pb2.Wrench(force=force_direction,
torque=torque_direction)
task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_R3_CIRCLE_FORCE
command = RobotCommandBuilder.constrained_manipulation_command(
task_type=task_type,
init_wrench_direction_in_frame_name=init_wrench_dir,
force_limit=force_lim,
torque_limit=torque_lim,
tangential_speed=tangential_velocity,
frame_name=frame_name)
return command
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 construct_drawer_task(velocity_normalized, force_limit=40):
""" Helper function for opening/closing drawers
params:
+ velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0]
+ force_limit (optional): positive value denoting max force robot will exert along task dimension
Output:
+ command: api command object
Notes:
In this function, we assume the initial motion of the drawer is
along the x axis of the hand (forward and backward). If the initial
grasp is such that the initial motion needs to be something else,
change the force direction.
"""
velocity_normalized = max(min(velocity_normalized, 1.0), -1.0)
velocity_limit = scale_velocity_lim_given_force_lim(force_limit)
tangential_velocity = velocity_normalized * velocity_limit
frame_name = "hand"
force_lim = force_limit
# Setting a placeholder value that doesn't matter, since we don't
# apply a pure torque in this task.
torque_lim = 5.0
force_direction = geometry_pb2.Vec3(x=1.0, y=0.0, z=0.0)
torque_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0)
init_wrench_dir = geometry_pb2.Wrench(force=force_direction,
torque=torque_direction)
task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_R3_LINEAR_FORCE
command = RobotCommandBuilder.constrained_manipulation_command(
task_type=task_type,
init_wrench_direction_in_frame_name=init_wrench_dir,
force_limit=force_lim,
torque_limit=torque_lim,
tangential_speed=tangential_velocity,
frame_name=frame_name)
return command
def construct_knob_task(velocity_normalized, torque_limit=5):
""" Helper function for turning purely rotational knobs
Use this for turning knobs/valves that do not have a lever arm
params:
+ velocity_normalized: normalized task rotational velocity in range [-1.0, 1.0]
+ torque_limit (optional): positive value denoting max torque robot will exert along axis of
rotation of the task
Output:
+ command: api command object
Notes:
This assumes that the axis of rotation of of the knob is roughly parallel
to the x axis of the hand. Change torque_direction if that is not the case.
"""
velocity_normalized = max(min(velocity_normalized, 1.0), -1.0)
rot_velocity_limit = scale_rot_velocity_lim_given_torque_lim(torque_limit)
rotational_velocity = velocity_normalized * rot_velocity_limit
frame_name = "hand"
# Setting a placeholder value that doesn't matter, since we don't
# apply a pure force in this task.
force_lim = 40.0
torque_lim = torque_limit
force_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0)
torque_direction = geometry_pb2.Vec3(x=1.0, y=0.0, z=0.0)
init_wrench_dir = geometry_pb2.Wrench(force=force_direction,
torque=torque_direction)
task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_SE3_ROTATIONAL_TORQUE
command = RobotCommandBuilder.constrained_manipulation_command(
task_type=task_type,
init_wrench_direction_in_frame_name=init_wrench_dir,
force_limit=force_lim,
torque_limit=torque_lim,
rotational_speed=rotational_velocity,
frame_name=frame_name)
return command
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 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 transform_vec3(self, vec3):
""""Computes the transformation (rotation by the quaternion) of a Vec3
point using the current math_helpers.Quat."""
x, y, z = self.transform_point(vec3.x, vec3.y, vec3.z)
return geometry_pb2.Vec3(x=x, y=y, z=z)
def angular(self):
"""Property to allow attribute access of the protobuf message field 'angular' similar to the geometry_pb2.SE3Velocity
for the math_helper.SE3Velocity."""
return geometry_pb2.Vec3(x=self.angular_velocity_x,
y=self.angular_velocity_y,
z=self.angular_velocity_z)
def arm_surface_contact(config):
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('ArmSurfaceContactExample')
robot = sdk.create_robot(config.hostname)
robot.authenticate(config.username, config.password)
robot.time_sync.wait_for_sync()
assert robot.has_arm(), "Robot requires an arm to run this example."
arm_surface_contact_client = robot.ensure_client(
ArmSurfaceContactClient.default_service_name)
# Verify the robot is not estopped and that an external application has registered and holds
# an estop endpoint.
assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \
"such as the estop SDK example, to configure E-Stop."
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
lease_client = robot.ensure_client(
bosdyn.client.lease.LeaseClient.default_service_name)
lease = lease_client.acquire()
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
# Now, we are ready to power on the robot. This call will block until the power
# is on. Commands would fail if this did not happen. We can also check that the robot is
# powered at any point.
robot.logger.info(
"Powering on robot... This may take a several seconds.")
robot.power_on(timeout_sec=20)
assert robot.is_powered_on(), "Robot power on failed."
robot.logger.info("Robot powered on.")
# Tell the robot to stand up. The command service is used to issue commands to a robot.
# The set of valid commands for a robot depends on hardware configuration. See
# SpotCommandHelper for more detailed examples on command building. The robot
# command service requires timesync between the robot and the client.
robot.logger.info("Commanding robot to stand...")
command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
blocking_stand(command_client, timeout_sec=10)
robot.logger.info("Robot standing.")
time.sleep(2.0)
# Unstow the arm
unstow = RobotCommandBuilder.arm_ready_command()
# Issue the command via the RobotCommandClient
command_client.robot_command(unstow)
robot.logger.info("Unstow command issued.")
time.sleep(3.0)
# ----------
#
# Now we'll use the arm_surface_contact service to do an accurate position move with
# some amount of force.
#
# Position of the hand:
hand_x = 0.75 # in front of the robot.
hand_y_start = 0 # centered
hand_y_end = -0.5 # to the right
hand_z = 0 # will be ignored since we'll have a force in the Z axis.
f_z = -0.05 # percentage of maximum press force, negative to press down
# be careful setting this too large, you can knock the robot over
percentage_press = geometry_pb2.Vec3(x=0, y=0, z=f_z)
hand_vec3_start_rt_body = geometry_pb2.Vec3(x=hand_x,
y=hand_y_start,
z=hand_z)
hand_vec3_end_rt_body = geometry_pb2.Vec3(x=hand_x,
y=hand_y_end,
z=hand_z)
# We want to point the hand straight down the entire time.
qw = 0.707
qx = 0
qy = 0.707
qz = 0
body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz)
# Build a position trajectory
body_T_hand1 = geometry_pb2.SE3Pose(
position=hand_vec3_start_rt_body, rotation=body_Q_hand)
body_T_hand2 = geometry_pb2.SE3Pose(position=hand_vec3_end_rt_body,
rotation=body_Q_hand)
robot_state = robot_state_client.get_robot_state()
odom_T_flat_body = get_a_tform_b(
robot_state.kinematic_state.transforms_snapshot,
ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME)
odom_T_hand1 = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
body_T_hand1)
odom_T_hand2 = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
body_T_hand2)
# Trajectory length
traj_time = 5.0 # in seconds
time_since_reference = seconds_to_duration(traj_time)
traj_point1 = trajectory_pb2.SE3TrajectoryPoint(
pose=odom_T_hand1.to_proto(),
time_since_reference=seconds_to_duration(0))
traj_point2 = trajectory_pb2.SE3TrajectoryPoint(
pose=odom_T_hand2.to_proto(),
time_since_reference=time_since_reference)
hand_traj = trajectory_pb2.SE3Trajectory(
points=[traj_point1, traj_point2])
# Open the gripper
gripper_cmd_packed = RobotCommandBuilder.claw_gripper_open_fraction_command(
0)
gripper_command = gripper_cmd_packed.synchronized_command.gripper_command.claw_gripper_command
cmd = arm_surface_contact_pb2.ArmSurfaceContact.Request(
pose_trajectory_in_task=hand_traj,
root_frame_name=ODOM_FRAME_NAME,
press_force_percentage=percentage_press,
x_axis=arm_surface_contact_pb2.ArmSurfaceContact.Request.
AXIS_MODE_POSITION,
y_axis=arm_surface_contact_pb2.ArmSurfaceContact.Request.
AXIS_MODE_POSITION,
z_axis=arm_surface_contact_pb2.ArmSurfaceContact.Request.
AXIS_MODE_FORCE,
z_admittance=arm_surface_contact_pb2.ArmSurfaceContact.Request.
ADMITTANCE_SETTING_LOOSE,
# Enable the cross term so that if the arm gets stuck in a rut, it will retract
# upwards slightly, preventing excessive lateral forces.
xy_to_z_cross_term_admittance=arm_surface_contact_pb2.
ArmSurfaceContact.Request.ADMITTANCE_SETTING_VERY_STIFF,
gripper_command=gripper_command)
# Enable walking
cmd.is_robot_following_hand = True
# A bias force (in this case, leaning forward) can help improve stability.
bias_force_x = -25
cmd.bias_force_ewrt_body.CopyFrom(
geometry_pb2.Vec3(x=bias_force_x, y=0, z=0))
proto = arm_surface_contact_service_pb2.ArmSurfaceContactCommand(
request=cmd)
# Send the request
robot.logger.info('Running arm surface contact...')
arm_surface_contact_client.arm_surface_contact_command(proto)
time.sleep(traj_time + 5.0)
robot.logger.info('Turning off...')
# Power the robot off. By specifying "cut_immediately=False", a safe power off command
# is issued to the robot. This will attempt to sit the robot before powering off.
robot.power_off(cut_immediately=False, timeout_sec=20)
assert not robot.is_powered_on(), "Robot power off failed."
robot.logger.info("Robot safely powered off.")
finally:
# If we successfully acquired a lease, return it.
lease_client.return_lease(lease)
def force_wrench(config):
"""Commanding a force / wrench with Spot's arm."""
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('ForceWrenchClient')
robot = sdk.create_robot(config.hostname)
robot.authenticate(config.username, config.password)
robot.time_sync.wait_for_sync()
assert robot.has_arm(), "Robot requires an arm to run this example."
# Verify the robot is not estopped and that an external application has registered and holds
# an estop endpoint.
assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \
"such as the estop SDK example, to configure E-Stop."
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
lease_client = robot.ensure_client(
bosdyn.client.lease.LeaseClient.default_service_name)
lease = lease_client.acquire()
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
# Now, we are ready to power on the robot. This call will block until the power
# is on. Commands would fail if this did not happen. We can also check that the robot is
# powered at any point.
robot.logger.info(
"Powering on robot... This may take a several seconds.")
robot.power_on(timeout_sec=20)
assert robot.is_powered_on(), "Robot power on failed."
robot.logger.info("Robot powered on.")
# Tell the robot to stand up. The command service is used to issue commands to a robot.
# The set of valid commands for a robot depends on hardware configuration. See
# SpotCommandHelper for more detailed examples on command building. The robot
# command service requires timesync between the robot and the client.
robot.logger.info("Commanding robot to stand...")
command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
blocking_stand(command_client, timeout_sec=10)
robot.logger.info("Robot standing.")
# Send a second stand command with a lowered body height to allow the arm to reach the ground.
stand_command = RobotCommandBuilder.synchro_stand_command(
body_height=-0.15)
command_id = command_client.robot_command(stand_command, timeout=2)
time.sleep(2.0)
# Unstow the arm
unstow = RobotCommandBuilder.arm_ready_command()
# Issue the command via the RobotCommandClient
command_client.robot_command(unstow)
robot.logger.info("Unstow command issued.")
time.sleep(3.0)
# Start in gravity-compensation mode (but zero force)
f_x = 0 # Newtons
f_y = 0
f_z = 0
# We won't have any rotational torques
torque_x = 0
torque_y = 0
torque_z = 0
# Duration in seconds.
seconds = 1000
# Use the helper function to build a single wrench
command = RobotCommandBuilder.arm_wrench_command(
f_x, f_y, f_z, torque_x, torque_y, torque_z, BODY_FRAME_NAME,
seconds)
# Send the request
command_client.robot_command(command)
robot.logger.info('Zero force commanded...')
time.sleep(5.0)
# Drive the arm into the ground with a specified force:
f_z = -5 # Newtons
command = RobotCommandBuilder.arm_wrench_command(
f_x, f_y, f_z, torque_x, torque_y, torque_z, BODY_FRAME_NAME,
seconds)
# Send the request
command_client.robot_command(command)
time.sleep(5.0)
# --------------- #
# Hybrid position-force mode and trajectories.
#
# We'll move the arm in an X/Y trajectory while maintaining some force on the ground.
# Hand will start to the left and move to the right.
hand_x = 0.75 # in front of the robot.
hand_y_start = 0.25 # to the left
hand_y_end = -0.25 # to the right
hand_z = 0 # will be ignored since we'll have a force in the Z axis.
f_z = -5 # Newtons
hand_vec3_start = geometry_pb2.Vec3(x=hand_x,
y=hand_y_start,
z=hand_z)
hand_vec3_end = geometry_pb2.Vec3(x=hand_x, y=hand_y_end, z=hand_z)
qw = 1
qx = 0
qy = 0
qz = 0
quat = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz)
# Build a position trajectory
hand_pose1_in_flat_body = geometry_pb2.SE3Pose(
position=hand_vec3_start, rotation=quat)
hand_pose2_in_flat_body = geometry_pb2.SE3Pose(
position=hand_vec3_end, rotation=quat)
# Convert the poses to the odometry frame.
robot_state = robot_state_client.get_robot_state()
odom_T_flat_body = get_a_tform_b(
robot_state.kinematic_state.transforms_snapshot,
ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME)
hand_pose1 = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
hand_pose1_in_flat_body)
hand_pose2 = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
hand_pose2_in_flat_body)
traj_time = 5.0
time_since_reference = seconds_to_duration(traj_time)
traj_point1 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose1.to_proto(),
time_since_reference=seconds_to_duration(0))
traj_point2 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose2.to_proto(),
time_since_reference=time_since_reference)
hand_traj = trajectory_pb2.SE3Trajectory(
points=[traj_point1, traj_point2])
# We'll use a constant wrench here. Build a wrench trajectory with a single point.
# Note that we only need to fill out Z-force because that is the only axis that will
# be in force mode.
force_tuple = odom_T_flat_body.rotation.transform_point(x=0,
y=0,
z=f_z)
force = geometry_pb2.Vec3(x=force_tuple[0],
y=force_tuple[1],
z=force_tuple[2])
torque = geometry_pb2.Vec3(x=0, y=0, z=0)
wrench = geometry_pb2.Wrench(force=force, torque=torque)
# We set this point to happen at 0 seconds. The robot will hold the wrench past that
# time, so we'll end up with a constant value.
duration = seconds_to_duration(0)
traj_point = trajectory_pb2.WrenchTrajectoryPoint(
wrench=wrench, time_since_reference=duration)
trajectory = trajectory_pb2.WrenchTrajectory(points=[traj_point])
arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request(
pose_trajectory_in_task=hand_traj,
root_frame_name=ODOM_FRAME_NAME,
wrench_trajectory_in_task=trajectory,
x_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_POSITION,
y_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_POSITION,
z_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
rx_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_POSITION,
ry_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_POSITION,
rz_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_POSITION)
arm_command = arm_command_pb2.ArmCommand.Request(
arm_cartesian_command=arm_cartesian_command)
synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request(
arm_command=arm_command)
robot_command = robot_command_pb2.RobotCommand(
synchronized_command=synchronized_command)
# Send the request
command_client.robot_command(robot_command)
robot.logger.info('Running mixed position and force mode.')
time.sleep(traj_time + 5.0)
# Power the robot off. By specifying "cut_immediately=False", a safe power off command
# is issued to the robot. This will attempt to sit the robot before powering off.
robot.power_off(cut_immediately=False, timeout_sec=20)
assert not robot.is_powered_on(), "Robot power off failed."
robot.logger.info("Robot safely powered off.")
finally:
# If we successfully acquired a lease, return it.
lease_client.return_lease(lease)
def 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('ForceTrajectoryClient')
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)
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)
# Demonstrate an example force trajectory by ramping up and down a vertical force over
# 10 seconds
f_x0 = 0 # Newtons
f_y0 = 0
f_z0 = 0
f_x1 = 0 # Newtons
f_y1 = 0
f_z1 = -10 # push down
# We won't have any rotational torques
torque_x = 0
torque_y = 0
torque_z = 0
# Duration in seconds.
traj_duration = 5
# First point on the trajectory
force0 = geometry_pb2.Vec3(x=f_x0, y=f_y0, z=f_z0)
torque0 = geometry_pb2.Vec3(x=torque_x, y=torque_y, z=torque_z)
wrench0 = geometry_pb2.Wrench(force=force0, torque=torque0)
t0 = seconds_to_duration(0)
traj_point0 = trajectory_pb2.WrenchTrajectoryPoint(
wrench=wrench0, time_since_reference=t0)
# Second point on the trajectory
force1 = geometry_pb2.Vec3(x=f_x1, y=f_y1, z=f_z1)
torque1 = geometry_pb2.Vec3(x=torque_x, y=torque_y, z=torque_z)
wrench1 = geometry_pb2.Wrench(force=force1, torque=torque1)
t1 = seconds_to_duration(traj_duration)
traj_point1 = trajectory_pb2.WrenchTrajectoryPoint(
wrench=wrench1, time_since_reference=t1)
# Build the trajectory
trajectory = trajectory_pb2.WrenchTrajectory(
points=[traj_point0, traj_point1])
# Build the full request, putting all axes into force mode.
arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request(
root_frame_name=ODOM_FRAME_NAME,
wrench_trajectory_in_task=trajectory,
x_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
y_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
z_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
rx_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
ry_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
rz_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE)
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('Force trajectory command issued...')
time.sleep(5.0 + traj_duration)
# 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 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 position(self):
"""Property to allow attribute access of the protobuf message field 'position' similar to the geometry_pb2.SE3Pose
for the math_helper.SE3Pose."""
return geometry_pb2.Vec3(x=self.x, y=self.y, z=self.z)
