def handle_trajectory(self, req):
"""ROS actionserver execution handler to handle receiving a request to move to a location"""
if req.target_pose.header.frame_id != 'body':
self.trajectory_server.set_aborted(TrajectoryResult(False, 'frame_id of target_pose must be \'body\''))
return
if req.duration.data.to_sec() <= 0:
self.trajectory_server.set_aborted(TrajectoryResult(False, 'duration must be larger than 0'))
return
cmd_duration = rospy.Duration(req.duration.data.secs, req.duration.data.nsecs)
resp = self.spot_wrapper.trajectory_cmd(
goal_x=req.target_pose.pose.position.x,
goal_y=req.target_pose.pose.position.y,
goal_heading=math_helpers.Quat(
w=req.target_pose.pose.orientation.w,
x=req.target_pose.pose.orientation.x,
y=req.target_pose.pose.orientation.y,
z=req.target_pose.pose.orientation.z
).to_yaw(),
cmd_duration=cmd_duration.to_sec(),
precise_position=req.precise_positioning,
)
def timeout_cb(trajectory_server, _):
trajectory_server.publish_feedback(TrajectoryFeedback("Failed to reach goal, timed out"))
trajectory_server.set_aborted(TrajectoryResult(False, "Failed to reach goal, timed out"))
# Abort the actionserver if cmd_duration is exceeded - the driver stops but does not provide feedback to
# indicate this so we monitor it ourselves
cmd_timeout = rospy.Timer(cmd_duration, functools.partial(timeout_cb, self.trajectory_server), oneshot=True)
# The trajectory command is non-blocking but we need to keep this function up in order to interrupt if a
# preempt is requested and to return success if/when the robot reaches the goal. Also check the is_active to
# monitor whether the timeout_cb has already aborted the command
rate = rospy.Rate(10)
while not rospy.is_shutdown() and not self.trajectory_server.is_preempt_requested() and not self.spot_wrapper.at_goal and self.trajectory_server.is_active():
if self.spot_wrapper.near_goal:
if self.spot_wrapper._last_trajectory_command_precise:
self.trajectory_server.publish_feedback(TrajectoryFeedback("Near goal, performing final adjustments"))
else:
self.trajectory_server.publish_feedback(TrajectoryFeedback("Near goal"))
else:
self.trajectory_server.publish_feedback(TrajectoryFeedback("Moving to goal"))
rate.sleep()
# If still active after exiting the loop, the command did not time out
if self.trajectory_server.is_active():
cmd_timeout.shutdown()
if self.trajectory_server.is_preempt_requested():
self.trajectory_server.publish_feedback(TrajectoryFeedback("Preempted"))
self.trajectory_server.set_preempted()
self.spot_wrapper.stop()
if self.spot_wrapper.at_goal:
self.trajectory_server.publish_feedback(TrajectoryFeedback("Reached goal"))
self.trajectory_server.set_succeeded(TrajectoryResult(resp[0], resp[1]))
else:
self.trajectory_server.publish_feedback(TrajectoryFeedback("Failed to reach goal"))
self.trajectory_server.set_aborted(TrajectoryResult(False, "Failed to reach goal"))
def main():
import argparse
parser = argparse.ArgumentParser()
bosdyn.client.util.add_common_arguments(parser)
options = parser.parse_args()
# Create robot object.
sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster')
sdk.load_app_token(options.app_token)
robot = sdk.create_robot(options.hostname)
robot.authenticate(options.username, options.password)
# Check that an estop is connected with the robot so that the robot commands can be executed.
verify_estop(robot)
# Create the lease client.
lease_client = robot.ensure_client(LeaseClient.default_service_name)
lease = lease_client.acquire()
robot.time_sync.wait_for_sync()
lk = bosdyn.client.lease.LeaseKeepAlive(lease_client)
# Setup clients for the robot state and robot command services.
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
robot_command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
# Power on the robot and stand it up.
robot.power_on()
blocking_stand(robot_command_client)
# Get robot state information. Specifically, we are getting the vision_tform_body transform to understand
# the robot's current position in the vision frame.
vision_tform_body = get_vision_tform_body(
robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot)
# We want to command a trajectory to go forward one meter in the x-direction of the body.
# It is simple to define this trajectory relative to the body frame, since we know that will be
# just 1 meter forward in the x-axis of the body.
# Note that the rotation is just math_helpers.Quat(), which is the identity quaternion. We want the
# rotation of the body at the goal to match the rotation of the body currently, so we do not need
# to transform the rotation.
body_tform_goal = math_helpers.SE3Pose(x=1,
y=0,
z=0,
rot=math_helpers.Quat())
# We can then transform this transform to get the goal position relative to the vision frame.
vision_tform_goal = vision_tform_body * body_tform_goal
# Command the robot to go to the goal point in the vision frame. The command will stop at the new
# position in the vision frame.
robot_cmd = RobotCommandBuilder.trajectory_command(
goal_x=vision_tform_goal.x,
goal_y=vision_tform_goal.y,
goal_heading=vision_tform_goal.rot.to_yaw(),
frame_name=VISION_FRAME_NAME)
end_time = 2.0
robot_command_client.robot_command(lease=None,
command=robot_cmd,
end_time_secs=time.time() + end_time)
time.sleep(end_time)
# Get new robot state information after moving the robot. Here we are getting the transform odom_tform_body,
# which describes the robot body's position in the odom frame.
odom_tform_body = get_odom_tform_body(robot_state_client.get_robot_state().
kinematic_state.transforms_snapshot)
# We want to command a trajectory to go backwards one meter and to the left one meter.
# It is simple to define this trajectory relative to the body frame, since we know that will be
# just 1 meter backwards (negative-value) in the x-axis of the body and one meter left (positive-value)
# in the y-axis of the body.
body_tform_goal = math_helpers.SE3Pose(x=-1,
y=1,
z=0,
rot=math_helpers.Quat())
# We can then transform this transform to get the goal position relative to the odom frame.
odom_tform_goal = odom_tform_body * body_tform_goal
# Command the robot to go to the goal point in the odom frame. The command will stop at the new
# position in the odom frame.
robot_cmd = RobotCommandBuilder.trajectory_command(
goal_x=odom_tform_goal.x,
goal_y=odom_tform_goal.y,
goal_heading=odom_tform_goal.rot.to_yaw(),
frame_name=ODOM_FRAME_NAME)
end_time = 5.0
robot_command_client.robot_command(lease=None,
command=robot_cmd,
end_time_secs=time.time() + end_time)
time.sleep(end_time)
return True
def _do_poses_match(x, y, z, pose_b):
# Hacky approach with string representation
pose_a = math_helpers.SE3Pose(x, y, z, math_helpers.Quat())
return str(pose_a) == str(pose_b)
def move_relative(robot_command_client,
robot_state_client,
x,
y,
yaw,
start_frame=None,
timeout=10.0,
block=False,
verbose=False):
if start_frame is not None:
vision_tform_body = start_frame
else:
# Get robot state information. Specifically, we are getting the vision_tform_body transform to understand
# the robot's current position in the vision frame.
vision_tform_body = get_vision_tform_body(
robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot)
# We want to command a trajectory to go forward one meter in the x-direction of the body.
# It is simple to define this trajectory relative to the body frame, since we know that will be
# just 1 meter forward in the x-axis of the body.
# Note that the rotation is just math_helpers.Quat(), which is the identity quaternion. We want the
# rotation of the body at the goal to match the rotation of the body currently, so we do not need
# to transform the rotation.
rot_quat = math_helpers.Quat().from_yaw(yaw)
body_tform_goal = math_helpers.SE3Pose(x=x, y=y, z=0, rot=rot_quat)
# We can then transform this transform to get the goal position relative to the vision frame.
vision_tform_goal = vision_tform_body * body_tform_goal
# print ((vision_tform_body.x, vision_tform_body.y), (vision_tform_goal.x, vision_tform_goal.y), )
# print (np.rad2deg(vision_tform_body.rotation.to_yaw()), np.rad2deg(vision_tform_goal.rotation.to_yaw()))
# Command the robot to go to the goal point in the vision frame. The command will stop at the new
# position in the vision frame.
robot_cmd = RobotCommandBuilder.trajectory_command( # synchro_se2_trajectory_command( #
goal_x=vision_tform_goal.x,
goal_y=vision_tform_goal.y,
goal_heading=vision_tform_goal.rot.to_yaw(),
frame_name=VISION_FRAME_NAME)
# robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + timeout)
command_id = robot_command_client.robot_command(
lease=None, command=robot_cmd, end_time_secs=time.time() + timeout)
# This will only issue the command, but it is not blocking. So we wait and check status manually
now = time.time()
start_time = time.time()
end_time = time.time() + timeout
while now < end_time:
time_until_timeout = end_time - now
rpc_timeout = max(time_until_timeout, 1)
start_call_time = time.time()
try:
response = robot_command_client.robot_command_feedback(
command_id, timeout=rpc_timeout)
except TimedOutError:
# Excuse the TimedOutError and let the while check bail us out if we're out of time.
print("Response timeout error")
pass
else:
# print (response.status, robot_command_pb2.RobotCommandFeedbackResponse.STATUS_PROCESSING)
# pdb.set_trace()
if response.status != robot_command_pb2.RobotCommandFeedbackResponse.STATUS_PROCESSING:
raise ValueError(
'Stand (ID {}) no longer processing (now {})'.format(
command_id, response.Status.Name(response.status)))
if verbose:
print(
response.feedback.mobility_feedback.
se2_trajectory_feedback.status, basic_command_pb2.
SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL)
if (response.feedback.mobility_feedback.
se2_trajectory_feedback.status == basic_command_pb2.
SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL):
# basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING):
if verbose: print(response)
break
# delta_t = time.time() - start_call_time
# time.sleep(max(min(delta_t, update_time), 0.0))
time.sleep(0.1)
now = time.time()
if verbose: print("Took %f sec" % (time.time() - start_time))
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)