forked from boston-dynamics/spot-sdk
-
Notifications
You must be signed in to change notification settings - Fork 0
/
hello_spot.py
174 lines (150 loc) · 8.33 KB
/
hello_spot.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
# Copyright (c) 2019 Boston Dynamics, Inc. All rights reserved.
#
# Downloading, reproducing, distributing or otherwise using the SDK Software
# is subject to the terms and conditions of the Boston Dynamics Software
# Development Kit License (20191101-BDSDK-SL).
"""Tutorial to show how to use the Boston Dynamics API"""
from __future__ import print_function
import argparse
import sys
import time
import bosdyn.client
import bosdyn.client.estop
import bosdyn.client.lease
import bosdyn.client.util
import bosdyn.geometry
from bosdyn.client.image import ImageClient
from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient, blocking_stand
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()
# Spot requires a software estop to be activated.
estop_client = robot.ensure_client(bosdyn.client.estop.EstopClient.default_service_name)
estop_endpoint = bosdyn.client.estop.EstopEndpoint(client=estop_client, name='HelloSpot',
estop_timeout=9.0)
estop_endpoint.force_simple_setup()
# 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), bosdyn.client.estop.EstopKeepAlive(
estop_endpoint):
# 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(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.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.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)
# 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 _maybe_display_image(image, display_time=3.0):
"""Try to display image, if client has correct deps."""
try:
from PIL import Image
import io
except ImportError:
logger = bosdyn.client.util.get_logger()
logger.warn("Missing dependencies. Can't display image.")
return
try:
image = Image.open(io.BytesIO(image.data))
image.show()
time.sleep(display_time)
except Exception as exc:
logger = bosdyn.client.util.get_logger()
logger.warn("Exception thrown displaying image. %s" % exc)
def main(argv):
"""Command line interface."""
parser = argparse.ArgumentParser()
bosdyn.client.util.add_common_arguments(parser)
options = parser.parse_args(argv)
try:
hello_spot(options)
return True
except Exception as exc: # pylint: disable=broad-except
logger = bosdyn.client.util.get_logger()
logger.error("Hello, Spot! threw an exception: %s", exc)
return False
if __name__ == '__main__':
if not main(sys.argv[1:]):
sys.exit(1)