def set_default_body_control():
"""Set default body control params to current body position"""
footprint_R_body = geometry.EulerZXY()
position = geo.Vec3(x=0.0, y=0.0, z=0.0)
rotation = footprint_R_body.to_quaternion()
pose = geo.SE3Pose(position=position, rotation=rotation)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE3Trajectory(points=[point])
return spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj)
def bodyPoseCallback(self, data):
"""Callback for cmd_vel command"""
q = Quaternion()
q.x = data.orientation.x
q.y = data.orientation.y
q.z = data.orientation.z
q.w = data.orientation.w
position = geometry_pb2.Vec3(z=data.position.z)
pose = geometry_pb2.SE3Pose(position=position, rotation=q)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE3Trajectory(points=[point])
body_control = spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj)
mobility_params = self.spot_wrapper.get_mobility_params()
mobility_params.body_control.CopyFrom(body_control)
self.spot_wrapper.set_mobility_params(mobility_params)
def move_along_trajectory(frame, velocity, se3_poses):
""" Builds an ArmSE3PoseCommand the arm to a point at a specific speed. Builds a
trajectory from the current location to a new location
velocity is in m/s """
last_pose = None
last_t = 0
points = []
# Create a trajectory from the points
for pose in se3_poses:
if last_pose is None:
time_in_sec = 0
seconds = int(0)
nanos = int(0)
else:
# Compute the distance from the current hand position to the new hand position
dist = math.sqrt((last_pose.x - pose.x)**2 + (last_pose.y - pose.y)**2 +
(last_pose.z - pose.z)**2)
time_in_sec = dist / velocity
seconds = int(time_in_sec + last_t)
nanos = int((time_in_sec + last_t - seconds) * 1e9)
position = geometry_pb2.Vec3(x=pose.x, y=pose.y, z=pose.z)
rotation = geometry_pb2.Quaternion(w=pose.rot.w, x=pose.rot.x, y=pose.rot.y, z=pose.rot.z)
this_se3_pose = geometry_pb2.SE3Pose(position=position, rotation=rotation)
points.append(
trajectory_pb2.SE3TrajectoryPoint(
pose=this_se3_pose,
time_since_reference=duration_pb2.Duration(seconds=seconds, nanos=nanos)))
last_pose = pose
last_t = time_in_sec + last_t
hand_trajectory = trajectory_pb2.SE3Trajectory(points=points)
return hand_trajectory
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 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 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 arm_trajectory(config):
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('ArmTrajectory')
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)
# Move the arm along a simple trajectory
vo_T_flat_body = get_a_tform_b(
robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot, VISION_FRAME_NAME,
GRAV_ALIGNED_BODY_FRAME_NAME)
x = 0.75 # a reasonable position in front of the robot
y1 = 0 # centered
y2 = 0.4 # 0.4 meters to the robot's left
y3 = -0.4 # 0.4 meters to the robot's right
z = 0 # at the body's height
# Use the same rotation as the robot's body.
rotation = math_helpers.Quat()
t_first_point = 0 # first point starts at t = 0 for the trajectory.
t_second_point = 4.0 # trajectory will last 1.0 seconds
t_third_point = 8.0 # trajectory will last 1.0 seconds
# Build the two points in the trajectory
hand_pose1 = math_helpers.SE3Pose(x=x, y=y1, z=z, rot=rotation)
hand_pose2 = math_helpers.SE3Pose(x=x, y=y2, z=z, rot=rotation)
hand_pose3 = math_helpers.SE3Pose(x=x, y=y3, z=z, rot=rotation)
# Build the points by combining the pose and times into protos.
traj_point1 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose1.to_proto(),
time_since_reference=seconds_to_duration(t_first_point))
traj_point2 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose2.to_proto(),
time_since_reference=seconds_to_duration(t_second_point))
traj_point3 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose3.to_proto(),
time_since_reference=seconds_to_duration(t_third_point))
# Build the trajectory proto by combining the two points
hand_traj = trajectory_pb2.SE3Trajectory(
points=[traj_point1, traj_point2, traj_point3])
# Build the command by taking the trajectory and specifying the frame it is expressed
# in.
#
# In this case, we want to specify the trajectory in the body's frame, so we set the
# root frame name to the the flat body frame.
arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request(
pose_trajectory_in_task=hand_traj,
root_frame_name=GRAV_ALIGNED_BODY_FRAME_NAME)
# Pack everything up in protos.
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)
# Keep the gripper closed the whole time.
robot_command = RobotCommandBuilder.claw_gripper_open_fraction_command(
0, build_on_command=robot_command)
robot.logger.info("Sending trajectory command...")
# Send the trajectory to the robot.
cmd_id = command_client.robot_command(robot_command)
# Wait until the arm arrives at the goal.
while True:
feedback_resp = command_client.robot_command_feedback(cmd_id)
robot.logger.info(
'Distance to final point: ' + '{:.2f} meters'.format(
feedback_resp.feedback.synchronized_feedback.
arm_command_feedback.arm_cartesian_feedback.
measured_pos_distance_to_goal) +
', {:.2f} radians'.format(
feedback_resp.feedback.synchronized_feedback.
arm_command_feedback.arm_cartesian_feedback.
measured_rot_distance_to_goal))
if feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.status == arm_command_pb2.ArmCartesianCommand.Feedback.STATUS_TRAJECTORY_COMPLETE:
robot.logger.info('Move complete.')
break
time.sleep(0.1)
# 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)
