def stand(self, monitor_command=True):
"""If the e-stop is enabled, and the motor power is enabled, stand the robot up."""
response = self._robot_command(
RobotCommandBuilder.stand_command(params=self._mobility_params))
if monitor_command:
self._last_stand_command = response[2]
return response[0], response[1]
def stand_cmd_srv(self, stand):
"""Callback that sends stand cmd at a given height delta [m] from standard configuration"""
cmd = RobotCommandBuilder.stand_command(
body_height=stand.body_pose.translation.z,
footprint_R_body=self.quat_to_euler(stand.body_pose.rotation))
ret = self.command_client.robot_command(cmd)
rospy.loginfo("Robot stand cmd sent. {}".format(ret))
return []
def _stand(self):
"""Sets robot in Stand mode.
"""
if not self.motors_powered:
return
if self.mode is not RobotMode.Stand:
self.mode = RobotMode.Stand
self.mobility_params = spot_command_pb2.MobilityParams(
locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=0)
cmd = RobotCommandBuilder.stand_command(
params=self.mobility_params)
self.command_client.robot_command_async(cmd)
def _walk(self):
"""Sets robot in Walk mode.
"""
if not self.motors_powered:
return
if self.mode is not RobotMode.Walk:
self.mode = RobotMode.Walk
self.mobility_params = spot_command_pb2.MobilityParams(
locomotion_hint=spot_command_pb2.HINT_SPEED_SELECT_TROT,
stair_hint=0)
cmd = RobotCommandBuilder.stand_command(
params=self.mobility_params)
self.command_client.robot_command_async(cmd)
def _hop(self):
"""Sets robot in Hop mode.
"""
if not self.motors_powered:
return
if self.mode is not RobotMode.Hop:
self.mode = RobotMode.Hop
self._reset_height()
self.mobility_params = spot_command_pb2.MobilityParams(
locomotion_hint=spot_command_pb2.HINT_HOP, stair_hint=0)
cmd = RobotCommandBuilder.stand_command(
params=self.mobility_params)
self.command_client.robot_command_async(cmd)
def orient(self, yaw=0.0, pitch=0.0, roll=0.0):
"""
Ask Spot to orient its body (e.g. yaw, pitch, roll)
@param[in] yaw Rotation about Z (+up/down) [rad]
@param[in] pitch Rotation about Y (+left/right) [rad]
@param[in] roll Rotation about X (+front/back) [rad]
"""
if self._robot is None:
return
if self._prep_for_motion():
rotation = geometry.EulerZXY(yaw=yaw, pitch=pitch, roll=roll)
command_client = self._robot.ensure_client(
RobotCommandClient.default_service_name)
cmd = RobotCommandBuilder.stand_command(footprint_R_body=rotation)
command_client.robot_command(cmd)
def _orientation_cmd_helper(self,
yaw=0.0,
roll=0.0,
pitch=0.0,
height=0.0):
"""Helper function that commands the robot with an orientation command;
Used by the other orientation functions.
Args:
yaw: Yaw of the robot body. Defaults to 0.0.
roll: Roll of the robot body. Defaults to 0.0.
pitch: Pitch of the robot body. Defaults to 0.0.
height: Height of the robot body from normal stand height. Defaults to 0.0.
"""
if not self.motors_powered:
return
orientation = EulerZXY(yaw, roll, pitch)
cmd = RobotCommandBuilder.stand_command(body_height=height,
footprint_R_body=orientation)
self.command_client.robot_command_async(cmd,
end_time_secs=time.time() +
VELOCITY_CMD_DURATION)
def _stand(self):
self._start_robot_command('stand', RobotCommandBuilder.stand_command())
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')
sdk.load_app_token(config.app_token)
# 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.
verify_estop(robot)
# 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.")
time.sleep(15)
# 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.6,
roll=0.3,
pitch=0.0)
cmd = RobotCommandBuilder.stand_command(
footprint_R_body=footprint_R_body)
command_client.robot_command(cmd)
robot.logger.info("Robot standing twisted.")
time.sleep(10)
# 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.stand_command(body_height=0.12)
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)
# 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():
'''Replay stored mission'''
import argparse
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('--localize',
action='store_true',
default=False,
help='Localize robot before running mission')
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 and localize 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_map_load = args.map_directory is not None
do_robot_stand = args.map_directory is not None
do_localization = (args.map_directory is not None) or args.localize
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,
args.app_token)
# 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)
if do_robot_stand:
# Ensure robot is powered on
assert ensure_power_on(robot), 'Robot power on failed.'
# Stand up
robot.logger.info('Commanding robot to stand...')
stand_command = RobotCommandBuilder.stand_command()
command_client.robot_command(stand_command)
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
try:
graph_nav_client.set_localization(
initial_guess_localization=localization)
except:
robot.logger.error('<<< INITIAL LOCALIZATION FAILED >>>')
localization_error = True
if not args.static_mode and not localization_error:
if args.duration == 0.0:
run_mission(robot,
mission_client,
lease_client,
fail_on_question,
mission_timeout=3)
else:
repeat_mission(robot, mission_client, lease_client,
args.duration, fail_on_question)
finally:
if do_robot_stand:
# Ensure robot is powered off
power_off_success = ensure_power_off(robot)
# Return lease
robot.logger.info('Returning lease...')
lease_client.return_lease(body_lease)
def test_stand_command():
command = RobotCommandBuilder.stand_command()
_test_has_mobility(command)
assert command.mobility_command.HasField("stand_request")
def build_stand_command(deprecated, **kwargs):
if (deprecated):
cmd = RobotCommandBuilder.stand_command(**kwargs)
else:
cmd = RobotCommandBuilder.synchro_stand_command(**kwargs)
return cmd
def _stand(self):
self._start_robot_command('stand', RobotCommandBuilder.stand_command(
body_height=self.height,
footprint_R_body=geometry.EulerZXY(roll=self.roll, yaw=self.yaw, pitch=self.pitch)))
