Update the xacro model to pass which interface to use

[carlosjoserg]

The interface to the xacro model is something like:

<xacro:kuka_lwr parent="world" name="lwr">
    <origin xyz="0 0 1" rpy="0 0 0"/>
</xacro:kuka_lwr>

Add a parameter to select whether to use position (PositionJointInterface that uses kuka position control strategy), position/stiffness (it can be called StiffnessJointInterface that uses kuka impedance control strategy ), effort (EffortJointInterface that uses kuka impedance control strategy), or finally position/stiffness/damping/effort (it can be called ImpedanceJointInterface that uses the full kuka impedance control strategy). For the latter there is a sketch here in that direction.

The parameter will be used to select the interface to be used in here as an example for ${name}_0_joint.

I just realize that, in the case of the lwr arm, all joints must be controlled with the same strategy, so that parameter could be passed as an arg to all transmissions!

[carlosjoserg]

Or also update the lwr_hw package to use ros-controls/ros_control#200

[carlosjoserg]

The ability to choose whether you want the gravity term added in the Stiffness, Effort and Impedance joint interfaces could be useful, like

<xacro:kuka_lwr parent="world" name="lwr" addGravityTerm="true">

In simulation, it is just not adding the term.

In real, one needs to read the value given by FRI and add it with opposite sign to the effort command (Several sources have told me that f_dyn(q, q_dot, q_dotdot) is actually f_dyn(q), the standard gravity term in robot control theory) .

Not to forget: This value depends on the load!

[carlosjoserg]

@marcoesposito1988 I think I've come with a good solution only in my head so far. I think the interfaces will be too much work and probably senseless.

I would do:

ROS CONTROL	FRI
PositionJointInterface	Strategy 10
EffortJointInterface	Strategy 30

The lwr_hw_real will expose only those 2 interfaces with all fri commands. It will need to handle the interfaces switchthat triggers KRL mode switching on the robot as well. I can work on this next week.

So, whatever controller uses EffortJointInterface must set to zero the FRI values that it does not use, as we did with the gravity compensation controller. So, for instance, basic joint controllers that use EffortJointInterface could be:

_Edit: commands by type, and generic gravterm

ControllerName	k_fri	q_fri	d_fri	tau_fri	grav_term
EffortController	0	0	0	double	bool
StiffnessController	double	double	0	0	bool
StiffnessEffortController	double	double	0	double	bool
ImpedanceController	double	double	double	0	bool
ImpedanceEffortController	double	double	double	double	bool

Note that the standard effort controller from ros_controllers can't be used with this robot since the controller needs to set other commands to zero.

Note also that, the f_dyn command is a boolean and not a computed value from the controller. So, within lwr_hw_real, I will read that value from FRI and add it with opposite sign to the command, and it will be true by default.

I'm still doubtful whether to have k_fri and d_fri as dynamic parameters (similar to pid gains), or as resources (similar to joint values).

[marcoesposito1988]

@carlosjoserg: the idea about the automatic FRI mode switching is really good and sounds realistic. It should be a very clean solution, I like it.

I am a bit confused about the f_dyn column: are all lines "true/false" on purpose? Should we have two variants of each controller, one computing f_dyn from the URDF and one not?

And since in the worst case we should send 4 different values for each joint (k,q,d,tau) I guess we will need a custom JointInterface, right?

[carlosjoserg]

I am a bit confused about the f_dyn column: are all lines "true/false" on purpose? Should we have two variants of each controller, one computing f_dyn from the URDF and one not?

No, only one variant. The true/false was just to differentiate that the parameter is not a double but a bool, probably not dynamic but passed as a fixed parameter in the yaml configuration. I updated the table to reflect that, and that f_dyn is actually a grav_term that can be added or not to the controller's output.

So, if it is true, you add it (right now it is not actually in the controller's output, but using the f_dyn term automatically added by the robot), and if it is false it is not added (right now, I would do the trick of adding f_dyn with opposite sign).

Later, if we manage to have an accurate URDF, then lwr_hw_real will always cancel the f_dyn with the trick, and the grav_term is computed from the URDF and optionally added.

Of course, one can have one single controller, and use a bool to enable/disable the control of any of the 4 different values, but I think having controllers for each set makes life easier and safer.

And since in the worst case we should send 4 different values for each joint (k,q,d,tau) I guess we will need a custom JointInterface, right?

It is not the worst case, it is the only case, you always send the 4 values, some are set to zero depending on what you would like to control.

So, only 2 interfaces are used. The question is:

1. If we treat k and d as dynamic parameters, we avoid the dummy joint and links, which is good.
2. If we treat k and d as resources, we need the dummy joint and links. You might think why then? My initial idea on this was to have a way to plan position & stiffness, for instance using moveit, so you generate trajectories for both. I think that is interesting.

[marcoesposito1988]

Ok, now the f_dyn matter is clear, thanks.

About the terms: I would also prefer to have a uniform approach and treat all four terms in the same manner (as resources, in your terminology). This would mean having four interfaces for each joint, so one real joint plus three dummy joints (for stiffness, damping and added effort respectively). Correct?

[carlosjoserg]

Correct, but only two dummy joints, stiffness (already there) and damping. Position and effort correspond to the real joint.

[marcoesposito1988]

That's right. But can we use them at the same time? Do we have problems if we try to take ownership of the position and effort handles at once?

[carlosjoserg]

Oh, good question, not sure, but...

[marcoesposito1988]

:sweat_smile: