Gravity compensation (TESTING IN PROGRESS)

[marcoesposito1988]

Here is a gravity compensation controller that should (!) be safe enough.

It continuously updates the current commanded position, so when the stiffness is raised again the arm stays in place.

It can be enabled/disabled through a std_msgs::Bool topic, and the stiffness in enable mode can be changed for all joints with a std_msgs::Float32 message.

[marcoesposito1988]

It starts getting very unstable as soon as a joint approaches its limit. It can be very dangerous if not in T1: once it started obscillating quite quickly. I will try clamping the position that is set.

[marcoesposito1988]

Even with clamping it behaves strangely when it reaches a joint limit (btw, this is not detected by the new clamping code). Sometimes it goes some degrees back at full speed, sometimes it starts then obscillating, sometimes it builds up more and more force on one axis.

All this is not present in the native gravcomp, of course. Any idea about the cause?

[marcoesposito1988]

Horrible suspect: might it be the joint_limits interface in lwr_hw?

[marcoesposito1988]

And the answer is yes! the crowd cheers

We should expose a topic from lwr_hw to allow enabling/disabling the joint limits enforcement.

[carlosjoserg]

Are you sure it can not be solved in other way? I don't like the joint limits enforcement enabling/disabling in the driver.

[carlosjoserg]

Sometimes it goes some degrees back at full speed, sometimes it starts then obscillating, sometimes it builds up more and more force on one axis.

When does this happen? When switching controllers or when moving it with your hand while in gravity compensation?

[marcoesposito1988]

It happens some times after I start the controller and I start moving it with my hand. I didn't even try unloading/loading the controller yet.

I think we should expose a mechanism to let the gravity controller enable/disable the joint limits enforcements, because that just solved all problems and now it looks very nice.

[carlosjoserg]

See at my suggestions. The way to work with this is to actually switch controllers. That is: http://wiki.ros.org/controller_manager?distro=indigo

Switch from the two (position/stiffness) controllers to gravity compensation and back, is this possible?

I'm not in the lab right now and can't test it.

[carlosjoserg]

According to the srv file, you can start/stop a list of controllers.

[marcoesposito1988]

Yes but loading/unloading controllers is cumbersome. For hand-eye calibration for example one may want to "pause" gravity compensation, letting the robot stay still for a moment, and then start again. Changing controllers all the time would be a nightmare, dynamic reconfigure is more handy in order to have a nice UI.

[carlosjoserg]

You need to call a service / publish a topic anyway to enable the gravity compensation, that's your pausing.

Besides, there is an rqt plugin for controller manager that makes the job of starting/stopping controllers very handy and with a very nice UI as well.

[marcoesposito1988]

Oh, I just noticed I forgot "stopping".

I am considering the alternative with rqt.

[marcoesposito1988]

I can alternate between joint_trajectory_controller (position+stiffness) and gravity_compensation without problems.

I will strip the controller of the parts you don't want, we can always add them back later if needed.

[marcoesposito1988]

The question of the limits in lwr_hw remains: it looks like it is absolutely necessary to disable it while the gravity_compensation controller is running. The controller itself could disable the limits when it starts and enable them back when it stops, as soon as we have an interface in lwr_hw for that.

What would your preferred approach be? param+topic? dynamic_reconfigure?

[carlosjoserg]

unneeded bits haha

Well, that's the part I liked the less, mainly because I still don't understand why. I'm actually re-reading the driver, and the joint_limit_interface to see if I can figure it out, would you give me some time before merging?

The controller itself could disable the limits when it starts and enable them back when it stops

Definitely, and I think param+topic seems like a good and quick option, if so.

[marcoesposito1988]

Good... I'm already working on it O:-)

No worries, there is no rush. I need it for myself so I need to get it done kind of quickly, but you can test it thoroughly (and rightly so).

I think that the joint_limits interface changes the position that is sent abruptly (-> quick and dangerous movement), so when in the loop we send the one we read it is already old (-> obscillation).

[carlosjoserg]

I detected that the stiffness saturation (sj_limits_interface_.enforceLimits(period);) is missing in the real hw write(), but that's not the cause.

I think that the joint_limits interface changes the position that is sent abruptly

Yes, and that's how it should be in a normal operation. You will never read an out-of-the-limit position because you will never command something out of the limits.

In this case, it is happening because the soft limits do not coincide with the robot limits, so you cross the boundary of the soft limit defined in the URDF, and as you say, in the next cycle it saturates the command = read position, creating the oscillation.

Could you try removing all safety controller tags from the URDF for all joints to confirm? Thanks

If so, then I would prefer that solution and treat soft limits in a yaml file as described here. For instance a T1.yaml could define conservative limits according to the T1 mode, like the one you configured in the moveit package, but in this case it is moveit-independent.

[marcoesposito1988]

So loading these conservative values would be done depending on a boolean argument to the launch file? Or how do you intend to load them only when not doing gravity compensation?

[marcoesposito1988]

Yes, also just removing the "safety controller" tags works.

[carlosjoserg]

boolean argument to the launch file

That could be an option, yes. The thing is that using more conservative limits for the position is required in very rare occasions, in my opinion. Besides, the arm already has a nice soft protection before the mechanical limits activate, which you can modify internally as well, otherwise you wouldn't be able to move the arm in gravity compensation without triggering the mechanical brakes.

So, I would go for having that as a an alternative, but not as a requiredt behavior. I mean, loading a yaml file to be optional, and take the limits in the URDF (without the safety controller as you did already in https://github.com/CentroEPiaggio/kuka-lwr/commit/11bd38c24080bbe9633813d1509a20b5c6514bff) as operational.

[carlosjoserg]

Final comment: I would keep the inheritance of the KinematicChainControllerBase to have it there for a future gravity term computation out of the urdf model. Infact, It s a good name for all this family of controllers. I have seen others using Cartesian controllers, but parallel robots can also position end-effectors in SE(3). So :+1: for the name.

I think the PR is ready, but to be sure, let me know when you are done to merge it.

[marcoesposito1988]

I totally agree, we can add the yaml parsing later. I would say we test it a bit more and then we merge it, even if it hasn't killed me yet and I have been using it in auto for a while.

I would say, it was a productive day. Cheers! :trophy:

[carlosjoserg]

hahah ok let me know !

[marcoesposito1988]

I have been testing it more and I haven't had problems yet, so I think we can merge it.

BTW, should we change the name to something like "NullStiffnessController" first?

[carlosjoserg]

Great

Not sure about the name change.

I think we can keep it as a Gravity Compensation controller, clarify that this is an approach by setting all FRI values to zero, and it uses the internal model of the KUKA, and that we plan to generalize later using an URDF model.

For the lwr dynamic parameters, I've seen 3 interesting works that we can use to update the URDF:

1. http://ifatwww.et.uni-magdeburg.de/syst/about_us/people/zometa/robotmpc/index.shtml#mdl
2. http://upcommons.upc.edu/handle/2099.1/24233
3. http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6907033