Trajectory · repose-trajectory

Cartesian blending

Industrial programs often say "move linearly to P1, round the corner within 5 mm, continue to P2". Cartesian blending does that: straight tool lines between waypoints, rounded corners inside each waypoint's zone, IK and collision checks on every generated point.

#Waypoints and zones

let waypoints = [
    CuCartesianWaypoint::new(q_start, m(0.0)),     // exact stop (first must be)
    CuCartesianWaypoint::new(q_p1, m(0.005)),      // fly by within 5 mm of P1's TCP
    CuCartesianWaypoint::new(q_p2, m(0.0)),        // exact stop in the middle
    CuCartesianWaypoint::new(q_end, m(0.0)),       // exact stop (last must be)
];

Waypoints are joint configurations; their TCP positions (via FK) define the lines. A zero zone is an exact stop. A positive zone is a fly-by sphere around the waypoint's TCP: the path leaves the line at the zone's edge and rejoins the next line at the edge, never leaving the sphere.

#Step 1: geometry

let mut path = CuCartesianBlendPath::<6, 512>::default();
let status = blend_cartesian_path(&chain, &model, &env, &waypoints, &CuCartesianBlendParams::default(), &mut path)?;

Each zone trims the incoming and outgoing lines and joins them with a quadratic Bézier inside the zone. Tool orientation follows shortest-arc spherical interpolation. The path is sampled so the TCP moves at most translation_step and rotates at most rotation_step between points; IK solves each point from the previous solution, and every edge is collision-checked at collision_step.

CuCartesianBlendStatus::InverseKinematicsFailed { waypoint, step, steps, corner } says exactly where IK failed: on the line to a waypoint or on the rounded corner in its zone. The output is cleared on any failure.

#Step 2: timing

let params = CuPathTimingParams { timing: timing_params, collision_step: CuJointResolution::default() };
let status = parameterize_cartesian_path(&chain, &model, &env, &path, &params, &mut traj)?;

The sampled points are joined by a C² cubic spline. Exact stops split the path into rest-to-rest pieces; inside a piece each zone is crossed at a constant corner speed bounded by the zone's curvature, and moves between stops and corners follow the chosen profile. The result is checked on the reconstruction the follower will execute (quintic for S-curve, cubic otherwise), stretched uniformly if a peak exceeds a limit, and rechecked for collisions.

#API reference

• CuCartesianWaypoint
• CuCartesianBlendPath
• CuCartesianBlendParams
• CuIkParams
• CuCartesianBlendStatus
• CuPathTimingParams
• CuJointResolution