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 theCallTriggerService
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.
New in version 6.1.0
For more practical examples, please refer to the Move until Force Contact