curobo.servo module

Joint-space servo utilities.

class JointTargetServo(
joint_limits,
servo_dt_s=0.02,
kp=8.0,
kd=0.1,
)

Bases: object

Smooth a joint-space target into limit-respecting fixed-rate commands.

This class is for a servo loop that tracks one current curobo._src.state.state_joint.JointState target. It is not a trajectory queue and does not use timestamps. Call set_target whenever a new target arrives, then call step with the measured robot state once per fixed control tick to generate the next command.

All command, target, and measured states must use the same joint_names and tensor shape (batch, dof). batch is the number of independent command streams, and dof is the number of active joints in joint_limits.

set_target only stores a target. The command state is initialized either by reset or by the first step, which starts from the measured state.

Parameters:

  • joint_limits (JointLimits) – Active joint limits as a curobo._src.robot.types.joint_limits.JointLimits dataclass, such as the object returned by kinematics.get_joint_limits().

  • servo_dt_s (float)

  • kp (float)

  • kd (float)

Store active-joint limits and fixed-rate servo gains.

Parameters:

  • joint_limits (JointLimits) – Active-joint JointLimits with limit tensors of shape (2, dof).

  • servo_dt_s (float) – Fixed servo tick duration in seconds.

  • kp (float) – Proportional gain applied to position error.

  • kd (float) – Damping gain applied to measured velocity.

Raises:

ValueError – If servo_dt_s or any symmetric motion limit is invalid.

__init__(
joint_limits,
servo_dt_s=0.02,
kp=8.0,
kd=0.1,
)

Store active-joint limits and fixed-rate servo gains.

Parameters:

  • joint_limits (curobo._src.robot.types.joint_limits.JointLimits) – Active-joint JointLimits with limit tensors of shape (2, dof).

  • servo_dt_s (float) – Fixed servo tick duration in seconds.

  • kp (float) – Proportional gain applied to position error.

  • kd (float) – Damping gain applied to measured velocity.

Raises:

ValueError – If servo_dt_s or any symmetric motion limit is invalid.

Return type:

None

property batch_size: int

Return the number of independent command streams.

Returns:

Batch dimension of the initialized command state.

Raises:

ValueError – If no command state has been initialized.

property dof: int

Return the number of active joints.

reset(command_state)

Replace command state with a known joint position and optional velocity.

Use this when starting the servo from the robot’s current state, or after a discontinuity where the previous command state should no longer influence acceleration and jerk limits. The reset state also becomes the current target, so a following step holds that state until set_target is called.

Parameters:

command_state (curobo._src.state.state_joint.JointState) – Command state with position shape (batch, dof) and joint names matching joint_limits. When velocity is omitted, command velocity is initialized to zero.

Raises:

  • TypeError – If command_state is not a JointState or contains non-tensor fields.

  • ValueError – If joint names, shape, dtype, device, or finite-value checks fail.

Return type:

None

set_target(
target_state,
)

Replace the current target used by future step calls.

The servo stores only this target; it does not queue previous targets or initialize command state from the target.

Parameters:

target_state (curobo._src.state.state_joint.JointState) – Target state with position shape (batch, dof) and joint names matching joint_limits. When velocity is omitted, the servo treats the target as a position-only target and applies a stopping limit.

Raises:

  • TypeError – If target_state is not a JointState or contains non-tensor fields.

  • ValueError – If joint names, shape, dtype, device, or finite-value checks fail.

Return type:

None

command_state()

Return a copy of the current command state.

Returns:

JointState with position, velocity, acceleration, and jerk tensors of shape (batch, dof) and active joint names from joint_limits.

Raises:

ValueError – If no command state has been initialized.

Return type:

curobo._src.state.state_joint.JointState

step(measured_state)

Generate the next command for one fixed servo tick.

The method assumes it is called at a fixed rate matching servo_dt_s. It applies a position feedback term from measured position, optional target velocity, and position/velocity/acceleration/jerk limits before updating the internal command state.

Parameters:

measured_state (curobo._src.state.state_joint.JointState) – Measured state with position and velocity tensors of shape (batch, dof) and joint names matching joint_limits.

Returns:

New JointState command after applying all joint-space limits.

Raises:

  • TypeError – If measured_state is not a JointState or contains non-tensor fields.

  • ValueError – If no target has been set, measured velocity is missing, or joint names, shape, dtype, device, or finite-value checks fail.

Return type:

curobo._src.state.state_joint.JointState