Adapt lwr_controllers to urdf with dummy links

[marcoesposito1988]

The new dummy links added to the URDF after the merge make it impossible to initialize the controllers in lwr_controller.

In particular, kdl_tree_.getNrOfJoints() returns 14, screwing up all index computations.

Is there a way to avoid these dummy links that clutter the URDF? It looks like we cannot use KDL any more, at least not without huge hacks.

[carlosjoserg]

Damn... didn't think of that, true. I need the stiffness joint to be like a real joint to appear in the hardware interface.

1. The short way is not use indexes, but maps relying on the names, I just added _stiffness to the dummy joints names.
2. The middle way is to use the transmission info from the robot_description which contains only the true joints.
3. The long and correct way is to actually create the mixed joint command interfaces I describe in Update the xacro model to pass which interface to use #19, in this case the StiffnessJointInterface and avoid completely the dummy joints and links.

[marcoesposito1988]

Actually I was wrong, it manages to start with a couple of little hacks. Still the compatibility with KDL is gone until we do things nicely (3rd option).

I think I will start working on the controller subclass I mentioned in #1, so that we can confine all the hacks in there.

[nrontsis]

@marcoesposito1988 could you mention the hacks that you used?

@carlosjoserg I am bit confused about the usefulness of the stiffness parameters in the simulation.
From my experience with the real kuka arm (hardware), in the "impedance mode", there is an inner "compliant" controller that is parameterized by the stiffness parameters, while the "FRI torque" is superimposed to the rest of the controller terms.

Are the efforts sent by the controllers in simulation similar to "FRI torque"?
Can I remove the stiffness joints (that cause me problems similar to the ones that you mentioned above) in the simulation and work on an pure torque controller?

[marcoesposito1988]

@nrontsis: I realized that there are no hacks necessary if the chain root and tip are specified in the yaml config file, and all joints are then found through the kdl chain. See commit 9ed8d39

I am still reviewing these changes, and still this does not solve the position/stiffness dicotomy.

[carlosjoserg]

@nrontsis Right now, the default interface to the gazebo model emulates the "Section 9.2.3 Controller strategy 30 (axis-specific stiffness controller)", that is, the control law is:
tau_cmd = K_FRI ( q_FRI - q_msr ) + D( D_FRI ) + tau_FRI + f_dyn

If you use position_controllers for Stiffness/Position you are actually sending K_FRI and q_FRI, while tau_FRI = D_FRI = 0.

q_msr is read from the simulation and f_dyn is the gravity term computed with KDL using the URDF parameters.

The stiffness joints are necessary for now to account for limits and to appear in the hardware interface as controllable joints, but actually they are not moving anything. I don't like that, but it is a temporary solution until a proper "stiffness" or "impedance" interface is implemented.

The problems in this issue refers to custom controllers in the package lwr_controllers, but if you use standard ros-controllers, particularly position_controllers, you can run all packages as they are. Check the single_lwr_example.

[carlosjoserg]

@nrontsis Ah, I see you want to mount the shadow hand on a kuka lwr. So, check also our vito robot where we have a custom Pisa/IIT SoftHand model as well X2.

[nrontsis]

Thank you @carlosjoserg I will check vito to see how you integrated the hand to kuka.

[marcoesposito1988]

I think this problem was addressed with pull requests #26 and #28.

@nrontsis, @carlosjoserg: can we close this? I will open a new issue about getting Gazebo to work again with the effort controllers. I have a very horrible hack in 675c0d7 that brings it to a working condition, we should make it into a viable solution.

[carlosjoserg]

yes!