Cartesian impedance controller

[hamalMarino]

These commits were part of the vito-devel branch, but were edited to make them coherent and to function inside the master branch. They implement a Cartesian impedance controller, using the Kuka functionality for doing so. This is in the effort of having as few commits on vito-devel as possible, rebasing it onto master: at now, there is only one which is really needed and is the one that adds robot namespaces to the transmission (in order to be able to use SoftHands for the Vito robot - the same one that needs a custom version of ros_control).

Of coure, this should NOT be merged until tested for being functional on the real robot.

Specifically, rebasing onto this also the PR #66 ( @marcoesposito1988 are you still working on this? I will try to test the functionality soon, but would you have time to do so would be awesome ) could avoid the use of the custom CentroEPiaggio/ros-control as it is not using the hands (even this commit can be avoided as the necessary class has been added in lwr_hw.h ).

@carlosjoserg what do you think about the last part, would this be ok?

[hamalMarino]

@miguelprada commits [1] and [2] create and use a new Cartesian hardware_interface, while [4] and successive ones remove output and dynamic memory allocation (hope I didn't miss any).

[miguelprada]

Hey, @hamalMarino, this already looks much better in my opinion.

Once thing I still don't like very much is that you're using separate handles for each element in the 3x4 transformation matrix, which semantically doesn't make a lot of sense. As opposed to joint handles, where it's perfectly reasonable to manage individual joints separately, I think that there should be something like a CartesianPoseHandle which claims the complete pose hardware handle (still quite nonsensical, but a bit less than coordinate-xy-of-transform-matrix hardware handle).

The way it is right now, one could write a controller which acts on individual elements of the transform matrix, which is no good. Same goes for the stiffness, damping and wrench interfaces.

If you have time and want to give it a try, you can maybe have a look at how the ForceTorqueSensorHandle unites the 6D force+torque measurements under a single handle.

Also, there are some identation issues here and there. I'd say most of the existing source uses two spaces per tab instead of literal tabs or four spaces.

Nice work anyways!

[hamalMarino]

I didn't start changing the "code" yet, just making it work in master.

I agree, though, that handling the whole pose (to me, that should be an XYZ + quaternion) should be done as a whole. Will think about how to do that later, but that makes a lot of sense: thanks for the feedback! EDIT: actually I'm not sure how to handle this well, as a pose handle should also claim corresponding joints... But this is probably too overthinking?

And about the indentation, it was a matter of linting, would I change that, I should commit whole files again... There was already a mixed way of spacing, so I kept my usual one!

[miguelprada]

actually I'm not sure how to handle this well, as a pose handle should also claim corresponding joints... But this is probably too overthinking?

You're absolutely right, it should. The matter is whose responsibility is to take care of this. Not a trivial issue, I'll give it a think too.

As a matter of fact, (I think) there's a similar problem going on with the driver if one controller claims a certain joint through a position interface and later another controller claims another joint through an effort interface. I believe the second switch request would succeed in the current implementation, leading to undefined behavior. I say it's similar in that a claim of any joint in a given interface should somehow claim all other joints in the rest of the interfaces, given that switching FRI modes for individual joints is not possible. Just some food for thought, since both problems might be able to be solved in one go.

[carlosjoserg]

My 2 cents...

@carlosjoserg what do you think about the last part, would this be ok?

Yes, sure @hamalMarino .. I think the Cartesian command/state interface being written here could be sent upstream to ros-controls once it is done

I agree, though, that handling the whole pose (to me, that should be an XYZ + quaternion) should be done as a whole.

Why not a 3x4 matrix? I mean, it will not be the driver's job to keep the structure of a quaternion or frame so what would be the advantage of using quaternions instead of a complete matrix besides the number of values? (Just asking for the sake of discussion, and in our case the translation to FRI values will be direct)

E.g. in the ForceTorqueSensorHandle there is no assumption about how the wrench is written, it is just a pointer to force and to torque. Each element could even be a 6-dimensional vector with the complementary part equals to zero and then summed to create a complete wrench, but that seems to be part of the controller's job (or assumed by convention) as seen in the ForceTorqueSensorController here, right?

The IMU sensor interface and its corresponding controller can be illustrative as well.

actually I'm not sure how to handle this well, as a pose handle should also claim corresponding joints... But this is probably too overthinking?

As a matter of fact, (I think) there's a similar problem going on with the driver if one controller claims a certain joint through a position interface and later another...

@miguelprada It could be.. I haven't tested switching controllers with a partial set of joints using a different command interface...

I'm not sure a Cartesian controller should claim any joint, that would look like specific for serial arms, while a Cartesian controller could also be used in another context.

IMHO and my humble guess is that this conflict should be tackled using a custom policy in checkForConflict.

Also enforcing a controller acting on 7 joints can be added to avoid undefined behaviors as pointed out.

What do you think?

And about the indentation, it was a matter of linting, would I change that, I should commit whole files again... There was already a mixed way of spacing, so I kept my usual one!

Yep, sorry about that. You did good... I'm actually using tabs nowadays ;)

[hamalMarino]

I'm not sure a Cartesian controller should claim any joint, that would look like specific for serial arms, while a Cartesian controller could also be used in another context.

You're probably right, this should not be handled in the Cartesian Interface (PositionCartesianInterface in this case) but on a different level (maybe ros_control).

IMHO and my humble guess is that this conflict should be tackled using a custom policy in checkForConflict.

Rather than a custom policy, I think there should be a way of claiming more than a single interface with a controller, but the fact that right now the controller is chosen based on the (single) hardware_interface it extends is a limitation to this. My guess is that I am probably overlooking something (should have a closer look to ros_control and ros_controllers before trying to go upstream), but for now this is a local feature of a Kuka LWR arm and the fact that Cartesian also implies Joints claiming is a safe way of avoiding problems with the robot when changing controller online.

Why not a 3x4 matrix? I mean, it will not be the driver's job to keep the structure of a quaternion or frame so what would be the advantage of using quaternions instead of a complete matrix besides the number of values? (Just asking for the sake of discussion, and in our case the translation to FRI values will be direct)

Just because it's a minimal parameterization which is not affected by singularity, but that's just my preference, no other particular reason!

[hamalMarino]

Added two last commits needed to make things work on the real robot: sorry I was not able to keep the whole history of PR #66 but I tried to keep at least the important commit (the HACK, which should probably not be there, but we weren't able to make the robot run without that).

This PR solves also #66: is anyone planning on testing it on another robot? I'd like to have another confirmation before merging it.

@carlosjoserg the sleep was probably added, locally, in most computers, but never pushed. I took the simplest, yet safe way of doing this and added it here, as it would not run otherwise (so it is in a sense needed to make the CartesianImpedanceController work!)

[carlosjoserg]

@carlosjoserg the sleep was probably added, locally, in most computers, but never pushed. I took the simplest, yet safe way of doing this and added it here, as it would not run otherwise (so it is in a sense needed to make the CartesianImpedanceController work!)

Ok, looks great!

[hamalMarino]

Any comments on this? We used the Cartesian impedance controller quite a few times already, so I'd like to close this PR soon if no one has different suggestions. I'll merge this tomorrow around 12pm CET if no problem crops up by then!

[carlosjoserg]

For me is ok, master it ! Thanks @hamalMarino

Did you try also switching controllers back and forth?

[miguelprada]

I still have some doubts about how the interface is implemented, but I haven't had time to articulate an alternative, so if this works for you now, please go ahead. What I'd like to see is a single handle for the complete cartesian pose and impedance, as opposed to individual handles for each element in the transform matrix.

If I manage to invest some time further down the line, I'll prepare a new PR on top of the merged master.

[hamalMarino]

You're right @miguelprada , this might be not the perfect solution, but the single handle which controls everything doesn't sound right to me either: maybe a full pose, divided into full position and full orientation, could be better? And, aside, a full 6D stiffness / damping term? I'm just not sure at this point... Anyway, I'd go on with this for now, and leave that as a possibility for future improvement!