-
Notifications
You must be signed in to change notification settings - Fork 0
/
spot_good_boi.py
146 lines (129 loc) · 7.28 KB
/
spot_good_boi.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
# Copyright (c) 2022 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).
from __future__ import print_function
import argparse
import os
import sys
import time
import bosdyn.client
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 spot_good_boi(config):
# 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('SpotGoodBoiClient')
# 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.
bosdyn.client.util.authenticate(robot)
# 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. The LeaseKeepAlive object takes care of acquiring and returning
# the lease for us.
lease_client = robot.ensure_client(bosdyn.client.lease.LeaseClient.default_service_name)
with bosdyn.client.lease.LeaseKeepAlive(lease_client, must_acquire=True, return_at_exit=True):
# 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
# RobotCommandBuilder 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(0.5)
# 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, roll=0, pitch=-1.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)
footprint_R_body = bosdyn.geometry.EulerZXY(yaw=-0.1, roll=-0.5, pitch=-1.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(0.5)
footprint_R_body = bosdyn.geometry.EulerZXY(yaw=0.1, roll=0.5, pitch=-1.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(1.5)
footprint_R_body = bosdyn.geometry.EulerZXY(yaw=-0.4, roll=-0.5, pitch=-1.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(0.5)
footprint_R_body = bosdyn.geometry.EulerZXY(yaw=0.4, roll=0.5, pitch=-1.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(1.5)
footprint_R_body = bosdyn.geometry.EulerZXY(yaw=0, roll=0, pitch=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(0.5)
# 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 = "Spot Good Boi finished."
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.")
def main(argv):
"""Command line interface."""
parser = argparse.ArgumentParser()
bosdyn.client.util.add_base_arguments(parser)
options = parser.parse_args(argv)
try:
spot_good_boi(options)
return True
except Exception as exc: # pylint: disable=broad-except
logger = bosdyn.client.util.get_logger()
logger.error("Hello, Spot! threw an exception: %r", exc)
return False
if __name__ == '__main__':
if not main(sys.argv[1:]):
sys.exit(1)