Ejemplo n.º 1
0
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)
Ejemplo n.º 2
0
    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)
Ejemplo n.º 3
0
    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]
Ejemplo n.º 4
0
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)
Ejemplo n.º 5
0
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)
Ejemplo n.º 6
0
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)
Ejemplo n.º 7
0
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)
Ejemplo n.º 8
0
    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 []
Ejemplo n.º 9
0
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)
Ejemplo n.º 10
0
 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)
Ejemplo n.º 11
0
 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)
Ejemplo n.º 12
0
 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)
Ejemplo n.º 13
0
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)
Ejemplo n.º 16
0
 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()))
Ejemplo n.º 17
0
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.")
Ejemplo n.º 18
0
    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
Ejemplo n.º 19
0
 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)
Ejemplo n.º 20
0
 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())
Ejemplo n.º 21
0
    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)
Ejemplo n.º 22
0
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
Ejemplo n.º 23
0
Archivo: wasd.py Proyecto: wau/spot-sdk
 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))
Ejemplo n.º 24
0
 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]
Ejemplo n.º 25
0
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
Ejemplo n.º 26
0
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)
Ejemplo n.º 27
0
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)
Ejemplo n.º 28
0
    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)
Ejemplo n.º 29
0
    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)
Ejemplo n.º 30
0
    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)