def test_synchro_stand_command():
command = RobotCommandBuilder.synchro_stand_command()
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
assert command.synchronized_command.mobility_command.HasField("stand_request")
# basic check with a build_on_command
arm_command = RobotCommandBuilder.arm_stow_command()
command = RobotCommandBuilder.synchro_stand_command(build_on_command=arm_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_arm(command.synchronized_command)
def pitch_up(robot):
"""Pitch robot up to look for door handle.
Args:
robot: (Robot) Interface to Spot robot.
"""
robot.logger.info("Pitching robot up...")
command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
footprint_R_body = geometry.EulerZXY(0.0, 0.0, -1 * math.pi / 6.0)
cmd = RobotCommandBuilder.synchro_stand_command(
footprint_R_body=footprint_R_body)
cmd_id = command_client.robot_command(cmd)
timeout_sec = 10.0
end_time = time.time() + timeout_sec
while time.time() < end_time:
response = command_client.robot_command_feedback(cmd_id)
synchronized_feedback = response.feedback.synchronized_feedback
status = synchronized_feedback.mobility_command_feedback.stand_feedback.status
if (status ==
basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING):
robot.logger.info("Robot pitched.")
return
time.sleep(1.0)
raise Exception("Failed to pitch robot.")
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.synchro_stand_command(
params=self._mobility_params))
if monitor_command:
self._last_stand_command = response[2]
return response[0], response[1]
def _amble(self):
"""Sets robot in Amble mode.
"""
if self.mode is not RobotMode.Amble:
self.mode = RobotMode.Amble
self.mobility_params = spot_command_pb2.MobilityParams(
locomotion_hint=spot_command_pb2.HINT_AMBLE, stair_hint=0)
cmd = RobotCommandBuilder.synchro_stand_command(
params=self.mobility_params)
self._issue_robot_command(cmd)
def _crawl(self):
"""Sets robot in Crawl mode.
"""
if self.mode is not RobotMode.Crawl:
self.mode = RobotMode.Crawl
self.mobility_params = spot_command_pb2.MobilityParams(
locomotion_hint=spot_command_pb2.HINT_CRAWL, stair_hint=0)
cmd = RobotCommandBuilder.synchro_stand_command(
params=self.mobility_params)
self._issue_robot_command(cmd)
def _stand(self):
"""Sets robot in Stand mode.
"""
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.synchro_stand_command(
params=self.mobility_params)
self._issue_robot_command(cmd)
def _hop(self):
"""Sets robot in Hop mode.
"""
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.synchro_stand_command(
params=self.mobility_params)
self._issue_robot_command(cmd)
def _walk(self):
"""Sets robot in Walk mode.
"""
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.synchro_stand_command(
params=self.mobility_params)
self._issue_robot_command(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.synchro_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.synchro_stand_command(
body_height=height, footprint_R_body=orientation)
self._issue_robot_command(cmd,
endtime=time.time() + VELOCITY_CMD_DURATION)
def test_build_synchro_command():
# two synchro subcommands of the same type:
arm_command_1 = RobotCommandBuilder.arm_ready_command()
arm_command_2 = RobotCommandBuilder.arm_stow_command()
command = RobotCommandBuilder.build_synchro_command(arm_command_1, arm_command_2)
_test_has_synchronized(command)
_test_has_arm(command.synchronized_command)
assert not command.synchronized_command.HasField("gripper_command")
assert not command.synchronized_command.HasField("mobility_command")
command_position = command.synchronized_command.arm_command.named_arm_position_command.position
assert command_position == arm_command_pb2.NamedArmPositionsCommand.POSITIONS_STOW
# two synchro subcommands of a different type:
arm_command = RobotCommandBuilder.arm_ready_command()
mobility_command = RobotCommandBuilder.synchro_stand_command()
command = RobotCommandBuilder.build_synchro_command(arm_command, mobility_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_arm(command.synchronized_command)
assert not command.synchronized_command.HasField("gripper_command")
assert command.synchronized_command.mobility_command.HasField("stand_request")
command_position = command.synchronized_command.arm_command.named_arm_position_command.position
assert command_position == arm_command_pb2.NamedArmPositionsCommand.POSITIONS_READY
# one synchro command and one deprecated mobility command:
deprecated_mobility_command = RobotCommandBuilder.sit_command()
command = RobotCommandBuilder.build_synchro_command(mobility_command,
deprecated_mobility_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
assert command.synchronized_command.mobility_command.HasField("sit_request")
# fullbody command is rejected
full_body_command = RobotCommandBuilder.selfright_command()
with pytest.raises(Exception):
command = RobotCommandBuilder.build_synchro_command(arm_command, full_body_command)
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')
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)
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
robot.logger.info('Commanding robot to stand...')
stand_command = RobotCommandBuilder.synchro_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
graph_nav_client.set_localization(
initial_guess_localization=localization,
ko_tform_body=None,
max_distance=None,
max_yaw=None)
# 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 test_synchro_stand_command():
command = RobotCommandBuilder.synchro_stand_command()
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
assert command.synchronized_command.mobility_command.HasField(
"stand_request")
def stand(self):
self.__execute_command("stand",
RobotCommandBuilder.synchro_stand_command())
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.synchro_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')
# 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 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 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)
