How to achieve impedance control or pure torque mode with ros_control ?

[amjack0]

I am trying to achieve impedance control (computed torque control) on UR5 arm in Gazebo, in which the applied torque is calculated as,

Acceleration = K_p(currentAngle - desiredAngle) + K_v (currentVelocity - desiredVelocity) + desiredAcceleration
torque = M * Acceleration + Coriolis*Velcoity + Gravity + friction

This series of torques is calculated to generate torque waypoints which gives torque trajectories.

Now, I want to publish this torque waypoints on robot controller to generate the trajectory in gazebo. For this I have written a custom action server over effort_controllers/JointGroupEffortController and I am sending effort goals to server using custom action client.

action_server
action_client

But I am still not sure If this is the way to achieve the impedance Control or pure torque mode with ros_control ?

ROS Melodic , 18.04
gazebo-9.0.0
Universal Robot (UR5) `https://github.com/ros-industrial/universal_robot/tree/melodic-devel

[destogl]

Hi @amjack0,

If you are generating torque trajectories, why not send them directly to HW? Either using command topic of the group controller or, even better, implementing your own controller, which generates trajectories and writes them to HW.

[amjack0]

I guess, Thats What I am doing now with the custom action server, this action server is written over JoinGroupEffortController and an action client generates the torque trajectories and sends it to action server. Then the action server writes the torque data to /command topic. But I am not able to simulate this as I am facing issue
Why do you think implementing your own controller is better option when we already have JointEffortController and JointGroupEffortController?

@destogl

[destogl]

You did I it kind of by writing your own action server, but you are putting again ROS topics between (timed) trajectory and its execution. You can get here non-deterministic delays. This could be also part of the issue. With your controller, or by extending JoingGroup controller you can overcome this

[amjack0]

I am able to implement it using JointGroupEffortController with action server and action client. I had some bugs related to threads and spin. Now it looks much better than before. Although, this results requires some more improvements !
@destogl

[qwq321qwq]

In my point of view, it is better to design a real trajectory to achive impedance control. When I use accelation and vel to control, i think it is better to keep a limited force because the aim to use impedance control is make the trajectory smooth. I'm now testing the impedance, too.

[amjack0]

@qwq321qwq, yes I solve the optimization problem to get the reference trajectory. This reference trajectory then can be tracked with the computed torque controller. I tried this with UR5 and JointGroupEffortController in Gazebo. video