Implement a gravity compensation controller

[carlosjoserg]

This is to avoid the script switch in the robot controller, as proposed in the already merged PR by @marcoesposito1988. We all know the the advantages of having such a behavior.

This way, gravity compensation is triggered in the PC and not in the robot controller. This requires having an accurate dynamic model of the robot. Or, another possibility could be to implement it in the HW as a service to stop all active controllers and set internally joint_position_command = joint_position and addTorque = 0 for all joints, to be sent to the robot.

I would prefer the controller solution (I think most of the basic code should be already there in the lwr_controllers package), however it is risky due to model uncertainties.

@manuelbonilla @enricocorvaglia @marcoesposito1988 what do you think?

[manuelbonilla]

Hi all,

I think that both solutions are useful. The one using a different script is really useful (and for me the correct way) for calibration issues for example. The solution implementing the "gravity compensation" in the controller is more a reflex strategy or something like that which can be implemented by almost any of the controllers already coded. I mean just setting the gains to 0. What I think is really a good Idea is the one proposed by @carlosjoserg about services to stop/reset/start controllers. So I think keeping both is even better than decide just for one.

@carlosjoserg I think we can also discuss about adding more functionality to the HW interface. I mean, read and publish all the info in the message coming from the robot, Jacobian, int variables, MAss matrix, etc. fore more info take a look at Msr data telegram part in the FRI manual.

[carlosjoserg]

+1 to adding something like /lwr/info topic to publish device info using Msr data.

Particularly, I think Jacobian is not useful anymore, it is always referred to the robot base that can be anywhere, as in our bi-manual robot. In general, IMHO, frames, and all frame-dependent data defined in the robot shouldn't be supported from the package point of view.

I'm still doubtful regarding having two scripts in the robot. I agree that the right way for camera-robot calibration is to use the built-in feature, but I think we need to study more KRL and learn how to use ints[] and bools[] to allow the switch between monitor and command mode in the robot. I think the Stanford FRIL script could be useful in that.

[marcoesposito1988]

Hi guys,

I would also vote for keeping both, as they are for two completely different use-cases, if that is not a problem.

I may also need the jacobian in the future for velocity control and visual servoing, and I can guess that others may need it, so please keep it! Maybe on a dedicated topic with a check on the number of listeners (as it is done for image publishing) if performance is a concern.

[marcoesposito1988]

Actually playing with the new stiffness controller it looks like we can achieve a similar behavior by setting the stiffness to a very low value. I would suggest to create a script that applies a given stiffness value to all joints and then create launch files for enabling/disabling this "pseudo-gravity compensation".

Would this approach be ok for you?

[carlosjoserg]

It is not pseudo, it is gravity compensation. The joint impedance control law is:

tau_cmd = k_FRI (q_FRI - q_mst) + D(d_FRI) + tau_FRI + f_dyn

So, if you set everything to zero, you are sending tau_cmd = f_dyn which is the gravity term, considering the configured tool (mass, CoM, inertia, etc.).

I had forgotten this issue, but now, the gravity compensation controller boils down to actually send a fixed zero value for all the FRI vars. And you should be able to switch between controllers without a problem if you are using the current HW implementation for Effort or Position/Stiffness interfaces, once the last comments in #26 is resolved, of course.

[marcoesposito1988]

Actually we have a problem when we raise the stiffness again. The arm goes back to the last commanded position, and if the target stiffness is very high this can become very dangerous.

I think we should implement a dedicated controller, that takes an enable/disable command, and when gravcomp is disabled it sets the new commanded position to the last measured position before increasing the stiffness again. What do you think of this approach?

[marcoesposito1988]

First implementation of controller in #31

Still unstable and dangerous (see pull request).

[carlosjoserg]

31 was merged, so I'm closing this.

If so, we can re-open with the improvement of computing the gravity term out of the URDF.