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]