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 stand_command(params=None,
body_height=0.0,
footprint_R_body=geometry.EulerZXY()):
"""Command robot to stand. If the robot is sitting, it will stand up. If the robot is
moving, it will come to a stop. Params can specify a trajectory for the body to follow
while standing. In the simplest case, this can be a specific position+orientation which the
body will hold at. The arguments body_height and footprint_R_body are ignored if params
argument is passed.
Args:
params(spot.MobilityParams): Spot specific parameters for mobility commands. If not set,
this will be constructed using other args.
body_height(float): Height, meters, to stand at relative to a nominal stand height.
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)
Returns:
RobotCommand, which can be issued to the robot command service.
"""
if not params:
params = RobotCommandBuilder.mobility_params(
body_height=body_height, footprint_R_body=footprint_R_body)
any_params = RobotCommandBuilder._to_any(params)
mobility_command = mobility_command_pb2.MobilityCommand.Request(
stand_request=basic_command_pb2.StandCommand.Request(),
params=any_params)
command = robot_command_pb2.RobotCommand(
mobility_command=mobility_command)
return command
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 freeze_command():
"""Command to freeze all joints at their current positions (no balancing control)
Returns:
RobotCommand, which can be issued to the robot command service.
"""
full_body_command = robot_command_pb2.FullBodyCommand.Request(
freeze_request=robot_command_pb2.FreezeCommand.Request())
command = robot_command_pb2.RobotCommand(full_body_command=full_body_command)
return command
def selfright_command():
"""Command to get the robot in a ready, sitting position. If the robot is on its back, it
will attempt to flip over.
Returns:
RobotCommand, which can be issued to the robot command service.
"""
full_body_command = robot_command_pb2.FullBodyCommand.Request(
selfright_request=robot_command_pb2.SelfRightCommand.Request())
command = robot_command_pb2.RobotCommand(full_body_command=full_body_command)
return command
def stop_command():
"""Command to stop with minimal motion. If the robot is walking, it will transition to
stand. If the robot is standing or sitting, it will do nothing.
Returns:
RobotCommand, which can be issued to the robot command service.
"""
full_body_command = robot_command_pb2.FullBodyCommand.Request(
stop_request=robot_command_pb2.StopCommand.Request())
command = robot_command_pb2.RobotCommand(full_body_command=full_body_command)
return command
def sit_command(params=None):
"""Command the robot to sit.
Returns:
RobotCommand, which can be issued to the robot command service.
"""
if not params:
params = RobotCommandBuilder.mobility_params()
any_params = RobotCommandBuilder._to_any(params)
mobility_command = robot_command_pb2.MobilityCommand.Request(
sit_request=robot_command_pb2.SitCommand.Request(), params=any_params)
command = robot_command_pb2.RobotCommand(mobility_command=mobility_command)
return command
def safe_power_off_command():
"""Command to get robot into a position where it is safe to power down, then power down. If
the robot has fallen, it will power down directly. If the robot is not in a safe position,
it will get to a safe position before powering down. The robot will not power down until it
is in a safe state.
Returns:
RobotCommand, which can be issued to the robot command service.
"""
full_body_command = robot_command_pb2.FullBodyCommand.Request(
safe_power_off_request=robot_command_pb2.SafePowerOffCommand.Request())
command = robot_command_pb2.RobotCommand(full_body_command=full_body_command)
return command
def make_robot_command(arm_joint_traj):
""" Helper function to create a RobotCommand from an ArmJointTrajectory.
The returned command will be a SynchronizedCommand with an ArmJointMoveCommand
filled out to follow the passed in trajectory. """
joint_move_command = arm_command_pb2.ArmJointMoveCommand.Request(
trajectory=arm_joint_traj)
arm_command = arm_command_pb2.ArmCommand.Request(
arm_joint_move_command=joint_move_command)
sync_arm = synchronized_command_pb2.SynchronizedCommand.Request(
arm_command=arm_command)
arm_sync_robot_cmd = robot_command_pb2.RobotCommand(
synchronized_command=sync_arm)
return RobotCommandBuilder.build_synchro_command(arm_sync_robot_cmd)
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 sit_command(params=None):
"""Command the robot to sit.
Args:
params(spot.MobilityParams): Spot specific parameters for mobility commands.
Returns:
RobotCommand, which can be issued to the robot command service.
"""
if not params:
params = RobotCommandBuilder.mobility_params()
any_params = RobotCommandBuilder._to_any(params)
mobility_command = mobility_command_pb2.MobilityCommand.Request(
sit_request=basic_command_pb2.SitCommand.Request(),
params=any_params)
command = robot_command_pb2.RobotCommand(
mobility_command=mobility_command)
return command
def safe_power_off(command_client,
state_client,
timeout_sec=30,
update_frequency=1.0,
**kwargs):
"""Power off robot safely. This function blocks until robot safely powers off. This means the
robot will attempt to sit before powering off.
Args:
command_client (RobotCommandClient): client for calling RobotCommandService safe power off.
state_client (RobotStateClient): client for monitoring power state.
timeout_sec (float): Max time this function will block for.
update_frequency (float): The frequency with which the robot should check if the command
has succeeded.
Raises:
RpcError: Problem communicating with the robot.
power.CommandTimedOutError: Did not power off within timeout_sec
RobotCommandResponseError: Something went wrong with the safe power off.
"""
start_time = time.time()
end_time = start_time + timeout_sec
update_time = 1.0 / update_frequency
full_body_command = full_body_command_pb2.FullBodyCommand.Request(
safe_power_off_request=basic_command_pb2.SafePowerOffCommand.Request())
command = robot_command_pb2.RobotCommand(
full_body_command=full_body_command)
command_client.robot_command(command=command, **kwargs)
while time.time() < end_time:
time_until_timeout = end_time - time.time()
start_call_time = time.time()
future = state_client.get_robot_state_async(**kwargs)
try:
response = future.result(timeout=time_until_timeout)
if response.power_state.motor_power_state == robot_state_pb2.PowerState.STATE_OFF:
return
except TimeoutError:
raise CommandTimedOutError
call_time = time.time() - start_call_time
sleep_time = max(0.0, update_time - call_time)
time.sleep(sleep_time)
raise CommandTimedOutError
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 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 gaze_control(config):
"""Commanding a gaze with Spot's arm."""
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('GazeDemoClient')
robot = sdk.create_robot(config.hostname)
robot.authenticate(config.username, config.password)
robot.time_sync.wait_for_sync()
assert robot.has_arm(), "Robot requires an arm to run this example."
# Verify the robot is not estopped and that an external application has registered and holds
# an estop endpoint.
assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \
"such as the estop SDK example, to configure E-Stop."
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
lease_client = robot.ensure_client(
bosdyn.client.lease.LeaseClient.default_service_name)
lease = lease_client.acquire()
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
# Now, we are ready to power on the robot. This call will block until the power
# is on. Commands would fail if this did not happen. We can also check that the robot is
# powered at any point.
robot.logger.info(
"Powering on robot... This may take a several seconds.")
robot.power_on(timeout_sec=20)
assert robot.is_powered_on(), "Robot power on failed."
robot.logger.info("Robot powered on.")
# Tell the robot to stand up. The command service is used to issue commands to a robot.
# The set of valid commands for a robot depends on hardware configuration. See
# SpotCommandHelper for more detailed examples on command building. The robot
# command service requires timesync between the robot and the client.
robot.logger.info("Commanding robot to stand...")
command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
blocking_stand(command_client, timeout_sec=10)
robot.logger.info("Robot standing.")
time.sleep(2.0)
# Unstow the arm
unstow = RobotCommandBuilder.arm_ready_command()
# Issue the command via the RobotCommandClient
command_client.robot_command(unstow)
robot.logger.info("Unstow command issued.")
time.sleep(3.0)
# Convert the location from the moving base frame to the world frame.
robot_state = robot_state_client.get_robot_state()
odom_T_flat_body = get_a_tform_b(
robot_state.kinematic_state.transforms_snapshot,
ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME)
# Look at a point 3 meters in front and 4 meters to the left.
# We are not specifying a hand location, the robot will pick one.
gaze_target_in_odom = odom_T_flat_body.transform_point(x=3.0,
y=4.0,
z=0)
gaze_command = RobotCommandBuilder.arm_gaze_command(
gaze_target_in_odom[0], gaze_target_in_odom[1],
gaze_target_in_odom[2], ODOM_FRAME_NAME)
# Make the open gripper RobotCommand
gripper_command = RobotCommandBuilder.claw_gripper_open_command()
# Combine the arm and gripper commands into one RobotCommand
synchro_command = RobotCommandBuilder.build_synchro_command(
gripper_command, gaze_command)
# Send the request
robot.logger.info("Requesting gaze.")
command_client.robot_command(synchro_command)
time.sleep(4.0)
# Look to the left and the right with the hand.
# Robot's frame is X+ forward, Z+ up, so left and right is +/- in Y.
x = 4.0 # look 4 meters ahead
start_y = 2.0
end_y = -2.0
z = 0 # Look ahead, not up or down
traj_time = 5.5 # take 5.5 seconds to look from left to right.
start_pos_in_odom_tuple = odom_T_flat_body.transform_point(
x=x, y=start_y, z=z)
start_pos_in_odom = geometry_pb2.Vec3(x=start_pos_in_odom_tuple[0],
y=start_pos_in_odom_tuple[1],
z=start_pos_in_odom_tuple[2])
end_pos_in_odom_tuple = odom_T_flat_body.transform_point(x=x,
y=end_y,
z=z)
end_pos_in_odom = geometry_pb2.Vec3(x=end_pos_in_odom_tuple[0],
y=end_pos_in_odom_tuple[1],
z=end_pos_in_odom_tuple[2])
# Create the trajectory points
point1 = trajectory_pb2.Vec3TrajectoryPoint(
point=start_pos_in_odom)
duration_seconds = int(traj_time)
duration_nanos = int((traj_time - duration_seconds) * 1e9)
point2 = trajectory_pb2.Vec3TrajectoryPoint(
point=end_pos_in_odom,
time_since_reference=duration_pb2.Duration(
seconds=duration_seconds, nanos=duration_nanos))
# Build the trajectory proto
traj_proto = trajectory_pb2.Vec3Trajectory(points=[point1, point2])
# Build the proto
gaze_cmd = arm_command_pb2.GazeCommand.Request(
target_trajectory_in_frame1=traj_proto,
frame1_name=ODOM_FRAME_NAME,
frame2_name=ODOM_FRAME_NAME)
arm_command = arm_command_pb2.ArmCommand.Request(
arm_gaze_command=gaze_cmd)
synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request(
arm_command=arm_command)
command = robot_command_pb2.RobotCommand(
synchronized_command=synchronized_command)
# Make the open gripper RobotCommand
gripper_command = RobotCommandBuilder.claw_gripper_open_command()
# Combine the arm and gripper commands into one RobotCommand
synchro_command = RobotCommandBuilder.build_synchro_command(
gripper_command, command)
# Send the request
gaze_cmd_id = command_client.robot_command(command)
robot.logger.info('Sending gaze trajectory.')
# Wait until the robot completes the gaze before issuing the next command.
block_until_arm_arrives(command_client,
gaze_cmd_id,
timeout_sec=traj_time + 3.0)
# ------------- #
# Now make a gaze trajectory that moves the hand around while maintaining the gaze.
# We'll use the same trajectory as before, but add a trajectory for the hand to move to.
# Hand will start to the left (while looking left) and move to the right.
hand_x = 0.75 # in front of the robot.
hand_y = 0 # centered
hand_z_start = 0 # body height
hand_z_end = 0.25 # above body height
hand_vec3_start = geometry_pb2.Vec3(x=hand_x,
y=hand_y,
z=hand_z_start)
hand_vec3_end = geometry_pb2.Vec3(x=hand_x, y=hand_y, z=hand_z_end)
# We specify an orientation for the hand, which the robot will use its remaining degree
# of freedom to achieve. Most of it will be ignored in favor of the gaze direction.
qw = 1
qx = 0
qy = 0
qz = 0
quat = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz)
# Build a trajectory
hand_pose1_in_flat_body = geometry_pb2.SE3Pose(
position=hand_vec3_start, rotation=quat)
hand_pose2_in_flat_body = geometry_pb2.SE3Pose(
position=hand_vec3_end, rotation=quat)
hand_pose1_in_odom = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
hand_pose1_in_flat_body)
hand_pose2_in_odom = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
hand_pose2_in_flat_body)
traj_point1 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose1_in_odom.to_proto())
# We'll make this trajectory the same length as the one above.
traj_point2 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose2_in_odom.to_proto(),
time_since_reference=duration_pb2.Duration(
seconds=duration_seconds, nanos=duration_nanos))
hand_traj = trajectory_pb2.SE3Trajectory(
points=[traj_point1, traj_point2])
# Build the proto
gaze_cmd = arm_command_pb2.GazeCommand.Request(
target_trajectory_in_frame1=traj_proto,
frame1_name=ODOM_FRAME_NAME,
tool_trajectory_in_frame2=hand_traj,
frame2_name=ODOM_FRAME_NAME)
arm_command = arm_command_pb2.ArmCommand.Request(
arm_gaze_command=gaze_cmd)
synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request(
arm_command=arm_command)
command = robot_command_pb2.RobotCommand(
synchronized_command=synchronized_command)
# Make the open gripper RobotCommand
gripper_command = RobotCommandBuilder.claw_gripper_open_command()
# Combine the arm and gripper commands into one RobotCommand
synchro_command = RobotCommandBuilder.build_synchro_command(
gripper_command, command)
# Send the request
gaze_cmd_id = command_client.robot_command(synchro_command)
robot.logger.info('Sending gaze trajectory with hand movement.')
# Wait until the robot completes the gaze before powering off.
block_until_arm_arrives(command_client,
gaze_cmd_id,
timeout_sec=traj_time + 3.0)
# Power the robot off. By specifying "cut_immediately=False", a safe power off command
# is issued to the robot. This will attempt to sit the robot before powering off.
robot.power_off(cut_immediately=False, timeout_sec=20)
assert not robot.is_powered_on(), "Robot power off failed."
robot.logger.info("Robot safely powered off.")
finally:
# If we successfully acquired a lease, return it.
lease_client.return_lease(lease)
def test_edit_timestamps():
def _set_new_time(key, proto):
"""If proto has a field named key, fill set it to end_time_secs as robot time. """
if key not in proto.DESCRIPTOR.fields_by_name:
return # No such field in the proto to be set to the end-time.
end_time = getattr(proto, key)
end_time.CopyFrom(timestamp_pb2.Timestamp(seconds=10))
command = robot_command_pb2.RobotCommand()
command.synchronized_command.arm_command.arm_cartesian_command.root_frame_name = "test"
command.synchronized_command.arm_command.arm_cartesian_command.pose_trajectory_in_task.reference_time.seconds = 25
command.synchronized_command.arm_command.arm_cartesian_command.wrench_trajectory_in_task.reference_time.seconds = 25
_edit_proto(command, EDIT_TREE_CONVERT_LOCAL_TIME_TO_ROBOT_TIME, _set_new_time)
assert command.synchronized_command.arm_command.arm_cartesian_command.pose_trajectory_in_task.reference_time.seconds == 10
assert command.synchronized_command.arm_command.arm_cartesian_command.wrench_trajectory_in_task.reference_time.seconds == 10
assert command.synchronized_command.arm_command.arm_cartesian_command.root_frame_name == "test"
command = robot_command_pb2.RobotCommand()
command.synchronized_command.arm_command.arm_joint_move_command.trajectory.maximum_velocity.value = 1
command.synchronized_command.arm_command.arm_joint_move_command.trajectory.reference_time.seconds = 25
_edit_proto(command, EDIT_TREE_CONVERT_LOCAL_TIME_TO_ROBOT_TIME, _set_new_time)
assert command.synchronized_command.arm_command.arm_joint_move_command.trajectory.reference_time.seconds == 10
assert command.synchronized_command.arm_command.arm_joint_move_command.trajectory.maximum_velocity.value == 1
command = robot_command_pb2.RobotCommand()
command.synchronized_command.arm_command.arm_gaze_command.frame1_name = "test"
command.synchronized_command.arm_command.arm_gaze_command.target_trajectory_in_frame1.reference_time.seconds = 25
command.synchronized_command.arm_command.arm_gaze_command.tool_trajectory_in_frame2.reference_time.seconds = 25
_edit_proto(command, EDIT_TREE_CONVERT_LOCAL_TIME_TO_ROBOT_TIME, _set_new_time)
assert command.synchronized_command.arm_command.arm_gaze_command.target_trajectory_in_frame1.reference_time.seconds == 10
assert command.synchronized_command.arm_command.arm_gaze_command.tool_trajectory_in_frame2.reference_time.seconds == 10
assert command.synchronized_command.arm_command.arm_gaze_command.frame1_name == "test"
command = robot_command_pb2.RobotCommand()
command.synchronized_command.gripper_command.claw_gripper_command.maximum_torque.value = 10
command.synchronized_command.gripper_command.claw_gripper_command.trajectory.reference_time.seconds = 25
_edit_proto(command, EDIT_TREE_CONVERT_LOCAL_TIME_TO_ROBOT_TIME, _set_new_time)
assert command.synchronized_command.gripper_command.claw_gripper_command.trajectory.reference_time.seconds == 10
assert command.synchronized_command.gripper_command.claw_gripper_command.maximum_torque.value == 10
command = robot_command_pb2.RobotCommand()
command.synchronized_command.mobility_command.se2_trajectory_request.trajectory.reference_time.seconds = 25
_edit_proto(command, EDIT_TREE_CONVERT_LOCAL_TIME_TO_ROBOT_TIME, _set_new_time)
assert command.synchronized_command.mobility_command.se2_trajectory_request.trajectory.reference_time.seconds == 10
command = robot_command_pb2.RobotCommand()
command.synchronized_command.mobility_command.se2_trajectory_request.end_time.seconds = 25
_edit_proto(command, END_TIME_EDIT_TREE, _set_new_time)
assert command.synchronized_command.mobility_command.se2_trajectory_request.end_time.seconds == 10
command = robot_command_pb2.RobotCommand()
command.synchronized_command.mobility_command.se2_velocity_request.end_time.seconds = 25
_edit_proto(command, END_TIME_EDIT_TREE, _set_new_time)
assert command.synchronized_command.mobility_command.se2_velocity_request.end_time.seconds == 10
command = robot_command_pb2.RobotCommand()
command.synchronized_command.mobility_command.stance_request.end_time.seconds = 25
_edit_proto(command, END_TIME_EDIT_TREE, _set_new_time)
assert command.synchronized_command.mobility_command.stance_request.end_time.seconds == 10
command = robot_command_pb2.RobotCommand()
command.synchronized_command.arm_command.arm_velocity_command.end_time.seconds = 25
_edit_proto(command, END_TIME_EDIT_TREE, _set_new_time)
assert command.synchronized_command.arm_command.arm_velocity_command.end_time.seconds == 10
def hello_arm(config):
"""A simple example of using the Boston Dynamics API to command Spot's arm and body at the same time.
Please be aware that this demo causes the robot to walk and move its arm. You can have some
control over how much the robot moves -- see _L_ROBOT_SQUARE and _L_ARM_CIRCLE -- but regardless, the
robot should have at least (_L_ROBOT_SQUARE + 3) m of space in each direction when this demo is used."""
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('HelloSpotClient')
robot = sdk.create_robot(config.hostname)
robot.authenticate(config.username, config.password)
robot.time_sync.wait_for_sync()
assert robot.has_arm(), "Robot requires an arm to run this example."
# Verify the robot is not estopped and that an external application has registered and holds
# an estop endpoint.
assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \
"such as the estop SDK example, to configure E-Stop."
lease_client = robot.ensure_client(
bosdyn.client.lease.LeaseClient.default_service_name)
lease = lease_client.acquire()
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
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.
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(1.0)
# Unstow the arm
# Build the unstow command using RobotCommandBuilder
unstow = RobotCommandBuilder.arm_ready_command()
# Issue the command via the RobotCommandClient
command_client.robot_command(unstow)
robot.logger.info("Unstow command issued.")
time.sleep(1.0)
# Get robot pose in vision frame from robot state (we want to send commands in vision
# frame relative to where the robot stands now)
robot_state = robot_state_client.get_robot_state()
vision_T_world = get_vision_tform_body(
robot_state.kinematic_state.transforms_snapshot)
# In this demo, the robot will walk in a square while moving its arm in a circle.
# There are some parameters that you can set below:
# Initialize a robot command message, which we will build out below
command = robot_command_pb2.RobotCommand()
# points in the square
x_vals = np.array([0, 1, 1, 0, 0])
y_vals = np.array([0, 0, 1, 1, 0])
# duration in seconds for each move
seconds_arm = _SECONDS_FULL / (_N_POINTS + 1)
seconds_body = _SECONDS_FULL / x_vals.size
# Commands will be sent in the visual odometry ("vision") frame
frame_name = VISION_FRAME_NAME
# Build an arm trajectory by assembling points (in meters)
# x will be the same for each point
x = _L_ROBOT_SQUARE + 0.5
for ii in range(_N_POINTS + 1):
# Get coordinates relative to the robot's body
y = (_L_ROBOT_SQUARE / 2) - _L_ARM_CIRCLE * (np.cos(
2 * ii * math.pi / _N_POINTS))
z = _VERTICAL_SHIFT + _L_ARM_CIRCLE * (np.sin(
2 * ii * math.pi / _N_POINTS))
# Using the transform we got earlier, transform the points into the world frame
x_ewrt_vision, y_ewrt_vision, z_ewrt_vision = vision_T_world.transform_point(
x, y, z)
# Add a new point to the robot command's arm cartesian command se3 trajectory
# This will be an se3 trajectory point
point = command.synchronized_command.arm_command.arm_cartesian_command.pose_trajectory_in_task.points.add(
)
# Populate this point with the desired position, rotation, and duration information
point.pose.position.x = x_ewrt_vision
point.pose.position.y = y_ewrt_vision
point.pose.position.z = z_ewrt_vision
point.pose.rotation.x = vision_T_world.rot.x
point.pose.rotation.y = vision_T_world.rot.y
point.pose.rotation.z = vision_T_world.rot.z
point.pose.rotation.w = vision_T_world.rot.w
traj_time = (ii + 1) * seconds_arm
duration = seconds_to_duration(traj_time)
point.time_since_reference.CopyFrom(duration)
# set the frame for the hand trajectory
command.synchronized_command.arm_command.arm_cartesian_command.root_frame_name = frame_name
# Build a body se2trajectory by first assembling points
for ii in range(x_vals.size):
# Pull the point in the square relative to the robot and scale according to param
x = _L_ROBOT_SQUARE * x_vals[ii]
y = _L_ROBOT_SQUARE * y_vals[ii]
# Transform desired position into world frame
x_ewrt_vision, y_ewrt_vision, z_ewrt_vision = vision_T_world.transform_point(
x, y, 0)
# Add a new point to the robot command's arm cartesian command se3 trajectory
# This will be an se2 trajectory point
point = command.synchronized_command.mobility_command.se2_trajectory_request.trajectory.points.add(
)
# Populate this point with the desired position, angle, and duration information
point.pose.position.x = x_ewrt_vision
point.pose.position.y = y_ewrt_vision
point.pose.angle = vision_T_world.rot.to_yaw()
traj_time = (ii + 1) * seconds_body
duration = seconds_to_duration(traj_time)
point.time_since_reference.CopyFrom(duration)
# set the frame for the body trajectory
command.synchronized_command.mobility_command.se2_trajectory_request.se2_frame_name = frame_name
# Constrain the robot not to turn, forcing it to strafe laterally.
speed_limit = SE2VelocityLimit(
max_vel=SE2Velocity(linear=Vec2(x=2, y=2), angular=0),
min_vel=SE2Velocity(linear=Vec2(x=-2, y=-2), angular=0))
mobility_params = spot_command_pb2.MobilityParams(
vel_limit=speed_limit)
command.synchronized_command.mobility_command.params.CopyFrom(
RobotCommandBuilder._to_any(mobility_params))
# Send the command using the command client
# The SE2TrajectoryRequest requires an end_time, which is set
# during the command client call
robot.logger.info("Sending arm and body trajectory commands.")
command_client.robot_command(command,
end_time_secs=time.time() +
_SECONDS_FULL)
time.sleep(_SECONDS_FULL + 2)
# 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 arm_trajectory(config):
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('ArmTrajectory')
robot = sdk.create_robot(config.hostname)
robot.authenticate(config.username, config.password)
robot.time_sync.wait_for_sync()
assert robot.has_arm(), "Robot requires an arm to run this example."
# Verify the robot is not estopped and that an external application has registered and holds
# an estop endpoint.
assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \
"such as the estop SDK example, to configure E-Stop."
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
lease_client = robot.ensure_client(
bosdyn.client.lease.LeaseClient.default_service_name)
lease = lease_client.acquire()
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
# Now, we are ready to power on the robot. This call will block until the power
# is on. Commands would fail if this did not happen. We can also check that the robot is
# powered at any point.
robot.logger.info(
"Powering on robot... This may take a several seconds.")
robot.power_on(timeout_sec=20)
assert robot.is_powered_on(), "Robot power on failed."
robot.logger.info("Robot powered on.")
# Tell the robot to stand up. The command service is used to issue commands to a robot.
# The set of valid commands for a robot depends on hardware configuration. See
# SpotCommandHelper for more detailed examples on command building. The robot
# command service requires timesync between the robot and the client.
robot.logger.info("Commanding robot to stand...")
command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
blocking_stand(command_client, timeout_sec=10)
robot.logger.info("Robot standing.")
time.sleep(2.0)
# Move the arm along a simple trajectory
vo_T_flat_body = get_a_tform_b(
robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot, VISION_FRAME_NAME,
GRAV_ALIGNED_BODY_FRAME_NAME)
x = 0.75 # a reasonable position in front of the robot
y1 = 0 # centered
y2 = 0.4 # 0.4 meters to the robot's left
y3 = -0.4 # 0.4 meters to the robot's right
z = 0 # at the body's height
# Use the same rotation as the robot's body.
rotation = math_helpers.Quat()
t_first_point = 0 # first point starts at t = 0 for the trajectory.
t_second_point = 4.0 # trajectory will last 1.0 seconds
t_third_point = 8.0 # trajectory will last 1.0 seconds
# Build the two points in the trajectory
hand_pose1 = math_helpers.SE3Pose(x=x, y=y1, z=z, rot=rotation)
hand_pose2 = math_helpers.SE3Pose(x=x, y=y2, z=z, rot=rotation)
hand_pose3 = math_helpers.SE3Pose(x=x, y=y3, z=z, rot=rotation)
# Build the points by combining the pose and times into protos.
traj_point1 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose1.to_proto(),
time_since_reference=seconds_to_duration(t_first_point))
traj_point2 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose2.to_proto(),
time_since_reference=seconds_to_duration(t_second_point))
traj_point3 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose3.to_proto(),
time_since_reference=seconds_to_duration(t_third_point))
# Build the trajectory proto by combining the two points
hand_traj = trajectory_pb2.SE3Trajectory(
points=[traj_point1, traj_point2, traj_point3])
# Build the command by taking the trajectory and specifying the frame it is expressed
# in.
#
# In this case, we want to specify the trajectory in the body's frame, so we set the
# root frame name to the the flat body frame.
arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request(
pose_trajectory_in_task=hand_traj,
root_frame_name=GRAV_ALIGNED_BODY_FRAME_NAME)
# Pack everything up in protos.
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)
# Keep the gripper closed the whole time.
robot_command = RobotCommandBuilder.claw_gripper_open_fraction_command(
0, build_on_command=robot_command)
robot.logger.info("Sending trajectory command...")
# Send the trajectory to the robot.
cmd_id = command_client.robot_command(robot_command)
# Wait until the arm arrives at the goal.
while True:
feedback_resp = command_client.robot_command_feedback(cmd_id)
robot.logger.info(
'Distance to final point: ' + '{:.2f} meters'.format(
feedback_resp.feedback.synchronized_feedback.
arm_command_feedback.arm_cartesian_feedback.
measured_pos_distance_to_goal) +
', {:.2f} radians'.format(
feedback_resp.feedback.synchronized_feedback.
arm_command_feedback.arm_cartesian_feedback.
measured_rot_distance_to_goal))
if feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.status == arm_command_pb2.ArmCartesianCommand.Feedback.STATUS_TRAJECTORY_COMPLETE:
robot.logger.info('Move complete.')
break
time.sleep(0.1)
# 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)
