def run_docking(config):
"""A simple example of using the Boston Dynamics API to use the docking service.
Copied from the hello_spot.py example"""
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('DockingClient')
robot = sdk.create_robot(config.hostname)
robot.authenticate(config.username, config.password)
robot.time_sync.wait_for_sync()
lease_client = robot.ensure_client(bosdyn.client.lease.LeaseClient.default_service_name)
# To steal the lease on a running robot you'd like to dock, change this to `lease_client.take()`
lease = lease_client.acquire()
command_client = robot.ensure_client(robot_command.RobotCommandClient.default_service_name)
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
# make sure we're powered on and standing
robot.power_on()
robot_command.blocking_stand(command_client)
# Dock the robot
blocking_dock_robot(robot, config.dock_id)
print("Docking Success")
finally:
lease_client.return_lease(lease)
def connect(self,
app_token,
hostname,
username,
password,
initialize_motion=True):
# Create robot object.
sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster')
sdk.load_app_token(app_token)
robot = sdk.create_robot(hostname)
robot.authenticate(username, password)
# Check that an estop is connected with the robot so that the robot commands can be executed.
verify_estop(robot)
# Create the lease client.
lease_client = robot.ensure_client(LeaseClient.default_service_name)
lease = lease_client.acquire()
robot.time_sync.wait_for_sync()
lk = bosdyn.client.lease.LeaseKeepAlive(lease_client)
# Setup clients for the robot state and robot command services.
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
robot_command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
# Power on the robot and stand it up.
robot.power_on()
if initialize_motion:
blocking_stand(robot_command_client)
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 stand(robot):
"""Stand robot.
Args:
robot: (Robot) Interface to Spot 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.")
def stand_up(self):
"""
Ask Spot to stand up
"""
if self._robot is None:
return
if self._prep_for_motion():
command_client = self._robot.ensure_client(
RobotCommandClient.default_service_name)
blocking_stand(command_client, timeout_sec=10)
def run_camera_calibration_routine(robot):
# Take lease.
lease_client = robot.ensure_client(LeaseClient.default_service_name)
lease = lease_client.take()
with LeaseKeepAlive(lease_client):
# Power on the robot.
robot.power_on(timeout_sec=POWER_TIMEOUT)
# Command the robot to stand.
command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
blocking_stand(command_client, timeout_sec=STAND_TIMEOUT)
# Return this lease and get a new one.
lease_client.return_lease(lease)
lease = lease_client.take()
# Run camera calibration.
spot_check_client = robot.ensure_client(
SpotCheckClient.default_service_name)
run_camera_calibration(spot_check_client, lease, verbose=True)
robot.logger.info("CameraCaliration ran with no errors!")
lease_client.return_lease(lease)
def hello_arm(config):
"""A simple example of using the Boston Dynamics API to command Spot's arm."""
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('StowUnstowClient')
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.
verify_estop(robot)
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)
# Stow the arm
# Build the stow command using RobotCommandBuilder
stow = RobotCommandBuilder.arm_stow_command()
# Issue the command via the RobotCommandClient
command_client.robot_command(stow)
robot.logger.info("Stow command issued.")
time.sleep(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 main():
'''Replay stored mission'''
body_lease = None
# Configure logging
bosdyn.client.util.setup_logging()
# Parse command-line arguments
parser = argparse.ArgumentParser()
bosdyn.client.util.add_common_arguments(parser)
# If the map directory is omitted, we assume that everything the user wants done is encoded in
# the mission itself.
parser.add_argument('--map_directory', nargs='?', help='Optional path to map directory')
parser.add_argument('--mission', dest='mission_file', help='Optional path to mission file')
parser.add_argument('--timeout', type=float, default=3.0, dest='timeout',
help='Mission client timeout (s).')
parser.add_argument('--noloc', action='store_true', default=False, dest='noloc',
help='Skip initial localization')
parser.add_argument('--disable_alternate_route_finding', action='store_true', default=False,
dest='disable_alternate_route_finding',
help='Disable creating alternate-route-finding graph structure')
group = parser.add_mutually_exclusive_group()
group.add_argument('--time', type=float, default=0.0, dest='duration',
help='Time to repeat mission (sec)')
group.add_argument('--static', action='store_true', default=False, dest='static_mode',
help='Stand, but do not run robot')
args = parser.parse_args()
# Use the optional map_directory argument as a proxy for these other tasks we normally do.
do_localization = (args.map_directory is not None) and (not args.noloc)
do_map_load = args.map_directory is not None
fail_on_question = args.map_directory is not None
if not args.mission_file:
if not args.map_directory:
raise Exception('Must specify at least one of map_directory or --mission.')
args.mission_file = os.path.join(args.map_directory, 'missions', 'autogenerated')
print('[ REPLAYING MISSION {} : MAP {} : HOSTNAME {} ]'.format(args.mission_file,
args.map_directory,
args.hostname))
# Initialize robot object
robot = init_robot(args.hostname, args.username, args.password)
# Acquire robot lease
robot.logger.info('Acquiring lease...')
lease_client = robot.ensure_client(bosdyn.client.lease.LeaseClient.default_service_name)
body_lease = lease_client.acquire()
if body_lease is None:
raise Exception('Lease not acquired.')
robot.logger.info('Lease acquired: %s', str(body_lease))
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
# Initialize clients
robot_state_client, command_client, mission_client, graph_nav_client = init_clients(
robot, body_lease, args.mission_file, args.map_directory, do_map_load,
args.disable_alternate_route_finding)
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."
# Ensure robot is powered on
assert ensure_power_on(robot), 'Robot power on failed.'
# Stand up and wait for the perception system to stabilize
robot.logger.info('Commanding robot to stand...')
blocking_stand(command_client, timeout_sec=10)
countdown(5)
robot.logger.info('Robot standing.')
# Localize robot
localization_error = False
if do_localization:
graph = graph_nav_client.download_graph()
robot.logger.info('Localizing robot...')
robot_state = robot_state_client.get_robot_state()
localization = nav_pb2.Localization()
# Attempt to localize using any visible fiducial
graph_nav_client.set_localization(
initial_guess_localization=localization, ko_tform_body=None, max_distance=None,
max_yaw=None,
fiducial_init=graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NEAREST)
# Run mission
if not args.static_mode and not localization_error:
if args.duration == 0.0:
run_mission(robot, mission_client, lease_client, fail_on_question, args.timeout)
else:
repeat_mission(robot, mission_client, lease_client, args.duration,
fail_on_question, args.timeout)
finally:
# Ensure robot is powered off
lease_client.lease_wallet.advance()
robot.logger.info('Powering off...')
power_off_success = ensure_power_off(robot)
# Return lease
robot.logger.info('Returning lease...')
lease_client.return_lease(body_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 run(config):
"""Testing API Stance
This example will cause the robot to power on, stand and reposition its feet (Stance) at the
location it's already standing at.
* Use sw-estop running on tablet/python etc.
* Have ~1m of free space all around the robot
* Ctrl-C to exit and return lease.
"""
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('StanceClient')
robot = sdk.create_robot(config.hostname)
robot.authenticate(config.username, config.password)
robot.time_sync.wait_for_sync()
# Acquire lease
lease_client = robot.ensure_client(
bosdyn.client.lease.LeaseClient.default_service_name)
lease = lease_client.acquire()
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
state = robot_state_client.get_robot_state()
# This example ues the current body position, but you can specify any position.
# A common use is to specify it relative to something you know, like a fiducial.
vo_T_body = frame_helpers.get_se2_a_tform_b(
state.kinematic_state.transforms_snapshot,
frame_helpers.VISION_FRAME_NAME,
frame_helpers.GRAV_ALIGNED_BODY_FRAME_NAME)
# Power On
robot.power_on()
assert robot.is_powered_on(), "Robot power on failed."
# Stand
robot_command.blocking_stand(command_client)
#### Example stance offsets from body position. ####
x_offset = config.x_offset
y_offset = config.y_offset
pos_fl_rt_vision = vo_T_body * math_helpers.SE2Pose(
x_offset, y_offset, 0)
pos_fr_rt_vision = vo_T_body * math_helpers.SE2Pose(
x_offset, -y_offset, 0)
pos_hl_rt_vision = vo_T_body * math_helpers.SE2Pose(
-x_offset, y_offset, 0)
pos_hr_rt_vision = vo_T_body * math_helpers.SE2Pose(
-x_offset, -y_offset, 0)
stance_cmd = RobotCommandBuilder.stance_command(
frame_helpers.VISION_FRAME_NAME, pos_fl_rt_vision.position,
pos_fr_rt_vision.position, pos_hl_rt_vision.position,
pos_hr_rt_vision.position)
print(
"After stance adjustment, press Ctrl-C to sit Spot and turn off motors."
)
while True:
# Update end time
stance_cmd.synchronized_command.mobility_command.stance_request.end_time.CopyFrom(
robot.time_sync.robot_timestamp_from_local_secs(
time.time() + 5))
# Send the command
command_client.robot_command(stance_cmd)
time.sleep(1)
finally:
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)
def main():
import argparse
parser = argparse.ArgumentParser()
bosdyn.client.util.add_common_arguments(parser)
options = parser.parse_args()
# Create robot object.
sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster')
sdk.load_app_token(options.app_token)
robot = sdk.create_robot(options.hostname)
robot.authenticate(options.username, options.password)
# Check that an estop is connected with the robot so that the robot commands can be executed.
verify_estop(robot)
# Create the lease client.
lease_client = robot.ensure_client(LeaseClient.default_service_name)
lease = lease_client.acquire()
robot.time_sync.wait_for_sync()
lk = bosdyn.client.lease.LeaseKeepAlive(lease_client)
# Setup clients for the robot state and robot command services.
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
robot_command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
# Power on the robot and stand it up.
robot.power_on()
blocking_stand(robot_command_client)
# Get robot state information. Specifically, we are getting the vision_tform_body transform to understand
# the robot's current position in the vision frame.
vision_tform_body = get_vision_tform_body(
robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot)
# We want to command a trajectory to go forward one meter in the x-direction of the body.
# It is simple to define this trajectory relative to the body frame, since we know that will be
# just 1 meter forward in the x-axis of the body.
# Note that the rotation is just math_helpers.Quat(), which is the identity quaternion. We want the
# rotation of the body at the goal to match the rotation of the body currently, so we do not need
# to transform the rotation.
body_tform_goal = math_helpers.SE3Pose(x=1,
y=0,
z=0,
rot=math_helpers.Quat())
# We can then transform this transform to get the goal position relative to the vision frame.
vision_tform_goal = vision_tform_body * body_tform_goal
# Command the robot to go to the goal point in the vision frame. The command will stop at the new
# position in the vision frame.
robot_cmd = RobotCommandBuilder.trajectory_command(
goal_x=vision_tform_goal.x,
goal_y=vision_tform_goal.y,
goal_heading=vision_tform_goal.rot.to_yaw(),
frame_name=VISION_FRAME_NAME)
end_time = 2.0
robot_command_client.robot_command(lease=None,
command=robot_cmd,
end_time_secs=time.time() + end_time)
time.sleep(end_time)
# Get new robot state information after moving the robot. Here we are getting the transform odom_tform_body,
# which describes the robot body's position in the odom frame.
odom_tform_body = get_odom_tform_body(robot_state_client.get_robot_state().
kinematic_state.transforms_snapshot)
# We want to command a trajectory to go backwards one meter and to the left one meter.
# It is simple to define this trajectory relative to the body frame, since we know that will be
# just 1 meter backwards (negative-value) in the x-axis of the body and one meter left (positive-value)
# in the y-axis of the body.
body_tform_goal = math_helpers.SE3Pose(x=-1,
y=1,
z=0,
rot=math_helpers.Quat())
# We can then transform this transform to get the goal position relative to the odom frame.
odom_tform_goal = odom_tform_body * body_tform_goal
# Command the robot to go to the goal point in the odom frame. The command will stop at the new
# position in the odom frame.
robot_cmd = RobotCommandBuilder.trajectory_command(
goal_x=odom_tform_goal.x,
goal_y=odom_tform_goal.y,
goal_heading=odom_tform_goal.rot.to_yaw(),
frame_name=ODOM_FRAME_NAME)
end_time = 5.0
robot_command_client.robot_command(lease=None,
command=robot_cmd,
end_time_secs=time.time() + end_time)
time.sleep(end_time)
return True
def manual_control_loop(self):
# Create robot object.
if self.config.norobot:
robot_command_client = None
robot_state_client = None
else:
sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster')
sdk.load_app_token(self.config.app_token)
robot = sdk.create_robot(self.config.hostname)
robot.authenticate(self.config.username, self.config.password)
# Check that an estop is connected with the robot so that the robot commands can be executed.
verify_estop(robot)
# Create the lease client.
lease_client = robot.ensure_client(
LeaseClient.default_service_name)
lease = lease_client.acquire()
robot.time_sync.wait_for_sync()
lk = bosdyn.client.lease.LeaseKeepAlive(lease_client)
# Setup clients for the robot state and robot command services.
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
robot_command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
# Power on the robot and stand it up.
if not self.config.nopower:
robot.power_on()
blocking_stand(robot_command_client)
# Initialize camera
camera_pipeline = camera.init_camera()
# Print pose
self.get_pose(robot_state_client)
is_server_on = False
# Control loop
while True:
cmd = input('Command (wasd/qe/c/r):')
if cmd == 'c':
break
elif cmd == 'r':
print("Remote control..")
try:
self.reset_last_frame()
if is_server_on:
asyncio.get_event_loop().run_forever()
else:
is_server_on = True
self.remote_control_loop(
robot_command_client,
robot_state_client,
camera_pipeline=camera_pipeline,
dummy=(robot_command_client is None))
except KeyboardInterrupt:
print("ending remote control")
pass
else:
self.reset_last_frame()
pos = self.execute_command(robot_command_client,
robot_state_client,
cmd,
wait_to_stabilize=False)
# move_relative(robot_command_client, robot_state_client, timeout=5.0)
robot_cmd = RobotCommandBuilder.safe_power_off_command()
robot_command_client.robot_command(lease=None,
command=robot_cmd,
end_time_secs=time.time() + 2.)
time.sleep(2.)
return True
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 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 main():
import argparse
parser = argparse.ArgumentParser()
bosdyn.client.util.add_common_arguments(parser)
parser.add_argument('--dx',
default=0,
type=float,
help='Position offset in body frame (meters forward).')
parser.add_argument('--dy',
default=0,
type=float,
help='Position offset in body frame (meters left).')
parser.add_argument('--dyaw',
default=0,
type=float,
help='Position offset in body frame (degrees ccw).')
parser.add_argument('--frame',
choices=[VISION_FRAME_NAME, ODOM_FRAME_NAME],
default=ODOM_FRAME_NAME,
help='Send the command in this frame.')
parser.add_argument('--stairs',
action='store_true',
help='Move the robot in stairs mode.')
options = parser.parse_args()
# Create robot object.
sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster')
robot = sdk.create_robot(options.hostname)
robot.authenticate(options.username, options.password)
# Check that an estop is connected with the robot so that the robot commands can be executed.
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."
# Create the lease client.
lease_client = robot.ensure_client(LeaseClient.default_service_name)
# Setup clients for the robot state and robot command services.
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
robot_command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
lease_client.acquire()
with LeaseKeepAlive(lease_client, return_at_exit=True):
# Power on the robot and stand it up.
robot.time_sync.wait_for_sync()
robot.power_on()
blocking_stand(robot_command_client)
try:
return relative_move(options.dx,
options.dy,
math.radians(options.dyaw),
options.frame,
robot_command_client,
robot_state_client,
stairs=options.stairs)
finally:
# Send a Stop at the end, regardless of what happened.
robot_command_client.robot_command(
RobotCommandBuilder.stop_command())
def arm_with_body_follow(config):
"""A simple example that moves the arm and asks the body to move to a good position based
on the arm's location."""
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('ArmWithBodyFollowClient')
robot = sdk.create_robot(config.hostname)
robot.authenticate(config.username, config.password)
robot.time_sync.wait_for_sync()
assert robot.has_arm(), "Robot requires an arm to run this example."
# Verify the robot is not estopped and that an external application has registered and holds
# an estop endpoint.
assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \
"such as the estop SDK example, to configure E-Stop."
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
lease_client = robot.ensure_client(
bosdyn.client.lease.LeaseClient.default_service_name)
lease = lease_client.acquire()
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
# Now, we are ready to power on the robot. This call will block until the power
# is on. Commands would fail if this did not happen. We can also check that the robot is
# powered at any point.
robot.logger.info(
"Powering on robot... This may take a several seconds.")
robot.power_on(timeout_sec=20)
assert robot.is_powered_on(), "Robot power on failed."
robot.logger.info("Robot powered on.")
# Tell the robot to stand up. The command service is used to issue commands to a robot.
# The set of valid commands for a robot depends on hardware configuration. See
# SpotCommandHelper for more detailed examples on command building. The robot
# command service requires timesync between the robot and the client.
robot.logger.info("Commanding robot to stand...")
command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
blocking_stand(command_client, timeout_sec=10)
robot.logger.info("Robot standing.")
time.sleep(2.0)
# Move the arm to a spot in front of the robot, and command the body to follow the hand.
# Build a position to move the arm to (in meters, relative to the body frame origin.)
x = 1.25
y = 0
z = 0.25
hand_pos_rt_body = geometry_pb2.Vec3(x=x, y=y, z=z)
# Rotation as a quaternion.
qw = 1
qx = 0
qy = 0
qz = 0
body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz)
# Build the SE(3) pose of the desired hand position in the moving body frame.
body_T_hand = geometry_pb2.SE3Pose(position=hand_pos_rt_body,
rotation=body_Q_hand)
# Transform the desired from the moving body frame to the odom frame.
robot_state = robot_state_client.get_robot_state()
odom_T_body = get_a_tform_b(
robot_state.kinematic_state.transforms_snapshot,
ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME)
odom_T_hand = odom_T_body * math_helpers.SE3Pose.from_obj(
body_T_hand)
# duration in seconds
seconds = 5
# Create the arm command.
arm_command = RobotCommandBuilder.arm_pose_command(
odom_T_hand.x, odom_T_hand.y, odom_T_hand.z, odom_T_hand.rot.w,
odom_T_hand.rot.x, odom_T_hand.rot.y, odom_T_hand.rot.z,
ODOM_FRAME_NAME, seconds)
# Tell the robot's body to follow the arm
follow_arm_command = RobotCommandBuilder.follow_arm_command()
# Combine the arm and mobility commands into one synchronized command.
command = RobotCommandBuilder.build_synchro_command(
follow_arm_command, arm_command)
# Send the request
command_client.robot_command(command)
robot.logger.info('Moving arm to position.')
time.sleep(6.0)
# Power the robot off. By specifying "cut_immediately=False", a safe power off command
# is issued to the robot. This will attempt to sit the robot before powering off.
robot.power_off(cut_immediately=False, timeout_sec=20)
assert not robot.is_powered_on(), "Robot power off failed."
robot.logger.info("Robot safely powered off.")
finally:
# If we successfully acquired a lease, return it.
lease_client.return_lease(lease)
def joint_move_example(config):
"""A simple example of using the Boston Dynamics API to command Spot's arm to perform joint moves."""
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('ArmJointMoveClient')
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.
verify_estop(robot)
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)
# Example 1: issue a single point trajectory without a time_since_reference in order to perform
# a minimum time joint move to the goal obeying the default acceleration and velocity limits.
sh0 = 0.0692
sh1 = -1.882
el0 = 1.652
el1 = -0.0691
wr0 = 1.622
wr1 = 1.550
traj_point = RobotCommandBuilder.create_arm_joint_trajectory_point(
sh0, sh1, el0, el1, wr0, wr1)
arm_joint_traj = arm_command_pb2.ArmJointTrajectory(
points=[traj_point])
# Make a RobotCommand
command = make_robot_command(arm_joint_traj)
# Send the request
cmd_id = command_client.robot_command(command)
robot.logger.info('Moving arm to position 1.')
# Query for feedback to determine how long the goto will take.
feedback_resp = command_client.robot_command_feedback(cmd_id)
robot.logger.info("Feedback for Example 1: single point goto")
time_to_goal = print_feedback(feedback_resp, robot.logger)
time.sleep(time_to_goal)
# Example 2: Single point trajectory with maximum acceleration/velocity constraints specified such
# that the solver has to modify the desired points to honor the constraints
sh0 = 0.0
sh1 = -2.0
el0 = 2.6
el1 = 0.0
wr0 = -0.6
wr1 = 0.0
max_vel = wrappers_pb2.DoubleValue(value=1)
max_acc = wrappers_pb2.DoubleValue(value=5)
traj_point = RobotCommandBuilder.create_arm_joint_trajectory_point(
sh0, sh1, el0, el1, wr0, wr1, time_since_reference_secs=1.5)
arm_joint_traj = arm_command_pb2.ArmJointTrajectory(
points=[traj_point],
maximum_velocity=max_vel,
maximum_acceleration=max_acc)
# Make a RobotCommand
command = make_robot_command(arm_joint_traj)
# Send the request
cmd_id = command_client.robot_command(command)
robot.logger.info(
'Requesting a single point trajectory with unsatisfiable constraints.'
)
# Query for feedback
feedback_resp = command_client.robot_command_feedback(cmd_id)
robot.logger.info(
"Feedback for Example 2: planner modifies trajectory")
time_to_goal = print_feedback(feedback_resp, robot.logger)
time.sleep(time_to_goal)
# Example 3: Single point trajectory with default acceleration/velocity constraints and
# time_since_reference_secs large enough such that the solver can plan a solution to the
# points that also satisfies the constraints.
sh0 = 0.0692
sh1 = -1.882
el0 = 1.652
el1 = -0.0691
wr0 = 1.622
wr1 = 1.550
traj_point = RobotCommandBuilder.create_arm_joint_trajectory_point(
sh0, sh1, el0, el1, wr0, wr1, time_since_reference_secs=1.5)
arm_joint_traj = arm_command_pb2.ArmJointTrajectory(
points=[traj_point])
# Make a RobotCommand
command = make_robot_command(arm_joint_traj)
# Send the request
cmd_id = command_client.robot_command(command)
robot.logger.info(
'Requesting a single point trajectory with satisfiable constraints.'
)
# Query for feedback
feedback_resp = command_client.robot_command_feedback(cmd_id)
robot.logger.info("Feedback for Example 3: unmodified trajectory")
time_to_goal = print_feedback(feedback_resp, robot.logger)
time.sleep(time_to_goal)
# ----- Do a two-point joint move trajectory ------
# First stow the arm.
# Build the stow command using RobotCommandBuilder
stow = RobotCommandBuilder.arm_stow_command()
# Issue the command via the RobotCommandClient
command_client.robot_command(stow)
robot.logger.info("Stow command issued.")
time.sleep(2.0)
# First point position
sh0 = -1.5
sh1 = -0.8
el0 = 1.7
el1 = 0.0
wr0 = 0.5
wr1 = 0.0
# First point time (seconds)
first_point_t = 2.0
# Build the proto for the trajectory point.
traj_point1 = RobotCommandBuilder.create_arm_joint_trajectory_point(
sh0, sh1, el0, el1, wr0, wr1, first_point_t)
# Second point position
sh0 = 1.0
sh1 = -0.2
el0 = 1.3
el1 = -1.3
wr0 = -1.5
wr1 = 1.5
# Second point time (seconds)
second_point_t = 4.0
# Build the proto for the second trajectory point.
traj_point2 = RobotCommandBuilder.create_arm_joint_trajectory_point(
sh0, sh1, el0, el1, wr0, wr1, second_point_t)
# Optionally, set the maximum allowable velocity in rad/s that a joint is allowed to
# travel at. Also set the maximum allowable acceleration in rad/s^2 that a joint is
# allowed to accelerate at. If these values are not set, a default will be used on
# the robot.
# Note that if these values are set too low and the trajectories are too aggressive
# in terms of time, the desired joint angles will not be hit at the specified time.
# Some other ways to help the robot actually hit the specified trajectory is to
# increase the time between trajectory points, or to only specify joint position
# goals without specifying velocity goals.
max_vel = wrappers_pb2.DoubleValue(value=2.5)
max_acc = wrappers_pb2.DoubleValue(value=15)
# Build up a proto.
arm_joint_traj = arm_command_pb2.ArmJointTrajectory(
points=[traj_point1, traj_point2],
maximum_velocity=max_vel,
maximum_acceleration=max_acc)
# Make a RobotCommand
command = make_robot_command(arm_joint_traj)
# Send the request
cmd_id = command_client.robot_command(command)
robot.logger.info('Moving arm along 2-point joint trajectory.')
# Query for feedback to determine exactly what the planned trajectory is.
feedback_resp = command_client.robot_command_feedback(cmd_id)
robot.logger.info("Feedback for 2-point joint trajectory")
print_feedback(feedback_resp, robot.logger)
# Wait until the move completes before powering off.
block_until_arm_arrives(command_client, cmd_id,
second_point_t + 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 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 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 walk_to_object(config):
"""Get an image and command the robot to walk up to a selected object.
We'll walk "up to" the object, not on top of it. The idea is that you
want to interact or manipulate the object."""
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('WalkToObjectClient')
robot = sdk.create_robot(config.hostname)
robot.authenticate(config.username, config.password)
robot.time_sync.wait_for_sync()
assert robot.has_arm(), "Robot requires an arm to run this example."
# Verify the robot is not estopped and that an external application has registered and holds
# an estop endpoint.
assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \
"such as the estop SDK example, to configure E-Stop."
lease_client = robot.ensure_client(
bosdyn.client.lease.LeaseClient.default_service_name)
image_client = robot.ensure_client(ImageClient.default_service_name)
manipulation_api_client = robot.ensure_client(
ManipulationApiClient.default_service_name)
lease = lease_client.acquire()
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
# Now, we are ready to power on the robot. This call will block until the power
# is on. Commands would fail if this did not happen. We can also check that the robot is
# powered at any point.
robot.logger.info(
"Powering on robot... This may take a several seconds.")
robot.power_on(timeout_sec=20)
assert robot.is_powered_on(), "Robot power on failed."
robot.logger.info("Robot powered on.")
# Tell the robot to stand up. The command service is used to issue commands to a robot.
# The set of valid commands for a robot depends on hardware configuration. See
# SpotCommandHelper for more detailed examples on command building. The robot
# command service requires timesync between the robot and the client.
robot.logger.info("Commanding robot to stand...")
command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
blocking_stand(command_client, timeout_sec=10)
robot.logger.info("Robot standing.")
time.sleep(2.0)
# Take a picture with a camera
robot.logger.info('Getting an image from: ' + config.image_source)
image_responses = image_client.get_image_from_sources(
[config.image_source])
if len(image_responses) != 1:
print('Got invalid number of images: ' +
str(len(image_responses)))
print(image_responses)
assert False
image = image_responses[0]
if image.shot.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_DEPTH_U16:
dtype = np.uint16
else:
dtype = np.uint8
img = np.fromstring(image.shot.image.data, dtype=dtype)
if image.shot.image.format == image_pb2.Image.FORMAT_RAW:
img = img.reshape(image.shot.image.rows, image.shot.image.cols)
else:
img = cv2.imdecode(img, -1)
# Show the image to the user and wait for them to click on a pixel
robot.logger.info('Click on an object to walk up to...')
image_title = 'Click to walk up to something'
cv2.namedWindow(image_title)
cv2.setMouseCallback(image_title, cv_mouse_callback)
global g_image_click, g_image_display
g_image_display = img
cv2.imshow(image_title, g_image_display)
while g_image_click is None:
key = cv2.waitKey(1) & 0xFF
if key == ord('q') or key == ord('Q'):
# Quit
print('"q" pressed, exiting.')
exit(0)
robot.logger.info('Walking to object at image location (' +
str(g_image_click[0]) + ', ' +
str(g_image_click[1]) + ')')
walk_vec = geometry_pb2.Vec2(x=g_image_click[0],
y=g_image_click[1])
# Optionally populate the offset distance parameter.
if config.distance is None:
offset_distance = None
else:
offset_distance = wrappers_pb2.FloatValue(
value=config.distance)
# Build the proto
walk_to = manipulation_api_pb2.WalkToObjectInImage(
pixel_xy=walk_vec,
transforms_snapshot_for_camera=image.shot.transforms_snapshot,
frame_name_image_sensor=image.shot.frame_name_image_sensor,
camera_model=image.source.pinhole,
offset_distance=offset_distance)
# Ask the robot to pick up the object
walk_to_request = manipulation_api_pb2.ManipulationApiRequest(
walk_to_object_in_image=walk_to)
# Send the request
cmd_response = manipulation_api_client.manipulation_api_command(
manipulation_api_request=walk_to_request)
# Get feedback from the robot
while True:
time.sleep(0.25)
feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest(
manipulation_cmd_id=cmd_response.manipulation_cmd_id)
# Send the request
response = manipulation_api_client.manipulation_api_feedback_command(
manipulation_api_feedback_request=feedback_request)
print(
'Current state: ',
manipulation_api_pb2.ManipulationFeedbackState.Name(
response.current_state))
if response.current_state == manipulation_api_pb2.MANIP_STATE_DONE:
break
robot.logger.info('Finished.')
time.sleep(4.0)
robot.logger.info('Sitting down and turning off.')
# Power the robot off. By specifying "cut_immediately=False", a safe power off command
# is issued to the robot. This will attempt to sit the robot before powering off.
robot.power_off(cut_immediately=False, timeout_sec=20)
assert not robot.is_powered_on(), "Robot power off failed."
robot.logger.info("Robot safely powered off.")
finally:
# If we successfully acquired a lease, return it.
lease_client.return_lease(lease)
def hello_spot(config):
"""A simple example of using the Boston Dynamics API to command a Spot robot."""
# 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('HelloSpotClient')
# 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 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."
# 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 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(3)
# Tell the robot to stand in a twisted position.
#
# The RobotCommandBuilder constructs command messages, which are then
# issued to the robot using "robot_command" on the command client.
#
# In this example, the RobotCommandBuilder generates a stand command
# message with a non-default rotation in the footprint frame. The footprint
# frame is a gravity aligned frame with its origin located at the geometric
# center of the feet. The X axis of the footprint frame points forward along
# the robot's length, the Z axis points up aligned with gravity, and the Y
# axis is the cross-product of the two.
footprint_R_body = bosdyn.geometry.EulerZXY(yaw=0.4, roll=0.0, pitch=0.0)
cmd = RobotCommandBuilder.synchro_stand_command(footprint_R_body=footprint_R_body)
command_client.robot_command(cmd)
robot.logger.info("Robot standing twisted.")
time.sleep(3)
# Now tell the robot to stand taller, using the same approach of constructing
# a command message with the RobotCommandBuilder and issuing it with
# robot_command.
cmd = RobotCommandBuilder.synchro_stand_command(body_height=0.1)
command_client.robot_command(cmd)
robot.logger.info("Robot standing tall.")
time.sleep(3)
# Capture an image.
# Spot has five sensors around the body. Each sensor consists of a stereo pair and a
# fisheye camera. The list_image_sources RPC gives a list of image sources which are
# available to the API client. Images are captured via calls to the get_image RPC.
# Images can be requested from multiple image sources in one call.
image_client = robot.ensure_client(ImageClient.default_service_name)
sources = image_client.list_image_sources()
image_response = image_client.get_image_from_sources(['frontleft_fisheye_image'])
_maybe_display_image(image_response[0].shot.image)
if config.save or config.save_path is not None:
_maybe_save_image(image_response[0].shot.image, config.save_path)
# Log a comment.
# Comments logged via this API are written to the robots test log. This is the best way
# to mark a log as "interesting". These comments will be available to Boston Dynamics
# devs when diagnosing customer issues.
log_comment = "HelloSpot tutorial user comment."
robot.operator_comment(log_comment)
robot.logger.info('Added comment "%s" to robot log.', log_comment)
# 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):
"""Command line interface.
Args:
argv: List of command-line arguments passed to the program.
"""
parser = argparse.ArgumentParser()
parser.add_argument(
"--model-path",
required=True,
help=
"Local file path to the Tensorflow model, example pre-trained models \
can be found at \
https://github.com/tensorflow/models/blob/master/research/object_detection/g3doc/detection_model_zoo.md"
)
parser.add_argument(
"--classes",
default='/classes.json',
type=str,
help="File containing json mapping of object class IDs to class names")
parser.add_argument(
"--number-tensorflow-processes",
default=1,
type=int,
help="Number of Tensorflow processes to run in parallel")
parser.add_argument(
"--detection-threshold",
default=0.7,
type=float,
help="Detection threshold to use for Tensorflow detections")
parser.add_argument(
"--sleep-between-capture",
default=1.0,
type=float,
help=
"Seconds to sleep between each image capture loop iteration, which captures "
+ "an image from all cameras")
parser.add_argument(
"--detection-class",
default=1,
type=int,
help="Detection classes to use in the" +
"Tensorflow model; Default is to use 1, which is a person in the Coco dataset"
)
parser.add_argument(
"--max-processing-delay",
default=7.0,
type=float,
help="Maximum allowed delay for processing an image; " +
"any image older than this value will be skipped")
bosdyn.client.util.add_common_arguments(parser)
options = parser.parse_args(argv)
try:
# Make sure the model path is a valid file
if not _check_model_path(options.model_path):
return False
# Check for classes json file, otherwise use the COCO class dictionary
_check_and_load_json_classes(options.classes)
global TENSORFLOW_PROCESS_BARRIER # pylint: disable=global-statement
TENSORFLOW_PROCESS_BARRIER = Barrier(
options.number_tensorflow_processes + 1)
# Start Tensorflow processes
start_tensorflow_processes(options.number_tensorflow_processes,
options.model_path, options.detection_class,
options.detection_threshold,
options.max_processing_delay)
# sleep to give the Tensorflow processes time to initialize
try:
TENSORFLOW_PROCESS_BARRIER.wait()
except BrokenBarrierError as exc:
print(
f'Error waiting for Tensorflow processes to initialize: {exc}')
return False
# Start the API related things
# Create robot object with a world object client
sdk = bosdyn.client.create_standard_sdk('SpotFollowClient')
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()
# Verify the robot is not estopped and that an external application has registered and holds
# an estop endpoint.
verify_estop(robot)
# Create the sdk clients
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
robot_command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
lease_client = robot.ensure_client(LeaseClient.default_service_name)
image_client = robot.ensure_client(ImageClient.default_service_name)
source_list = get_source_list(image_client)
image_task = AsyncImage(image_client, source_list)
robot_state_task = AsyncRobotState(robot_state_client)
task_list = [image_task, robot_state_task]
_async_tasks = AsyncTasks(task_list)
print('Detect and follow client connected.')
lease = lease_client.take()
lease_keep = LeaseKeepAlive(lease_client)
# Power on the robot and stand it up
resp = robot.power_on()
try:
blocking_stand(robot_command_client)
except CommandFailedError as exc:
print(
f'Error ({exc}) occurred while trying to stand. Check robot surroundings.'
)
return False
except CommandTimedOutError as exc:
print(f'Stand command timed out: {exc}')
return False
print('Robot powered on and standing.')
params_set = get_mobility_params()
# This thread starts the async tasks for image and robot state retrieval
update_thread = Thread(target=_update_thread, args=[_async_tasks])
update_thread.daemon = True
update_thread.start()
# Wait for the first responses.
while any(task.proto is None for task in task_list):
time.sleep(0.1)
# Start image capture process
image_capture_thread = Thread(target=capture_images,
args=(
image_task,
options.sleep_between_capture,
))
image_capture_thread.start()
while True:
# This comes from the tensorflow processes and limits the rate of this loop
entry = PROCESSED_BOXES_QUEUE.get()
# find the highest confidence bounding box
highest_conf_source = _find_highest_conf_source(entry)
if highest_conf_source is None:
# no boxes or scores found
continue
capture_to_use = entry[highest_conf_source]
raw_time = capture_to_use['raw_image_time']
time_gap = time.time() - raw_time
if time_gap > options.max_processing_delay:
continue # Skip image due to delay
# Find the transform to highest confidence object using the depth sensor
world_tform_object = get_object_position(
capture_to_use['world_tform_cam'],
capture_to_use['visual_image'], capture_to_use['depth_image'],
capture_to_use['boxes'][0],
ROTATION_ANGLES[capture_to_use['source']])
# get_object_position can fail if there is insufficient depth sensor information
if not world_tform_object:
continue
scores = capture_to_use['scores']
print(
f'Transform for object with confidence {scores[0]}: {world_tform_object}'
)
print(
f'Process latency: {time.time() - capture_to_use["system_cap_time"]}'
)
tag_cmd = get_go_to(world_tform_object, robot_state_task.proto,
params_set)
end_time = 15.0
if tag_cmd is not None:
robot_command_client.robot_command(lease=None,
command=tag_cmd,
end_time_secs=time.time() +
end_time)
return True
except Exception as exc: # pylint: disable=broad-except
LOGGER.error("Spot Tensorflow Detector threw an exception: %s", exc)
return False
def gesture_control(config):
"""A simple example of using the Boston Dynamics API to command a Spot robot."""
# 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('HelloSpotClient')
# 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 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."
# 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 several seconds.")
robot.power_on(timeout_sec=20)
assert robot.is_powered_on(), "Robot power on failed."
robot.logger.info("Robot powered on.")
# _robot_command_client = robot.ensure_client(RobotCommandClient.default_service_name)
fileHands = mp.solutions.hands
hands = fileHands.Hands(max_num_hands=1,
min_detection_confidence=0.98)
drawHands = mp.solutions.drawing_utils
gestureRecognizer = load_model("mp_hand_gesture")
print("here")
file = open("gesture.names", "r")
gestureNames = file.read().split('\n')
file.close()
vid = cv.VideoCapture(0)
while True:
_, frame = vid.read()
x, y, z = frame.shape
frame = cv.flip(frame, 1)
frameColored = cv.cvtColor(frame, cv.COLOR_BGR2RGB)
gesturePrediction = hands.process(frameColored)
gestureName = ""
if (gesturePrediction.multi_hand_landmarks):
handLandmarks = []
for handDetails in gesturePrediction.multi_hand_landmarks:
for landmark in handDetails.landmark:
landmarkX = int(landmark.x * x)
landmarkY = int(landmark.y * y)
handLandmarks.append([landmarkX, landmarkY])
drawHands.draw_landmarks(frame, handDetails,
fileHands.HAND_CONNECTIONS)
gesture = gestureRecognizer.predict([handLandmarks])
gestureIndex = np.argmax(gesture)
gestureName = gestureNames[gestureIndex]
print(gestureName)
if (gestureName == "walkForward"):
print("Forward")
# call function for Spot to walk forward
if (gestureName == "walkBackward"):
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.")
print("Spot, shut down!")
# call function for Spot to walk backward
if (gestureName == "walkLeft"):
print("Spot, take a step to the left!")
# call function for Spot to walk left
if (gestureName == "walkRight"):
print("Spot, take a step to the right!")
# call function for Spot to walk right
if (gestureName == "turnLeft"):
print("Spot, turn to the left!")
# call function for Spot to turn left
if (gestureName == "turnRight"):
print("Spot, turn to the right!")
# call function for Spot to turn right
if (gestureName == "tiltUp"):
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(3)
print("Spot, stand up!")
# call function for Spot to tilt up
if (gestureName == "tiltDown"):
print("Spot, tilt down!")
# call function for Spot to tilt down
# potential implementation of time.sleep()
cv.imshow("Gesture", frame)
if (cv.waitKey(1) == ord("x")):
break
vid.release()
cv.destroyAllWindows()
# 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.
# Tell the robot to stand in a twisted position.
#
# The RobotCommandBuilder constructs command messages, which are then
# issued to the robot using "robot_command" on the command client.
#
# In this example, the RobotCommandBuilder generates a stand command
# message with a non-default rotation in the footprint frame. The footprint
# frame is a gravity aligned frame with its origin located at the geometric
# center of the feet. The X axis of the footprint frame points forward along
# the robot's length, the Z axis points up aligned with gravity, and the Y
# axis is the cross-product of the two.
footprint_R_body = bosdyn.geometry.EulerZXY(yaw=0.4,
roll=0.0,
pitch=0.0)
cmd = RobotCommandBuilder.synchro_stand_command(
footprint_R_body=footprint_R_body)
command_client.robot_command(cmd)
robot.logger.info("Robot standing twisted.")
time.sleep(3)
# Now tell the robot to stand taller, using the same approach of constructing
# a command message with the RobotCommandBuilder and issuing it with
# robot_command.
cmd = RobotCommandBuilder.synchro_stand_command(body_height=0.1)
command_client.robot_command(cmd)
robot.logger.info("Robot standing tall.")
time.sleep(3)
# Capture an image.
# Spot has five sensors around the body. Each sensor consists of a stereo pair and a
# fisheye camera. The list_image_sources RPC gives a list of image sources which are
# available to the API client. Images are captured via calls to the get_image RPC.
# Images can be requested from multiple image sources in one call.
image_client = robot.ensure_client(
ImageClient.default_service_name)
sources = image_client.list_image_sources()
image_response = image_client.get_image_from_sources(
['frontleft_fisheye_image'])
# _maybe_display_image(image_response[0].shot.image)
# if config.save or config.save_path is not None:
# maybe_save_image(image_response[0].shot.image, config.save_path)
# Log a comment.
# Comments logged via this API are written to the robots test log. This is the best way
# to mark a log as "interesting". These comments will be available to Boston Dynamics
# devs when diagnosing customer issues.
log_comment = "HelloSpot tutorial user comment."
robot.operator_comment(log_comment)
robot.logger.info('Added comment "%s" to robot log.', log_comment)
# 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.
finally:
# If we successfully acquired a lease, return it.
lease_client.return_lease(lease)
def hello_arm(config):
"""A simple example of using the Boston Dynamics API to command Spot's arm."""
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('HelloSpotClient')
robot = sdk.create_robot(config.hostname)
robot.authenticate(config.username, config.password)
robot.time_sync.wait_for_sync()
assert robot.has_arm(), "Robot requires an arm to run this example."
# Verify the robot is not estopped and that an external application has registered and holds
# an estop endpoint.
assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \
"such as the estop SDK example, to configure E-Stop."
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
lease_client = robot.ensure_client(
bosdyn.client.lease.LeaseClient.default_service_name)
lease = lease_client.acquire()
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
# Now, we are ready to power on the robot. This call will block until the power
# is on. Commands would fail if this did not happen. We can also check that the robot is
# powered at any point.
robot.logger.info(
"Powering on robot... This may take a several seconds.")
robot.power_on(timeout_sec=20)
assert robot.is_powered_on(), "Robot power on failed."
robot.logger.info("Robot powered on.")
# Tell the robot to stand up. The command service is used to issue commands to a robot.
# The set of valid commands for a robot depends on hardware configuration. See
# SpotCommandHelper for more detailed examples on command building. The robot
# command service requires timesync between the robot and the client.
robot.logger.info("Commanding robot to stand...")
command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
blocking_stand(command_client, timeout_sec=10)
robot.logger.info("Robot standing.")
time.sleep(2.0)
# Move the arm to a spot in front of the robot, and open the gripper.
# Make the arm pose RobotCommand
# Build a position to move the arm to (in meters, relative to and expressed in the gravity aligned body frame).
x = 0.75
y = 0
z = 0.25
hand_ewrt_flat_body = geometry_pb2.Vec3(x=x, y=y, z=z)
# Rotation as a quaternion
qw = 1
qx = 0
qy = 0
qz = 0
flat_body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz)
flat_body_T_hand = geometry_pb2.SE3Pose(
position=hand_ewrt_flat_body, rotation=flat_body_Q_hand)
robot_state = robot_state_client.get_robot_state()
odom_T_flat_body = get_a_tform_b(
robot_state.kinematic_state.transforms_snapshot,
ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME)
odom_T_hand = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
flat_body_T_hand)
# duration in seconds
seconds = 2
arm_command = RobotCommandBuilder.arm_pose_command(
odom_T_hand.x, odom_T_hand.y, odom_T_hand.z, odom_T_hand.rot.w,
odom_T_hand.rot.x, odom_T_hand.rot.y, odom_T_hand.rot.z,
ODOM_FRAME_NAME, seconds)
# Make the open gripper RobotCommand
gripper_command = RobotCommandBuilder.claw_gripper_open_fraction_command(
1.0)
# Combine the arm and gripper commands into one RobotCommand
command = RobotCommandBuilder.build_synchro_command(
gripper_command, arm_command)
# Send the request
cmd_id = command_client.robot_command(command)
robot.logger.info('Moving arm to position 1.')
# Wait until the arm arrives at the goal.
block_until_arm_arrives_with_prints(robot, command_client, cmd_id)
# Move the arm to a different position
hand_ewrt_flat_body.z = 0
flat_body_Q_hand.w = 0.707
flat_body_Q_hand.x = 0.707
flat_body_Q_hand.y = 0
flat_body_Q_hand.z = 0
flat_body_T_hand2 = geometry_pb2.SE3Pose(
position=hand_ewrt_flat_body, rotation=flat_body_Q_hand)
odom_T_hand = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
flat_body_T_hand2)
arm_command = RobotCommandBuilder.arm_pose_command(
odom_T_hand.x, odom_T_hand.y, odom_T_hand.z, odom_T_hand.rot.w,
odom_T_hand.rot.x, odom_T_hand.rot.y, odom_T_hand.rot.z,
ODOM_FRAME_NAME, seconds)
# Close the gripper
gripper_command = RobotCommandBuilder.claw_gripper_open_fraction_command(
0.0)
# Build the proto
command = RobotCommandBuilder.build_synchro_command(
gripper_command, arm_command)
# Send the request
cmd_id = command_client.robot_command(command)
robot.logger.info('Moving arm to position 2.')
# Wait until the arm arrives at the goal.
# Note: here we use the helper function provided by robot_command.
block_until_arm_arrives(command_client, cmd_id)
robot.logger.info('Done.')
# Power the robot off. By specifying "cut_immediately=False", a safe power off command
# is issued to the robot. This will attempt to sit the robot before powering off.
robot.power_off(cut_immediately=False, timeout_sec=20)
assert not robot.is_powered_on(), "Robot power off failed."
robot.logger.info("Robot safely powered off.")
finally:
# If we successfully acquired a lease, return it.
lease_client.return_lease(lease)
def main(raw_args=None):
"""Replay stored mission"""
body_lease = None
# Configure logging
bosdyn.client.util.setup_logging()
# Parse command-line arguments
parser = argparse.ArgumentParser()
bosdyn.client.util.add_common_arguments(parser)
parser.add_argument('--timeout',
type=float,
default=3.0,
dest='timeout',
help='Mission client timeout (s).')
parser.add_argument('--noloc',
action='store_true',
default=False,
dest='noloc',
help='Skip initial localization')
parser.add_argument(
'--disable_alternate_route_finding',
action='store_true',
default=False,
dest='disable_alternate_route_finding',
help='Disable creating alternate-route-finding graph structure')
parser.add_argument(
'--disable_directed_exploration',
action='store_true',
default=False,
dest='disable_directed_exploration',
help='Disable directed exploration for skipped blocked branches')
group = parser.add_mutually_exclusive_group()
group.add_argument('--time',
type=float,
default=0.0,
dest='duration',
help='Time to repeat mission (sec)')
group.add_argument('--static',
action='store_true',
default=False,
dest='static_mode',
help='Stand, but do not run robot')
# Subparser for mission type
subparsers = parser.add_subparsers(dest='mission_type',
help='Mission type')
subparsers.required = True
# Subparser for simple mission
simple_parser = subparsers.add_parser('simple',
help='Simple mission (non-Autowalk)')
simple_parser.add_argument('simple_mission_file',
help='Mission file for non-Autowalk mission.')
# Subparser for Autowalk mission
autowalk_parser = subparsers.add_parser(
'autowalk', help='Autowalk mission using graph_nav')
autowalk_parser.add_argument(
'map_directory',
help=
'Directory containing graph_nav map and default Autowalk mission file.'
)
autowalk_parser.add_argument(
'--autowalk_mission',
dest='autowalk_mission_file',
help='Optional alternate Autowalk mission file.')
args = parser.parse_args(raw_args)
if args.mission_type == 'simple':
do_map_load = False
fail_on_question = False
do_localization = False
mission_file = args.simple_mission_file
map_directory = None
print('[ REPLAYING SIMPLE MISSION {} : HOSTNAME {} ]'.format(
mission_file, args.hostname))
else:
do_map_load = True
fail_on_question = True
if args.noloc:
do_localization = False
else:
do_localization = True
map_directory = args.map_directory
if args.autowalk_mission_file:
mission_file = args.autowalk_mission_file
else:
mission_file = map_directory + '/missions/autogenerated'
print(
'[ REPLAYING AUTOWALK MISSION {} : MAP DIRECTORY {} : HOSTNAME {} ]'
.format(mission_file, map_directory, args.hostname))
# Initialize robot object
robot = init_robot(args.hostname, args.username, args.password)
# Acquire robot lease
robot.logger.info('Acquiring lease...')
lease_client = robot.ensure_client(
bosdyn.client.lease.LeaseClient.default_service_name)
body_lease = lease_client.acquire()
if body_lease is None:
raise Exception('Lease not acquired.')
robot.logger.info('Lease acquired: %s', str(body_lease))
# Initialize power client
robot.logger.info('Starting power client...')
power_client = robot.ensure_client(PowerClient.default_service_name)
# Initialize other clients
robot_state_client, command_client, mission_client, graph_nav_client = init_clients(
robot, body_lease, mission_file, map_directory, do_map_load,
args.disable_alternate_route_finding)
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
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."
# Turn on power
power_on(power_client)
# Stand up and wait for the perception system to stabilize
robot.logger.info('Commanding robot to stand...')
blocking_stand(command_client, timeout_sec=20)
countdown(5)
robot.logger.info('Robot standing.')
# Localize robot
localization_error = False
if do_localization:
graph = graph_nav_client.download_graph()
robot.logger.info('Localizing robot...')
robot_state = robot_state_client.get_robot_state()
localization = nav_pb2.Localization()
# Attempt to localize using any visible fiducial
graph_nav_client.set_localization(
initial_guess_localization=localization,
ko_tform_body=None,
max_distance=None,
max_yaw=None,
fiducial_init=graph_nav_pb2.SetLocalizationRequest.
FIDUCIAL_INIT_NEAREST)
# Run mission
if not args.static_mode and not localization_error:
if args.duration == 0.0:
run_mission(robot, mission_client, lease_client,
fail_on_question, args.timeout,
args.disable_directed_exploration)
else:
repeat_mission(robot, mission_client, lease_client,
args.duration, fail_on_question,
args.timeout,
args.disable_directed_exploration)
finally:
# Turn off power
lease_client.lease_wallet.advance()
robot.logger.info('Powering off...')
safe_power_off(command_client, robot_state_client)
# Return lease
robot.logger.info('Returning lease...')
lease_client.return_lease(body_lease)
def stand(self):
"""Stand the robot."""
blocking_stand(self._robot_command_client)
