Joint Velocity Controller

Overview

This package contains a ROS 2 controller that can be used to move the joints of a robot using velocity setpoints. It contains a ROS-agnostic joint velocity setpoint generator and a ROS 2 controller (using ros2_control) that uses this generator to compute the reference signals to send to the robot, via the hardware interface. The controller takes desired joint velocities as input, in a control_msgs/JointJog message, and computes the immediate next joint positions and velocities to send to the robot, while respecting the robot's joint velocity and acceleration constraints. Joint velocity and acceleration limits are defined in the controller's configuration file, and can also be dynamically reconfigured at runtime via a ROS 2 Service call.

Design

The Joint Velocity Controller has been designed with the following goals in mind:

1. ROS-agnostic core library + ros2_control wrapper. This makes it easier to reuse the core in other control frameworks outside ROS if needed.
2. Streaming interface. The controller is designed to be used with a streaming interface, where the user is expected to send joint velocities at some rate. This makes it specially suitable as a teleoperation controller, or for use with other higher level closed loop controllers (e.g. visual servoing, learnt policy, etc.).
3. Real-time safe: no allocations in real-time thread. Validated via memory allocation tests (see malloc_counter.hpp).
4. Exit conditions (e.g. command timeout) trigger a full stop, satisfying user-defined joint-space velocities and decelerations.
5. MISRA C++ 23 compliant.

Safety behavior

The controller runs the following checks every real-time cycle:

• Command timeout. If no new command arrives within command_timeout (default 0.2 s), the controller stops. For streamed control this means the robot stops on its own when commands stop arriving.
• Joint position-limit prediction. Each cycle the controller predicts whether a maximum-deceleration stop, started now, would carry a joint past its soft position limit (with a joint_limit_position_tolerance margin, default 0.02 rad), and stops before the limit is reached. The prediction uses the last commanded state, not the measured state.
• Velocity and acceleration clamping. Commanded velocities and accelerations that exceed max_joint_velocity / max_joint_acceleration are scaled down uniformly so the direction of motion is preserved.
• Non-finite value rejection. The controller rejects non-finite (NaN/Inf) values at every boundary: the commanded velocity, the joint state from hardware, and the final hardware command. On detection it engages the stop trajectory, or refuses to start if idle, and logs the offending joint, interface, and value. Available in MoveIt Pro 9.4 and later.

All stops brake along the configured max_joint_acceleration; the controller never cuts the command to zero or releases the joints.

The controller enforces limits on the commands it generates, not on how well the hardware follows them: it has no measured-vs-commanded tracking guard. On deactivation it releases its hardware interfaces without commanding a hold or zero. Reaching a safe state is left to the robot driver and the physical safety chain.

Parameter reference

Here's the list of all the parameters that can be configured, with their descriptions and default values:

joint_velocity_controller:
  planning_group_name: {
    type: string,
    description: "Specifies the planning group name. Must be a valid planning group defined in the SRDF config file.",
    validation: {
      not_empty<>
    },
    read_only: true
  }
  command_joints: {
    type: string_array,
    default_value: [],
    description: "Specifies which joints will be commanded by the controller. If empty, it defaults to the joints included in the given planning group.",
    read_only: true
  }
  command_timeout: {
    type: double,
    default_value: 0.2,
    description: "Timeout in seconds after which the controller will stop motion if no new commands are received.",
    validation: {
      gt<>: 0.0
    },
    read_only: true
  }
  max_joint_velocity: {
    type: double_array,
    description: "Joint velocity limits (absolute value), one element per robot joint.",
    validation: {
      lower_element_bounds<>: 0.001,
      not_empty<>
    },
    read_only: true
  }
  max_joint_acceleration: {
    type: double_array,
    description: "Joint acceleration limits (absolute value), one element per robot joint.",
    validation: {
      lower_element_bounds<>: 0.001,
      not_empty<>
    },
    read_only: true
  }
  joint_limit_position_tolerance: {
    type: double,
    default_value: 0.02, # radians.
    description: "Padding (in radians) to add to joint position limits as a safety margin when predicting joint limit violations.",
    validation: {
      gt<>: 0.0
    },
    read_only: true
  }
  command_interfaces: {
    type: string_array,
    default_value: ["position"],
    description: "Specifies the command interfaces that the controller will use.",
    validation: {
      not_empty<>,
      unique<>,
      subset_of<>: [["position", "velocity"]]
    },
    read_only: true
  }
  state_publish_rate: {
    type: int,
    default_value: 50,
    description: "Rate in Hz at which the controller will publish the state. Set to zero to disable state publishing.",
    validation: {
      gt_eq<>: 0
    },
    read_only: true
  }