Kinematics · repose-kinematics

Servoing

Servoing drives the tool by velocity instead of by target pose: "move 2 cm/s along z" each cycle, from a joystick, a visual tracker or a force loop. Differential IK turns that tool twist into joint velocities and stays well-behaved near singularities and limits.

One solve

rust
let mut qd = [CuJointVelocity::default(); 6];
let status = chain.differential_ik(&q, &twist, &CuDiffIkParams::default(), &mut qd)?;

Integrate qd over your cycle time, or send it to a velocity-controlled bridge. Run it again next cycle from the new measured q.

What it does

  1. Pick the Jacobian matching the twist's frame: space for Base, body for Body.
  2. Measure manipulability w². Above singularity_threshold_sq, damping is the constant base_damping. Below it, damping grows smoothly up to base_damping + max_singularity_damping at an exact singularity.
  3. Solve damped least squares q̇ = Jᵀ (J Jᵀ + λ² I)⁻¹ v. The tool slows down in directions it can barely move in, instead of the joints racing.
  4. Damp near joint limits. Within limit_margin (revolute) or linear_limit_margin (prismatic) of a limit, motion toward that limit fades smoothly to zero; motion away from it is untouched.
  5. Scale to velocity limits. If any joint exceeds its max_velocity, all joint velocities are scaled by the same factor, so the tool keeps its direction and only slows down.

Status: CuDiffIkStatus

Nominal { manipulability_sq }Normal tracking.
SingularityDamped { manipulability_sq, effective_damping }Near a singularity: the tool will deviate from the commanded twist. A good signal for a UI warning.
VelocityScaled { manipulability_sq, scale }Joint velocities were scaled by scale in (0, 1] to respect limits.

Parameters: CuDiffIkParams

base_damping = 0.02λ₀, used everywhere. Must be positive.
max_singularity_damping = 0.25Extra damping at an exact singularity.
singularity_threshold_sq = 1e-4w² below which extra damping ramps in. 0 disables it. Tune it per robot from manipulability_squared values you observe.
limit_margin = 0.15 radRevolute joint-limit damping zone.
linear_limit_margin = 0.015 mPrismatic joint-limit damping zone.
enforce_velocity_limits = trueUniform scaling to the chain's max_velocity values.

As a task

CuServoTask<N> takes (CuTwist6D, CuJointPositionsPayload<N>) and publishes CuJointVelocitiesPayload<N> with the status attached. Configure it with chain and an optional diff_ik block:

ron
(
    id: "servo",
    type: "repose_kinematics::CuServoTask<6>",
    config: {"chain": "igus_rebel", "diff_ik": {"base_damping": 0.02, "limit_margin": 0.15}},
),

Servoing is reactive and does no collision checking. Use it for small, supervised motions. Plan anything that moves through the workspace.

API reference