Overview

This package contains a ROS 2 controller that can be used to control a robot's end-effector in 3D space using Cartesian velocity and force setpoints. The controller is designed to be used with a force-torque sensor, and it can be used to implement hybrid force/velocity control. The controller can also be used without a force-torque sensor, in which case it will be a Cartesian velocity controller.

See the main documentation for more information on the controller and how to use it.

Design

The VFC has been designed with the following ideas in mind:

ROS-agnostic core library + ros2_control wrapper. This makes it easier to reuse the core in other control frameworks outside ROS if needed.
Streaming interface. The controller is designed to be used with a streaming interface, where the user is expected to send velocity and force setpoints at some rate. This makes it specially suitable for as a teleoperation controller, or for use with other higher level closed loop controllers (e.g. visual servoing).
Real-time safe: no allocations in real-time thread. Validated via memory allocation tests (see malloc_counter.hpp).
Exit conditions (e.g. command timeout) trigger a full stop, satisfying user-defined Cartesian and joint-space velocities and decelerations.
Robust to singularities via Jacobian damping.
Support for user-defined nullspace tasks to be executed in the nullspace of the primary task.
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. The commanded Cartesian velocity (including the force-driven term) is scaled to max_cartesian_velocity / max_cartesian_acceleration, and the resulting joint velocities and accelerations are scaled to max_joint_velocity / max_joint_acceleration. All scaling is uniform, so the direction of motion is preserved.
Singularity robustness. Differential inverse kinematics uses a damped least-squares inverse (jacobian_damping, default 0.001) to bound joint velocities near kinematic singularities.
Non-finite value rejection. The controller rejects non-finite (NaN/Inf) values at every boundary: the commanded Cartesian velocity and wrench, the force/torque reading, 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 component, interface, and value. Available in MoveIt Pro 9.4 and later.

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

The commanded wrench is a control input, not a safety limit: unlike the Joint Trajectory Admittance Controller, VFC has no absolute force/torque abort threshold. The controller also has no measured-vs-commanded tracking guard; it enforces limits on the commands it generates, not on how well the hardware follows them. 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.

ROS Interface

See the main documentation for information about the ROS2 interface.

Velocity and force references default to the configured ee_frame. To express them in a fixed frame instead (e.g. for world-relative motion), set the control_frame_id field of the command message to the name of a link in the robot model; an empty value keeps the end-effector-relative behavior.

Parameter reference

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

velocity_force_controller:
  planning_group_name: {
    type: string,
    default_value: "",
    description: "Specifies the planning group name. Must be a valid planning group defined in the SRDF config file.",
    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
  }
  sensor_frame: {
    type: string,
    default_value: "",
    description: "Specifies the frame/link name of the force torque sensor. Must exist in the robot description."
  }
  ee_frame: {
    type: string,
    default_value: "",
    description: "Specifies the frame/link name of the end-effector frame. Must exist in the robot description."
  }
  ft_sensor_name: {
    type: string,
    default_value: "",
    description: "Specifies the name of the force torque sensor in the robot description which will be used in the admittance calculation."
  }
  ft_cutoff_frequency_ratio: {
    type: double,
    default_value: 1.0,
    description: "Specifies the cutoff frequency ratio for the FTS filter. Valid values range from 0 to 1, where 1 is the sampling frequency.",
    validation: {
      bounds<>: [0.0, 1.0]
    }
  }
  ft_force_deadband: {
    type: double,
    default_value: 0.0,
    description: "Specifies the deadband threshold for the force measurements (N).",
    validation: {
      gt_eq<>: 0.0
    }
  }
  ft_torque_deadband: {
    type: double,
    default_value: 0.0,
    description: "Specifies the deadband threshold for the torque measurements (Nm).",
    validation: {
      gt_eq<>: 0.0
    }
  }
  zero_fts_num_samples: {
    type: int,
    default_value: 10,
    description: "Number of force/torque samples to average when the ~/zero_fts service is triggered. Averaging several readings prevents a single noisy sample from biasing the sensor offset. The sensor is zeroed only while the controller is idle: a trigger received while a command is actively controlling is rejected and the reply reports failure. Samples are then collected one per control cycle; keep the sensor load constant for the first N cycles after triggering, until the offset takes effect. Trigger only when the sensor is free of external contact: the applied offset is the average of the N readings sampled over the window after the trigger, so any load present across that window (a real contact or preload, not just a static bias) is subtracted from all later readings and will no longer be detected. Must be between 1 and 1000.",
    validation: {
      bounds<>: [1, 1000]
    },
    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
    }
  }
  jacobian_damping: {
    type: double,
    default_value: 0.001,
    description: "Specifies the damping coefficient for the Jacobian damped least-squares inverse.",
    validation: {
      gt<>: 0.0
    }
  }
  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
    }
  }
  max_joint_velocity: {
    type: double_array,
    description: "Joint velocity limits (absolute value), one element per robot joint.",
    validation: {
      lower_element_bounds<>: 0.001
    }
  }
  max_joint_acceleration: {
    type: double_array,
    description: "Joint acceleration limits (absolute value), one element per robot joint.",
    validation: {
      lower_element_bounds<>: 0.001
    }
  }
  max_cartesian_velocity: {
    type: double_array,
    description: "Cartesian velocity limits (absolute value), one element per Cartesian axis (x, y, z, rx, ry, rz).",
    validation: {
      fixed_size<>: 6,
      lower_element_bounds<>: 0.001
    }
  }
  max_cartesian_acceleration: {
    type: double_array,
    description: "Cartesian acceleration limits (absolute value), one element per Cartesian axis (x, y, z, rx, ry, rz).",
    validation: {
      fixed_size<>: 6,
      lower_element_bounds<>: 0.001
    }
  }
  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
    }
  }

Relevant headers

velocity_force_setpoint_generator.hpp: This header contains a ROS-agnostic velocity/force setpoint generator, used to generate velocity and force setpoints for the controller.
cartesian_stop_trajectory.hpp: This header contains a Cartesian stop trajectory generator, used to generate a stop trajectory for the controller.
velocity_force_controller.hpp: This header contains the main controller class, within the ros2_control framework.