Joint trajectory controller with force control

edit

Hi, I have been using ros_control for over a year now, but I have run into a problem with implementation.

We have two KUKA Agilus robots in our lab, and we have set them up each with a joint_trajectory_controller. We have been using this together with moveit to plan paths with obstacle avoidance, with great success.

Now, I have a couple of force/torque sensors attached to each end effector, and get data from them using netft.

But! I am trying to make the robot move after a path create by moveit and offset the path to compensate for an applied force, or just compensate for the applied force when no path is given. This is easy in theory, because you only add the force control joint output to the path command, but I have no clue of how to implement this.

I tried to edit the hardware interface to the KUKA robots, but found out that that was not the approach. I also tried to create my own controller that inherits the joint_trajectory_controller class and just add the force output each update, but that did not work since neither moveit or rqt recognizes it as a trajectory_joint_controller.

Is this the right way to go, and I just have done something wrong? Or is there an easier way? Is there a way that I can have two controllers control the same joints?

Thanks in advance,

Adam