Motion planning · repose-motion-planning

Plan-and-execute pipeline

Two small tasks connect planning to the rest of the stack, so a full "plan from where I am to there, then do it" loop is pure configuration.

text
joint_state ─► CuJointGoalTask ─► planner task ─► CuPlanToTrajectoryTask ─► CuFollowTrajectoryTask ─► CuControllerTask ─► bridge

CuJointGoalTask

In: CuJointStatePayload<N>. Out: CuJointPlanRequest<N>. It waits for the first state that reports Ready, uses it as the start, and publishes one request to the configured goal. Every other cycle publishes nothing, so the planner runs exactly one job.

ron
(id: "goal", type: "repose_motion_planning::CuJointGoalTask<6>",
 config: {"goal": [{"revolute": 0.4}, {"revolute": -0.3}, {"revolute": 0.5},
                   {"revolute": 0.0}, {"revolute": 0.4}, {"revolute": 0.0}]}),

The request carries an empty scene, so only self-collision is checked. For obstacles, or for more than one goal, write your own source task that publishes CuJointPlanRequests.

CuPlanToTrajectoryTask

In: CuJointPlanPath<N>. Out: CuTrajectoryPayload<N, CAP>. It times each final path into a trajectory within the configured limits (see Timing). Failure statuses and empty cycles publish nothing. A path that can't be timed within the limits or the capacity CAP is an error.

ron
(id: "timing", type: "repose_motion_planning::CuPlanToTrajectoryTask<6, 1024>",
 config: {
    "profile": "cubic",          // trapezoidal (default) | s_curve | minimum_jerk | cubic
    "sample_period": 0.02,       // s, default 0.02
    "limits": {
        "max_velocities":    [{"revolute": 0.8}, {"revolute": 0.8}, {"revolute": 0.8},
                              {"revolute": 0.8}, {"revolute": 0.8}, {"revolute": 0.8}],
        "max_accelerations": [{"revolute": 2.0}, {"revolute": 2.0}, {"revolute": 2.0},
                              {"revolute": 2.0}, {"revolute": 2.0}, {"revolute": 2.0}],
    },
 }),

Timing doesn't recheck collisions: the interpolated motion follows the planned straight segments closely, and the path's scene_revision says which scene it was checked against.

The connections

cnx
(src: "robot/joint_state", dst: "goal",       msg: "repose_kinematics::CuJointStatePayload<6>"),
(src: "goal",              dst: "planner",    msg: "repose_motion_planning::CuJointPlanRequest<6>"),
(src: "planner",           dst: "timing",     msg: "repose_motion_planning::CuJointPlanPath<6>"),
(src: "timing",            dst: "follower",   msg: "repose_trajectory::CuTrajectoryPayload<6, 1024>"),
(src: "follower",          dst: "controller", msg: "repose_trajectory::CuTrajectoryFollowOutput<6>"),
(src: "robot/joint_state", dst: "controller", msg: "repose_kinematics::CuJointStatePayload<6>"),
(src: "controller",        dst: "robot/joint_command", msg: "repose_controller::CuJointCommandPayload<6>"),

The MuJoCo example is exactly this graph, running against a simulated ReBeL.

API reference