Implementing Collision Response in Cartesian Force Controller

[AndreasTUB]

Hi @stefanscherzinger,

i am currently programming a UR10e robot in Gazebo with ROS Noetic. My objective is to drive the end effector towards an object at a specific speed and execute a collision. Subsequently, the robot should pull back in the opposite direction at a different calculated speed given a specific mass. Although I have successfully implemented the Cartesian Force Controller, I am encountering difficulties in solving this particular task.
I would like to know if it is generally possible to solve this task with the Cartesian Force Controller or if I should develop my own controller. I appreciate any additional advice.

[captain-yoshi]

This is my personal thoughts on your problem.

I don`t think you can select a specific cartesian velocity for the force controller. For the rest of you're use case, I think the controller is able to do as you describe, but you will need to develop some logic to achieve it. You have 2 options.

Option 1
Derive the CartesianForceController and add whatever logic you want your controller to do. Implement you're execute a collision, I think you want it to go back after a certain force has been applied to an axis or if it's not moving anymore. That means you need to compare some force before changing the PD gains and/or the target wrench to make it go the other way.

Option 2
Publish the force controller states that you need (wrench). Then create a ROS node, some sort of Cartesian Feedback, that will be able to detect when in collision based on some criterion.

1. Start the cartesian feedback
2. Start the controller with some parameters and the target wrench (Moves towards the collision)
3. Wait until the cartesian feedback has reached it's goal.
4. Send the wrench to the opposite direction and augment the pd gains or the solver error so that it moves quicker.

[stefanscherzinger]

@AndreasTUB

Sorry for not responding earlier. If still relevant for your, I would go with @captain-yoshi 's Option 2. That would be implementing a force-torque sensor in Gazebo and use its feedback to drive the cartesian_force_controller. There should be code an examples out there, but if I remember correctly, I didn't find a solution in ROS1 that I was happy with. You might need to implement some Gazebo plugin yourself.
Concerning velocity inputs for the cartesian_controllers: I'm aware that people might want this. It's not supported at the moment but I'm happy to take PRs since my time is a little limited at the moment. I saw a fork here that looked promising, though (ROS2).