Force-based Stopping During Trajectory

[emfebert]

Hi,

I'd like to add a functionality to the xarm planning framework that allows terminating a trajectory when the force sensed by a 6-axis force sensor mounted at the end-effector exceeds a specified threshold.
I'm using the 6-axis force-torque sensor that came with the xarm.

Where would be the best location to add this? Inside the controller perhaps?

Thanks a lot!