def set_mobility_params(self):
"""Set robot mobility params to disable obstacle aworldidance."""
obstacles = spot_command_pb2.ObstacleParams(
disable_vision_body_obstacle_avoidance=True,
disable_vision_foot_obstacle_avoidance=True,
disable_vision_foot_constraint_avoidance=True,
obstacle_avoidance_padding=.001)
body_control = self.set_default_body_control()
if self._limit_speed:
speed_limit = SE2VelocityLimit(max_vel=SE2Velocity(
linear=Vec2(x=self._max_x_vel, y=self._max_y_vel),
angular=self._max_ang_vel))
if not self._avoid_obstacles:
mobility_params = spot_command_pb2.MobilityParams(
obstacle_params=obstacles,
vel_limit=speed_limit,
body_control=body_control,
locomotion_hint=spot_command_pb2.HINT_AUTO)
else:
mobility_params = spot_command_pb2.MobilityParams(
vel_limit=speed_limit,
body_control=body_control,
locomotion_hint=spot_command_pb2.HINT_AUTO)
elif not self._avoid_obstacles:
mobility_params = spot_command_pb2.MobilityParams(
obstacle_params=obstacles,
body_control=body_control,
locomotion_hint=spot_command_pb2.HINT_AUTO)
else:
#When set to none, RobotCommandBuilder populates with good default values
mobility_params = None
return mobility_params
def initialize_robot_from_config(self, config):
"""Initializes SDK from command line arguments.
Args:
config: Command-line arguments as argparse object.
"""
sdk = bosdyn.client.create_standard_sdk(self.client_name)
self.robot = sdk.create_robot(config.hostname)
self.robot.authenticate(config.username, config.password)
self.robot.time_sync.wait_for_sync()
self.command_client = self.robot.ensure_client(
RobotCommandClient.default_service_name)
self.lease_client = self.robot.ensure_client(
LeaseClient.default_service_name)
self.estop_client = self.robot.ensure_client(
EstopClient.default_service_name)
self.mobility_params = spot_command_pb2.MobilityParams(
locomotion_hint=spot_command_pb2.HINT_AUTO)
# Print controls
print(
textwrap.dedent("""\
| Button Combination | Functionality |
|--------------------|--------------------------|
| A | Walk |
| B | Stand |
| X | Sit |
| Y | Stairs |
| LB + : | |
| - D-pad up/down | Walk height |
| - D-pad left | Battery-Change Pose |
| - D-pad right | Self right |
| - Y | Jog |
| - A | Amble |
| - B | Crawl |
| - X | Hop |
| | |
| If Stand Mode | |
| - Left Stick | |
| -- X | Rotate body in roll axis |
| -- Y | Control height |
| - Right Stick | |
| -- X | Turn body in yaw axis |
| -- Y | Turn body in pitch axis |
| Else | |
| - Left Stick | Move |
| - Right Stick | Turn |
| | |
| LB + RB + B | E-Stop |
| Start | Motor power & Control |
| Back | Exit |
"""))
# Describe the necessary steps before one can command the robot.
print("Before you can command the robot: \n" + \
"\t1. Acquire a software E-Stop (Left Button + Right Button + B). \n" + \
"\t2. Obtain a lease and power on the robot's motors (Start button).")
def get_mobility_params():
"""Gets mobility parameters for following"""
vel_desired = .75
speed_limit = geo.SE2VelocityLimit(
max_vel=geo.SE2Velocity(linear=geo.Vec2(x=vel_desired, y=vel_desired), angular=.25))
body_control = set_default_body_control()
mobility_params = spot_command_pb2.MobilityParams(vel_limit=speed_limit, obstacle_params=None,
body_control=body_control,
locomotion_hint=spot_command_pb2.HINT_TROT)
return mobility_params
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 _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 _sit(self):
"""Sets robot in Sit mode.
"""
if self.mode is not RobotMode.Sit:
self.mode = RobotMode.Sit
self.mobility_params = spot_command_pb2.MobilityParams(
locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=0)
cmd = RobotCommandBuilder.synchro_sit_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 _sit(self):
"""Sets robot in Sit mode.
"""
if not self.motors_powered:
return
if self.mode is not RobotMode.Sit:
self.mode = RobotMode.Sit
self.mobility_params = spot_command_pb2.MobilityParams(
locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=0)
cmd = RobotCommandBuilder.sit_command(params=self.mobility_params)
self.command_client.robot_command_async(cmd)
def _crawl(self):
"""Sets robot in Crawl mode.
"""
if not self.motors_powered:
return
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.stand_command(
params=self.mobility_params)
self.command_client.robot_command_async(cmd)
def _amble(self):
"""Sets robot in Amble mode.
"""
if not self.motors_powered:
return
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.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 initialize_robot_from_config(self, config):
"""Initializes SDK from command line arguments.
Args:
config: Command-line arguments as argparse object.
"""
sdk = bosdyn.client.create_standard_sdk(self.client_name)
sdk.load_app_token(config.app_token)
self.robot = sdk.create_robot(config.hostname)
self.robot.authenticate(config.username, config.password)
self.robot.time_sync.wait_for_sync()
self.command_client = self.robot.ensure_client(
RobotCommandClient.default_service_name)
self.lease_client = self.robot.ensure_client(
LeaseClient.default_service_name)
self.estop_client = self.robot.ensure_client(
EstopClient.default_service_name)
self.mobility_params = spot_command_pb2.MobilityParams(
locomotion_hint=spot_command_pb2.HINT_AUTO)
def mobility_params(body_height=0.0, footprint_R_body=geometry.EulerZXY(),
locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=False,
external_force_params=None):
"""Helper to create Mobility params for spot mobility commands. This function is designed
to help get started issuing commands, but lots of options are not exposed via this
interface. See spot.robot_command_pb2 for more details. If unset, good defaults will be
chosen by the robot.
Returns:
spot.MobilityParams, params for spot mobility commands.
"""
# Simplified body control params
position = geometry_pb2.Vec3(z=body_height)
rotation = footprint_R_body.to_quaternion()
pose = geometry_pb2.SE3Pose(position=position, rotation=rotation)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
frame = geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_BODY)
traj = trajectory_pb2.SE3Trajectory(points=[point], frame=frame)
body_control = spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj)
return spot_command_pb2.MobilityParams(body_control=body_control,
locomotion_hint=locomotion_hint,
stair_hint=stair_hint,
external_force_params=external_force_params)
def mobility_params(body_height=0.0,
footprint_R_body=geometry.EulerZXY(),
locomotion_hint=spot_command_pb2.HINT_AUTO,
stair_hint=False,
external_force_params=None):
"""Helper to create Mobility params for spot mobility commands. This function is designed
to help get started issuing commands, but lots of options are not exposed via this
interface. See spot.robot_command_pb2 for more details. If unset, good defaults will be
chosen by the robot.
Args:
body_height: Body height in meters.
footprint_R_body(EulerZXY): The orientation of the body frame with respect to the
footprint frame (gravity aligned framed with yaw computed
from the stance feet)
locomotion_hint: Locomotion hint to use for the command.
stair_hint: Boolean to specify if stair mode should be used.
external_force_params(spot.BodyExternalForceParams): Robot body external force
parameters.
Returns:
spot.MobilityParams, params for spot mobility commands.
"""
# Simplified body control params
position = geometry_pb2.Vec3(z=body_height)
rotation = footprint_R_body.to_quaternion()
pose = geometry_pb2.SE3Pose(position=position, rotation=rotation)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE3Trajectory(points=[point])
body_control = spot_command_pb2.BodyControlParams(
base_offset_rt_footprint=traj)
return spot_command_pb2.MobilityParams(
body_control=body_control,
locomotion_hint=locomotion_hint,
stair_hint=stair_hint,
external_force_params=external_force_params)
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)
