def test_synchro_se2_trajectory_point_command():
goal_x = 1.0
goal_y = 2.0
goal_heading = 3.0
frame = ODOM_FRAME_NAME
command = RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x, goal_y, goal_heading, frame)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
# with a build_on_command
arm_command = RobotCommandBuilder.arm_stow_command()
command = RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x, goal_y, goal_heading, frame, build_on_command=arm_command)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
_test_has_arm(command.synchronized_command)
def go_to_tag(self, fiducial_rt_world):
"""Use the position of the april tag in vision world frame and command the robot."""
# Compute the go-to point (offset by .5m from the fiducial position) and the heading at
# this point.
self._current_tag_world_pose, self._angle_desired = self.offset_tag_pose(
fiducial_rt_world, self._tag_offset)
#Command the robot to go to the tag in kinematic odometry frame
mobility_params = self.set_mobility_params()
tag_cmd = RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x=self._current_tag_world_pose[0],
goal_y=self._current_tag_world_pose[1],
goal_heading=self._angle_desired,
frame_name=VISION_FRAME_NAME,
params=mobility_params,
body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_AUTO)
end_time = 5.0
if self._movement_on and self._powered_on:
#Issue the command to the robot
self._robot_command_client.robot_command(
lease=None,
command=tag_cmd,
end_time_secs=time.time() + end_time)
# #Feedback to check and wait until the robot is in the desired position or timeout
start_time = time.time()
current_time = time.time()
while (not self.final_state()
and current_time - start_time < end_time):
time.sleep(.25)
current_time = time.time()
return
def test_synchro_se2_trajectory_point_command():
goal_x = 1.0
goal_y = 2.0
goal_heading = 3.0
frame = ODOM_FRAME_NAME
command = RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x, goal_y, goal_heading, frame)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
def _return_to_origin(self):
self._start_robot_command(
'fwd_and_rotate',
RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x=0.0,
goal_y=0.0,
goal_heading=0.0,
frame_name=ODOM_FRAME_NAME,
params=None,
body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_SPEED_SELECT_TROT),
end_time_secs=time.time() + 20)
def relative_move(dx,
dy,
dyaw,
frame_name,
robot_command_client,
robot_state_client,
stairs=False):
transforms = robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot
# Build the transform for where we want the robot to be relative to where the body currently is.
body_tform_goal = math_helpers.SE2Pose(x=dx, y=dy, angle=dyaw)
# We do not want to command this goal in body frame because the body will move, thus shifting
# our goal. Instead, we transform this offset to get the goal position in the output frame
# (which will be either odom or vision).
out_tform_body = get_se2_a_tform_b(transforms, frame_name, BODY_FRAME_NAME)
out_tform_goal = out_tform_body * body_tform_goal
# Command the robot to go to the goal point in the specified frame. The command will stop at the
# new position.
robot_cmd = RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x=out_tform_goal.x,
goal_y=out_tform_goal.y,
goal_heading=out_tform_goal.angle,
frame_name=frame_name,
params=RobotCommandBuilder.mobility_params(stair_hint=stairs))
end_time = 10.0
cmd_id = robot_command_client.robot_command(lease=None,
command=robot_cmd,
end_time_secs=time.time() +
end_time)
# Wait until the robot has reached the goal.
while True:
feedback = robot_command_client.robot_command_feedback(cmd_id)
mobility_feedback = feedback.feedback.synchronized_feedback.mobility_command_feedback
if mobility_feedback.status != RobotCommandFeedbackStatus.STATUS_PROCESSING:
print("Failed to reach the goal")
return False
traj_feedback = mobility_feedback.se2_trajectory_feedback
if (traj_feedback.status == traj_feedback.STATUS_AT_GOAL
and traj_feedback.body_movement_status
== traj_feedback.BODY_STATUS_SETTLED):
print("Arrived at the goal.")
return True
time.sleep(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')
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.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=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.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=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 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]
