Use Velocity / Force Control

Many applications require applying a controlled force on the environment along some axes, while moving at a velocity along other axes. For instance, a cutting task may require application of a constant force on the material being cut while moving at a constant velocity along a cutting direction. Similarly, inserting an object into a container may require moving along an insertion direction while simultaneously complying with sensed forces and torques to align the object and the container.

These applications are naturally described as the combination of force-controlled motions and velocity-controlled motions where force control is applied in some directions and velocity control is applied along other tangential directions in the 3D space. This is in contrast to the more classical “trajectory tracking” approach (covered here) where a controller just tracks a given trajectory, with very limited sensor feedback.

We have developed a Hybrid Cartesian Velocity / Force Controller (VFC) that is capable of executing such motions. The controller generates joint-space setpoints based on Cartesian velocity and force references provided by the user. The velocity and force control components are applied along orthogonal axes so that they don’t interfere with each other:

• The Cartesian velocity component regulates motion to track the desired Cartesian velocity. The 'commanded' Cartesian velocity along the velocity-controlled axes is generated by applying Cartesian acceleration and Cartesian velocity limits to the desired Cartesian velocity.
• The force control component implements a simple proportional control law to produce a second subset of commanded Cartesian velocities along force-controlled axes.
• The two disjoint subsets of Cartesian velocities are then combined into a unique commanded Cartesian velocity vector, which is then mapped into joint-space via the robot inverse differential kinematics.

In contrast to joint-space trajectory tracking controllers, the VFC doesn’t require pre-planning and timing a trajectory, which makes it lower latency and therefore more suitable for tasks that require regulating forces. In addition, it can explicitly regulate forces/torques to a desired reference.

The controller has been developed as a ros2_control plugin, with special emphasis on real-time safety and MISRA compliance.

Adding a Velocity / Force Controller to your site config

To use the VFC, your robot needs a force/torque sensor installed at the wrist.

As with any other ros2_control controller, it can be configured in your MoveIt Pro configuration package, as described here.

A minimal configuration file with the velocity / force controller may look as follows:

controller_manager:
  ros__parameters:
    update_rate: 200  # Hz
    force_torque_sensor_broadcaster:
      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
    velocity_force_controller:
      type: velocity_force_controller/VelocityForceController


force_torque_sensor_broadcaster:
  ros__parameters:
    sensor_name: tcp_fts_sensor
    state_interface_names:
      - force.x
      - force.y
      - force.z
      - torque.x
      - torque.y
      - torque.z
    frame_id: tool0
    topic_name: ft_data

velocity_force_controller:
  ros__parameters:
    # Joints that the controller will control.
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
    # If the timeout is reached without receiving a new command, motion is stopped.
    command_timeout: 0.2
    # Base frame of the kinematic chain, for computation of arm kinematics.
    base_frame: base_link
    # The tip link of the kinematic chain, i.e. the frame that will be controlled.
    ee_frame: grasp_link
    # The frame where the force / torque sensor reading is given.
    # Needs to exist in the kinematic chain.
    sensor_frame: tool0
    # The force/torque sensor name.
    ft_sensor_name: tcp_fts_sensor
    # An optional deadband to apply to the force/torque signal. Set to 0.0 to disable.
    ft_force_deadband: 2.0
    ft_torque_deadband: 1.0
    # Maximum joint-space velocities.
    max_joint_velocity: [0.524, 0.524, 0.524, 1.047, 1.047, 1.047]
    # Maximum joint-space accelerations.
    max_joint_acceleration: [0.524, 0.524, 0.524, 0.524, 0.524, 0.524]
    # Maximum Cartesian-space velocities.
    max_cartesian_velocity: [0.25, 0.25, 0.25, 1.5707, 1.5707, 1.5707]                
    # Maximum Cartesian-space accelerations.
    max_cartesian_acceleration: [2.0, 2.0, 2.0, 4.0, 4.0, 4.0]

Using the Velocity / Force Controller

The controller exposes three ROS 2 interfaces:

• ~/zero_fts: A service to set (tare) the force/torque zero value. Often done before sending commands, to 'reset' the sensor zero reference value. This service can be called with the CallTriggerService Behavior.
• ~/configure: A service to configure runtime parameters of the VFC, e.g. Cartesian-space velocities, accelerations. etc. These override the values set in the config file.
• ~/command: A topic command interface where Behaviors can stream commands.

This controller expects a stream of commands as input. Once the first command is received, the controller expects the next command within command_timeout seconds. If no new command is received within the timeout, the controller executes an emergency stop in joint-space (using maximum joint velocities and accelerations). Otherwise, the controller just updates motion towards the new commanded values while obeying motion limits.

To send commands to the VFC, use the CreateStampedWrench, CreateStampedTwist and PublishVelocityForceCommand Behavior.

CreateStampedWrench and CreateStampedTwist let you define the force/torque and velocity references and put them into blackboard variables. PublishVelocityForceCommand takes those references and sends them to the Velocity / Force Controller. Please refer to the Behavior documentation in MoveIt Pro for details.