def test_arm_pose_command(): x = 0.75 y = 0 z = 0.25 qw = 1 qx = 0 qy = 0 qz = 0 command = RobotCommandBuilder.arm_pose_command(x, y, z, qw, qx, qy, qz, BODY_FRAME_NAME) _test_has_synchronized(command) _test_has_arm(command.synchronized_command) assert command.synchronized_command.arm_command.HasField("arm_cartesian_command") arm_cartesian_command = command.synchronized_command.arm_command.arm_cartesian_command assert arm_cartesian_command.root_frame_name == BODY_FRAME_NAME assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.position.x == x assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.position.y == y assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.position.z == z assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.rotation.x == qx assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.rotation.y == qy assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.rotation.z == qz assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.rotation.w == qw # with a build_on_command mobility_command = RobotCommandBuilder.synchro_sit_command() command = RobotCommandBuilder.arm_pose_command(x, y, z, qw, qx, qy, qz, BODY_FRAME_NAME, build_on_command=mobility_command) _test_has_synchronized(command) _test_has_mobility(command.synchronized_command) _test_has_arm(command.synchronized_command)
def _move(self, left_x, left_y, right_x): """Commands the robot with a velocity command based on left/right stick values. Args: left_x: X value of left stick. left_y: Y value of left stick. right_x: X value of right stick. """ # Stick left_x controls robot v_y v_y = -left_x * VELOCITY_BASE_SPEED # Stick left_y controls robot v_x v_x = left_y * VELOCITY_BASE_SPEED # Stick right_x controls robot v_rot v_rot = -right_x * VELOCITY_BASE_ANGULAR # Recreate mobility_params with the latest information self.mobility_params = RobotCommandBuilder.mobility_params( body_height=self.body_height, locomotion_hint=self.mobility_params.locomotion_hint, stair_hint=self.mobility_params.stair_hint) cmd = RobotCommandBuilder.velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot, params=self.mobility_params) self.command_client.robot_command_async(cmd, end_time_secs=time.time() + VELOCITY_CMD_DURATION)
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 test_synchro_sit_command(): command = RobotCommandBuilder.synchro_sit_command() _test_has_synchronized(command) _test_has_mobility(command.synchronized_command) assert command.synchronized_command.mobility_command.HasField("sit_request") # with a build_on_command arm_command = RobotCommandBuilder.arm_stow_command() command = RobotCommandBuilder.synchro_sit_command(build_on_command=arm_command) _test_has_synchronized(command) _test_has_mobility(command.synchronized_command) _test_has_arm(command.synchronized_command)
def test_claw_gripper_close_command(): command = RobotCommandBuilder.claw_gripper_close_command() _test_has_synchronized(command) _test_has_gripper(command.synchronized_command) assert (command.synchronized_command.gripper_command.WhichOneof("command") == "claw_gripper_command") # with a build_on_command mobility_command = RobotCommandBuilder.synchro_sit_command() command = RobotCommandBuilder.claw_gripper_close_command(build_on_command=mobility_command) _test_has_synchronized(command) _test_has_mobility(command.synchronized_command) _test_has_gripper(command.synchronized_command)
def test_arm_carry_command(): command = RobotCommandBuilder.arm_ready_command() _test_has_synchronized(command) _test_has_arm(command.synchronized_command) assert (command.synchronized_command.arm_command.WhichOneof("command") == 'named_arm_position_command') # with a build_on_command mobility_command = RobotCommandBuilder.synchro_sit_command() command = RobotCommandBuilder.arm_carry_command(build_on_command=mobility_command) _test_has_synchronized(command) _test_has_mobility(command.synchronized_command) _test_has_arm(command.synchronized_command)
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 velocity_cmd_listener(self, twist): """Callback that sends instantaneous velocity [m/s] commands to Spot""" MAX_MARCHING_TIME = 5 v_x = twist.linear.x v_y = twist.linear.y v_rot = twist.angular.z all_zero = (v_x == 0) and (v_y == 0) and (v_rot == 0) if (all_zero): if self.t_last_non_zero_cmd < time.monotonic() - MAX_MARCHING_TIME: # No need to send command, just return. return else: self.t_last_non_zero_cmd = time.monotonic() cmd = RobotCommandBuilder.velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot) try: self.command_client.robot_command(cmd, end_time_secs=time.time() + self.VELOCITY_CMD_DURATION) except: print("Invalid command: v_x=${},v_y=${},v_rot${}".format( v_x, v_y, v_rot)) # rospy.loginfo( # "Robot velocity cmd sent: v_x=${},v_y=${},v_rot${}".format(v_x, v_y, v_rot)) return []
def test_synchro_se2_trajectory_command(): goal_x = 1.0 goal_y = 2.0 goal_heading = 3.0 frame = ODOM_FRAME_NAME position = geometry_pb2.Vec2(x=goal_x, y=goal_y) goal_se2 = geometry_pb2.SE2Pose(position=position, angle=goal_heading) command = RobotCommandBuilder.synchro_se2_trajectory_command(goal_se2, 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_command(goal_se2, 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 __execute_velocity(self, description, v_x=0.0, v_y=0.0, v_rot=0.0): self.__execute_command( description, RobotCommandBuilder.synchro_velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot), time.time() + self.__VELOCITY_CMD_DURATION)
def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0): self._start_robot_command(desc, RobotCommandBuilder.velocity_command( v_x=v_x, v_y=v_y, v_rot=v_rot, body_height=self.height, locomotion_hint=spot_command_pb2.HINT_AMBLE), end_time_secs=time.time() + VELOCITY_CMD_DURATION)
def _return_to_origin(self): self._start_robot_command('fwd_and_rotate', RobotCommandBuilder.trajectory_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 construct_hold_pose_task(): """ Helper function for holding the pose of the hand Use this if you want to hold the position of the hand, without leaving constrained manipulation. Output: + command: api command object """ frame_name = "hand" force_lim = 80 torque_lim = 10 force_direction = geometry_pb2.Vec3(x=1.0, y=0.0, z=0.0) torque_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0) init_wrench_dir = geometry_pb2.Wrench(force=force_direction, torque=torque_direction) task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_HOLD_POSE command = RobotCommandBuilder.constrained_manipulation_command( task_type=task_type, init_wrench_direction_in_frame_name=init_wrench_dir, force_limit=force_lim, torque_limit=torque_lim, tangential_speed=0.0, frame_name=frame_name) return command
def self_right_cmd_srv(self, stand): """ Callback that sends self-right cmd""" cmd = RobotCommandBuilder.selfright_command() ret = self.command_client.robot_command(cmd) rospy.loginfo("Robot self right cmd sent. {}".format(ret)) return []
def trajectory_cmd_srv(self, trajectory): ''' Callback that specifies waypoint(s) (Point) [m] with a final orientation [rad] The name of the frame that trajectory is relative to. The trajectory must be expressed in a gravity aligned frame, so either "vision", "odom", or "flat_body". Any other provided se2_frame_name will be rejected and the trajectory command will not be exectuted. ''' # TODO: Support other reference frames (currently only VISION ref. frame) for pose in trajectory.waypoints.poses: x = pose.position.x y = pose.position.y heading = math.atan2(y,x) frame = VISION_FRAME_NAME cmd = RobotCommandBuilder.trajectory_command( goal_x=x, goal_y=y, goal_heading=heading, frame_name=frame, ) self.command_client.robot_command(lease=None, command=cmd, end_time_secs=time.time() + self.TRAJECTORY_CMD_TIMEOUT) robot_state = self.get_robot_state()[0].vision_tform_body final_pose = geometry_msgs.msg.Pose() final_pose.position = robot_state.translation final_pose.orientation = robot_state.rotation spot_ros_srvs.srv.TrajectoryResponse(final_pose)
def power_off(self): """Power off the robot.""" safe_power_off_cmd = RobotCommandBuilder.safe_power_off_command() self._robot_command_client.robot_command(lease=None, command=safe_power_off_cmd) time.sleep(2.5) print("Powered Off " + str(not self._robot.is_powered_on()))
def pitch_up(robot): """Pitch robot up to look for door handle. Args: robot: (Robot) Interface to Spot robot. """ robot.logger.info("Pitching robot up...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) footprint_R_body = geometry.EulerZXY(0.0, 0.0, -1 * math.pi / 6.0) cmd = RobotCommandBuilder.synchro_stand_command( footprint_R_body=footprint_R_body) cmd_id = command_client.robot_command(cmd) timeout_sec = 10.0 end_time = time.time() + timeout_sec while time.time() < end_time: response = command_client.robot_command_feedback(cmd_id) synchronized_feedback = response.feedback.synchronized_feedback status = synchronized_feedback.mobility_command_feedback.stand_feedback.status if (status == basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING): robot.logger.info("Robot pitched.") return time.sleep(1.0) raise Exception("Failed to pitch robot.")
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.trajectory_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 _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0): self._start_robot_command( desc, RobotCommandBuilder.synchro_velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot), end_time_secs=time.time() + VELOCITY_CMD_DURATION)
def _on_quit(self): """Cleanup on quit from the command line interface.""" # Sit the robot down + power off after the navigation command is complete. if self._powered_on and not self._started_powered_on: self._robot_command_client.robot_command( RobotCommandBuilder.safe_power_off_command(), end_time_secs=time.time())
def _selfright(self): """Executes selfright command, which causes the robot to automatically turn if it is on its back. """ cmd = RobotCommandBuilder.selfright_command() self._issue_robot_command(cmd)
def get_go_to(world_tform_object, robot_state, mobility_params, dist_margin=1.2): """Gets trajectory command to a goal location Args: world_tform_object (SE3Pose): Transform from vision frame to target object robot_state (RobotState): Current robot state mobility_params (MobilityParams): Mobility parameters dist_margin (float): Distance margin to target """ vo_tform_robot = get_vision_tform_body( robot_state.kinematic_state.transforms_snapshot) delta_ewrt_vo = np.array([ world_tform_object.x - vo_tform_robot.x, world_tform_object.y - vo_tform_robot.y, 0 ]) norm = np.linalg.norm(delta_ewrt_vo) if norm == 0: return None delta_ewrt_vo_norm = delta_ewrt_vo / norm heading = _get_heading(delta_ewrt_vo_norm) vo_tform_goal = np.array([ world_tform_object.x - delta_ewrt_vo_norm[0] * dist_margin, world_tform_object.y - delta_ewrt_vo_norm[1] * dist_margin ]) tag_cmd = RobotCommandBuilder.trajectory_command( goal_x=vo_tform_goal[0], goal_y=vo_tform_goal[1], goal_heading=heading, frame_name=VISION_FRAME_NAME, params=mobility_params) return tag_cmd
def _battery_change_pose(self): # Default HINT_RIGHT, maybe add option to choose direction? self._start_robot_command( 'battery_change_pose', RobotCommandBuilder.battery_change_pose_command( dir_hint=basic_command_pb2.BatteryChangePoseCommand.Request. HINT_RIGHT))
def stand(self, monitor_command=True): """If the e-stop is enabled, and the motor power is enabled, stand the robot up.""" response = self._robot_command( RobotCommandBuilder.stand_command(params=self._mobility_params)) if monitor_command: self._last_stand_command = response[2] return response[0], response[1]
def get_walking_params(max_linear_vel, max_rotation_vel): max_vel_linear = geometry_pb2.Vec2(x=max_linear_vel, y=max_linear_vel) max_vel_se2 = geometry_pb2.SE2Velocity(linear=max_vel_linear, angular=max_rotation_vel) vel_limit = geometry_pb2.SE2VelocityLimit(max_vel=max_vel_se2) params = RobotCommandBuilder.mobility_params() params.vel_limit.CopyFrom(vel_limit) return params
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 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 _battery_change_pose(self): """Executes the battery-change pose command which causes the robot to sit down if standing then roll to its [right]/left side for easier battery changing. """ cmd = RobotCommandBuilder.battery_change_pose_command( dir_hint=basic_command_pb2.BatteryChangePoseCommand.Request. HINT_RIGHT) self._issue_robot_command(cmd)
def _selfright(self): """Executes selfright command, which causes the robot to automatically turn if it is on its back. """ if not self.motors_powered: return cmd = RobotCommandBuilder.selfright_command() self.command_client.robot_command_async(cmd)
def _force_safe_power_off(self): """Safely powers off the robot. """ if self.robot.is_powered_on(): lease = self.lease_client.take() with LeaseKeepAlive(self.lease_client): cmd = RobotCommandBuilder.safe_power_off_command() self.command_client.robot_command(cmd) self.lease_client.return_lease(lease)