def trajectory_cmd(self,
goal_x,
goal_y,
goal_heading,
cmd_duration,
frame_name='odom'):
"""Send a trajectory motion command to the robot.
Args:
goal_x: Position X coordinate in meters
goal_y: Position Y coordinate in meters
goal_heading: Pose heading in radians
cmd_duration: Time-to-live for the command in seconds.
frame_name: frame_name to be used to calc the target position. 'odom' or 'vision'
"""
self._at_goal = False
self._logger.info("got command duration of {}".format(cmd_duration))
end_time = time.time() + cmd_duration
if frame_name == 'vision':
vision_tform_body = frame_helpers.get_vision_tform_body(
self._robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot)
body_tform_goal = math_helpers.SE3Pose(
x=goal_x,
y=goal_y,
z=0,
rot=math_helpers.Quat.from_yaw(goal_heading))
vision_tform_goal = vision_tform_body * body_tform_goal
response = self._robot_command(
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=frame_helpers.VISION_FRAME_NAME,
params=self._mobility_params),
end_time_secs=end_time)
elif frame_name == 'odom':
odom_tform_body = frame_helpers.get_odom_tform_body(
self._robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot)
body_tform_goal = math_helpers.SE3Pose(
x=goal_x,
y=goal_y,
z=0,
rot=math_helpers.Quat.from_yaw(goal_heading))
odom_tform_goal = odom_tform_body * body_tform_goal
response = self._robot_command(
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=frame_helpers.ODOM_FRAME_NAME,
params=self._mobility_params),
end_time_secs=end_time)
else:
raise ValueError('frame_name must be \'vision\' or \'odom\'')
if response[0]:
self._last_trajectory_command = response[2]
return response[0], response[1]
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 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 _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 trajectory_cmd(self,
goal_x,
goal_y,
goal_heading,
cmd_duration,
frame_name='odom',
precise_position=False):
"""Send a trajectory motion command to the robot.
Args:
goal_x: Position X coordinate in meters
goal_y: Position Y coordinate in meters
goal_heading: Pose heading in radians
cmd_duration: Time-to-live for the command in seconds.
frame_name: frame_name to be used to calc the target position. 'odom' or 'vision'
precise_position: if set to false, the status STATUS_NEAR_GOAL and STATUS_AT_GOAL will be equivalent. If
true, the robot must complete its final positioning before it will be considered to have successfully
reached the goal.
"""
self._at_goal = False
self._near_goal = False
self._last_trajectory_command_precise = precise_position
self._logger.info("got command duration of {}".format(cmd_duration))
end_time = time.time() + cmd_duration
if frame_name == 'vision':
vision_tform_body = frame_helpers.get_vision_tform_body(
self._robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot)
body_tform_goal = math_helpers.SE3Pose(
x=goal_x,
y=goal_y,
z=0,
rot=math_helpers.Quat.from_yaw(goal_heading))
vision_tform_goal = vision_tform_body * body_tform_goal
response = self._robot_command(
RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x=vision_tform_goal.x,
goal_y=vision_tform_goal.y,
goal_heading=vision_tform_goal.rot.to_yaw(),
frame_name=frame_helpers.VISION_FRAME_NAME,
params=self._mobility_params),
end_time_secs=end_time)
elif frame_name == 'odom':
odom_tform_body = frame_helpers.get_odom_tform_body(
self._robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot)
body_tform_goal = math_helpers.SE3Pose(
x=goal_x,
y=goal_y,
z=0,
rot=math_helpers.Quat.from_yaw(goal_heading))
odom_tform_goal = odom_tform_body * body_tform_goal
response = self._robot_command(
RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x=odom_tform_goal.x,
goal_y=odom_tform_goal.y,
goal_heading=odom_tform_goal.rot.to_yaw(),
frame_name=frame_helpers.ODOM_FRAME_NAME,
params=self._mobility_params),
end_time_secs=end_time)
else:
raise ValueError('frame_name must be \'vision\' or \'odom\'')
if response[0]:
self._last_trajectory_command = response[2]
return response[0], response[1]
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)