def test_arm_ready_command():
command = RobotCommandBuilder.arm_ready_command()
_test_has_synchronized(command)
_test_has_arm(command.synchronized_command)
assert (command.synchronized_command.arm_command.WhichOneof("command") ==
'named_arm_position_command')
# with a build_on_command
mobility_command = RobotCommandBuilder.synchro_sit_command()
command = RobotCommandBuilder.arm_ready_command(build_on_command=mobility_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_arm(command.synchronized_command)
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 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('ForceTrajectoryClient')
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()
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(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)
# Demonstrate an example force trajectory by ramping up and down a vertical force over
# 10 seconds
f_x0 = 0 # Newtons
f_y0 = 0
f_z0 = 0
f_x1 = 0 # Newtons
f_y1 = 0
f_z1 = -10 # push down
# We won't have any rotational torques
torque_x = 0
torque_y = 0
torque_z = 0
# Duration in seconds.
traj_duration = 5
# First point on the trajectory
force0 = geometry_pb2.Vec3(x=f_x0, y=f_y0, z=f_z0)
torque0 = geometry_pb2.Vec3(x=torque_x, y=torque_y, z=torque_z)
wrench0 = geometry_pb2.Wrench(force=force0, torque=torque0)
t0 = seconds_to_duration(0)
traj_point0 = trajectory_pb2.WrenchTrajectoryPoint(
wrench=wrench0, time_since_reference=t0)
# Second point on the trajectory
force1 = geometry_pb2.Vec3(x=f_x1, y=f_y1, z=f_z1)
torque1 = geometry_pb2.Vec3(x=torque_x, y=torque_y, z=torque_z)
wrench1 = geometry_pb2.Wrench(force=force1, torque=torque1)
t1 = seconds_to_duration(traj_duration)
traj_point1 = trajectory_pb2.WrenchTrajectoryPoint(
wrench=wrench1, time_since_reference=t1)
# Build the trajectory
trajectory = trajectory_pb2.WrenchTrajectory(
points=[traj_point0, traj_point1])
# Build the full request, putting all axes into force mode.
arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request(
root_frame_name=ODOM_FRAME_NAME,
wrench_trajectory_in_task=trajectory,
x_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
y_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
z_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
rx_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
ry_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
rz_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE)
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('Force trajectory command issued...')
time.sleep(5.0 + traj_duration)
# 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 arm_surface_contact(config):
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('ArmSurfaceContactExample')
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."
arm_surface_contact_client = robot.ensure_client(
ArmSurfaceContactClient.default_service_name)
# 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.")
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)
# ----------
#
# Now we'll use the arm_surface_contact service to do an accurate position move with
# some amount of force.
#
# Position of the hand:
hand_x = 0.75 # in front of the robot.
hand_y_start = 0 # centered
hand_y_end = -0.5 # to the right
hand_z = 0 # will be ignored since we'll have a force in the Z axis.
f_z = -0.05 # percentage of maximum press force, negative to press down
# be careful setting this too large, you can knock the robot over
percentage_press = geometry_pb2.Vec3(x=0, y=0, z=f_z)
hand_vec3_start_rt_body = geometry_pb2.Vec3(x=hand_x,
y=hand_y_start,
z=hand_z)
hand_vec3_end_rt_body = geometry_pb2.Vec3(x=hand_x,
y=hand_y_end,
z=hand_z)
# We want to point the hand straight down the entire time.
qw = 0.707
qx = 0
qy = 0.707
qz = 0
body_Q_hand = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz)
# Build a position trajectory
body_T_hand1 = geometry_pb2.SE3Pose(
position=hand_vec3_start_rt_body, rotation=body_Q_hand)
body_T_hand2 = geometry_pb2.SE3Pose(position=hand_vec3_end_rt_body,
rotation=body_Q_hand)
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)
odom_T_hand1 = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
body_T_hand1)
odom_T_hand2 = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
body_T_hand2)
# Trajectory length
traj_time = 5.0 # in seconds
time_since_reference = seconds_to_duration(traj_time)
traj_point1 = trajectory_pb2.SE3TrajectoryPoint(
pose=odom_T_hand1.to_proto(),
time_since_reference=seconds_to_duration(0))
traj_point2 = trajectory_pb2.SE3TrajectoryPoint(
pose=odom_T_hand2.to_proto(),
time_since_reference=time_since_reference)
hand_traj = trajectory_pb2.SE3Trajectory(
points=[traj_point1, traj_point2])
# Open the gripper
gripper_cmd_packed = RobotCommandBuilder.claw_gripper_open_fraction_command(
0)
gripper_command = gripper_cmd_packed.synchronized_command.gripper_command.claw_gripper_command
cmd = arm_surface_contact_pb2.ArmSurfaceContact.Request(
pose_trajectory_in_task=hand_traj,
root_frame_name=ODOM_FRAME_NAME,
press_force_percentage=percentage_press,
x_axis=arm_surface_contact_pb2.ArmSurfaceContact.Request.
AXIS_MODE_POSITION,
y_axis=arm_surface_contact_pb2.ArmSurfaceContact.Request.
AXIS_MODE_POSITION,
z_axis=arm_surface_contact_pb2.ArmSurfaceContact.Request.
AXIS_MODE_FORCE,
z_admittance=arm_surface_contact_pb2.ArmSurfaceContact.Request.
ADMITTANCE_SETTING_LOOSE,
# Enable the cross term so that if the arm gets stuck in a rut, it will retract
# upwards slightly, preventing excessive lateral forces.
xy_to_z_cross_term_admittance=arm_surface_contact_pb2.
ArmSurfaceContact.Request.ADMITTANCE_SETTING_VERY_STIFF,
gripper_command=gripper_command)
# Enable walking
cmd.is_robot_following_hand = True
# A bias force (in this case, leaning forward) can help improve stability.
bias_force_x = -25
cmd.bias_force_ewrt_body.CopyFrom(
geometry_pb2.Vec3(x=bias_force_x, y=0, z=0))
proto = arm_surface_contact_service_pb2.ArmSurfaceContactCommand(
request=cmd)
# Send the request
robot.logger.info('Running arm surface contact...')
arm_surface_contact_client.arm_surface_contact_command(proto)
time.sleep(traj_time + 5.0)
robot.logger.info('Turning off...')
# 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 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)
def _unstow(self):
self._start_robot_command('stow',
RobotCommandBuilder.arm_ready_command())
def gaze_control(config):
"""Commanding a gaze 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('GazeDemoClient')
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.")
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)
# Convert the location from the moving base frame to the world 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)
# Look at a point 3 meters in front and 4 meters to the left.
# We are not specifying a hand location, the robot will pick one.
gaze_target_in_odom = odom_T_flat_body.transform_point(x=3.0,
y=4.0,
z=0)
gaze_command = RobotCommandBuilder.arm_gaze_command(
gaze_target_in_odom[0], gaze_target_in_odom[1],
gaze_target_in_odom[2], ODOM_FRAME_NAME)
# Make the open gripper RobotCommand
gripper_command = RobotCommandBuilder.claw_gripper_open_command()
# Combine the arm and gripper commands into one RobotCommand
synchro_command = RobotCommandBuilder.build_synchro_command(
gripper_command, gaze_command)
# Send the request
robot.logger.info("Requesting gaze.")
command_client.robot_command(synchro_command)
time.sleep(4.0)
# Look to the left and the right with the hand.
# Robot's frame is X+ forward, Z+ up, so left and right is +/- in Y.
x = 4.0 # look 4 meters ahead
start_y = 2.0
end_y = -2.0
z = 0 # Look ahead, not up or down
traj_time = 5.5 # take 5.5 seconds to look from left to right.
start_pos_in_odom_tuple = odom_T_flat_body.transform_point(
x=x, y=start_y, z=z)
start_pos_in_odom = geometry_pb2.Vec3(x=start_pos_in_odom_tuple[0],
y=start_pos_in_odom_tuple[1],
z=start_pos_in_odom_tuple[2])
end_pos_in_odom_tuple = odom_T_flat_body.transform_point(x=x,
y=end_y,
z=z)
end_pos_in_odom = geometry_pb2.Vec3(x=end_pos_in_odom_tuple[0],
y=end_pos_in_odom_tuple[1],
z=end_pos_in_odom_tuple[2])
# Create the trajectory points
point1 = trajectory_pb2.Vec3TrajectoryPoint(
point=start_pos_in_odom)
duration_seconds = int(traj_time)
duration_nanos = int((traj_time - duration_seconds) * 1e9)
point2 = trajectory_pb2.Vec3TrajectoryPoint(
point=end_pos_in_odom,
time_since_reference=duration_pb2.Duration(
seconds=duration_seconds, nanos=duration_nanos))
# Build the trajectory proto
traj_proto = trajectory_pb2.Vec3Trajectory(points=[point1, point2])
# Build the proto
gaze_cmd = arm_command_pb2.GazeCommand.Request(
target_trajectory_in_frame1=traj_proto,
frame1_name=ODOM_FRAME_NAME,
frame2_name=ODOM_FRAME_NAME)
arm_command = arm_command_pb2.ArmCommand.Request(
arm_gaze_command=gaze_cmd)
synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request(
arm_command=arm_command)
command = robot_command_pb2.RobotCommand(
synchronized_command=synchronized_command)
# Make the open gripper RobotCommand
gripper_command = RobotCommandBuilder.claw_gripper_open_command()
# Combine the arm and gripper commands into one RobotCommand
synchro_command = RobotCommandBuilder.build_synchro_command(
gripper_command, command)
# Send the request
gaze_cmd_id = command_client.robot_command(command)
robot.logger.info('Sending gaze trajectory.')
# Wait until the robot completes the gaze before issuing the next command.
block_until_arm_arrives(command_client,
gaze_cmd_id,
timeout_sec=traj_time + 3.0)
# ------------- #
# Now make a gaze trajectory that moves the hand around while maintaining the gaze.
# We'll use the same trajectory as before, but add a trajectory for the hand to move to.
# Hand will start to the left (while looking left) and move to the right.
hand_x = 0.75 # in front of the robot.
hand_y = 0 # centered
hand_z_start = 0 # body height
hand_z_end = 0.25 # above body height
hand_vec3_start = geometry_pb2.Vec3(x=hand_x,
y=hand_y,
z=hand_z_start)
hand_vec3_end = geometry_pb2.Vec3(x=hand_x, y=hand_y, z=hand_z_end)
# We specify an orientation for the hand, which the robot will use its remaining degree
# of freedom to achieve. Most of it will be ignored in favor of the gaze direction.
qw = 1
qx = 0
qy = 0
qz = 0
quat = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz)
# Build a 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)
hand_pose1_in_odom = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
hand_pose1_in_flat_body)
hand_pose2_in_odom = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
hand_pose2_in_flat_body)
traj_point1 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose1_in_odom.to_proto())
# We'll make this trajectory the same length as the one above.
traj_point2 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose2_in_odom.to_proto(),
time_since_reference=duration_pb2.Duration(
seconds=duration_seconds, nanos=duration_nanos))
hand_traj = trajectory_pb2.SE3Trajectory(
points=[traj_point1, traj_point2])
# Build the proto
gaze_cmd = arm_command_pb2.GazeCommand.Request(
target_trajectory_in_frame1=traj_proto,
frame1_name=ODOM_FRAME_NAME,
tool_trajectory_in_frame2=hand_traj,
frame2_name=ODOM_FRAME_NAME)
arm_command = arm_command_pb2.ArmCommand.Request(
arm_gaze_command=gaze_cmd)
synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request(
arm_command=arm_command)
command = robot_command_pb2.RobotCommand(
synchronized_command=synchronized_command)
# Make the open gripper RobotCommand
gripper_command = RobotCommandBuilder.claw_gripper_open_command()
# Combine the arm and gripper commands into one RobotCommand
synchro_command = RobotCommandBuilder.build_synchro_command(
gripper_command, command)
# Send the request
gaze_cmd_id = command_client.robot_command(synchro_command)
robot.logger.info('Sending gaze trajectory with hand movement.')
# Wait until the robot completes the gaze before powering off.
block_until_arm_arrives(command_client,
gaze_cmd_id,
timeout_sec=traj_time + 3.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)
def hello_arm(config):
"""A simple example of using the Boston Dynamics API to command 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('StowUnstowClient')
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.
verify_estop(robot)
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(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)
# Stow the arm
# Build the stow command using RobotCommandBuilder
stow = RobotCommandBuilder.arm_stow_command()
# Issue the command via the RobotCommandClient
command_client.robot_command(stow)
robot.logger.info("Stow command issued.")
time.sleep(3.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)
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)
