joint_(damping & stiffness)_command_

[gpldecha]

Hello,

I have been using your package with our KUKA and have been implementing a joint position impedance controller with the hardware_interface::PositionJointInterface. My efforts have been based on duplicating/modifiying your Effort impedance controller (joint_impedance_controller.cpp). I have not figured out how "joint_positioncommand" gets set in the lwr_hw.h. I guess it is taken in some way from the loaded controller but I do not see where this happens. I am trying to also set "joint_dampingcommand" & "joint_stiffnesscommand" in my joint position impedance controller. I know that position is set through jointhandles[i].setCommand(). But I have no idea how to do the same with the stiffness and damping values K & D.

This is not really an issue, but I am not sure where else to ask.

Any help would be much appreciated.

[carlosjoserg]

Hi @gpldecha ,

That's something it has been in the ToDo list for a while. We had one version in the history in the beginnings where stiffness could be controlled along with position using the joint impedance strategy, setting the added torque and damping to zero, for instance.

We have thought of several approaches. Those values can be thought as control parameters, think as if they were the gains of a PID. But another possibility is to think of them as resources, like virtual joints that can be moved only through the position interface (since you are only setting values). The advantage is that you could potentially "plan" for them with current tools such as MoveIt!, or use a joint trajectory controller to change the stiffness slowly for instance.

I like the latter the most, and in fact, that's the reason why in the model, you will find the stiffness limits and the ramification to dummy links using the "stiffness joints".

Regarding to where the joint_position_command_ gets set, that's the controller's job, who should be acting on the position interface, and then, I just use it in the write() function of the corresponding to-hardware interface, that is, here for gazebo, here for the KUKA FRI, and here for the Standfor FRI. The joint_handles_[i].setCommand() should be called in the controller side.

Another final approach would be to create a joint command interface like the standard ones defined here for joint impedance, so the setCommand() would involve the position, stiffness, and damping set points all together, and not independently as described above. But I don't like much this solution since it requires more work, modify the standard ros control, which unless you justify it well, it wouldn't make sense, which leads me to the reason why not: it is not a real hardware resource. We thought of the following "practical" modes motivated by the fact that each mode could have their own command interface: JOINT_POSITION -> strategy 10 CARTESIAN_IMPEDANCE -> strategy 20 JOINT_IMPEDANCE -> full strategy 30 JOINT_EFFORT -> strategy 30 with only added torque, K, D, dq = 0 JOINT_STIFFNESS -> strategy 30 with added torque = 0 and D = 0 JOINT_SHOCK_ABSORBER -> strategy 30 with added torque = 0

But don't think it is a good idea anymore, probably only the first three are needed, and from the controller side one can set to zero the values according to the needs, only that establishing the "practical" mode in the lwr_hw and not on the controller, should make the controller code more scalable.

W.r.t. the joint_impedance_controller.cpp, the objective there was to emulate the strategy 30, compute the added torque, and then use the strategy 30 in JOINT_EFFORT above, so I don't think it is good given that the lwr can be deal with that directly. However, the controller could be used in a robot that doesn't.

Hope that clarifies things.

[gpldecha]

Thanks a lot for your long response, this really helps my understanding. I see why your solution regarding dummy links is preferable over control parameters.

I was looking in lwr_hw.cpp and I see that you register two handles to positioninterface. This means I should have access to both joint_positioncommand and joint_stiffnesscommand in my controller implementation. However I am still perplexed on setting the cmd values of joint_stiffnesscommand in my controller.

When I ros print the list of contollers which are loaded I see mine as :

name: joint_position_impedance_controller state: stopped type: lwr_controllers/JointPositionImpedanceController hardware_interface: hardware_interface::PositionJointInterface resources: ['lwr_0_joint', 'lwr_1_joint', 'lwr_2_joint', 'lwr_3_joint', 'lwr_4_joint', 'lwr_5_joint', 'lwr_6_joint']

This indicates I guess that I cannot set the values of joint_stiffnesscommand because 'lwr_i_joint_stiffness' are not loaded as resources. If I want to set damping in a similar fashion I should add another dummy joint for damping right ? If this is correct I should have a total number of 7 * 3 resources loaded for the controller.

Once all the resources loaded in my controller code, I should have some like this:

int index=0
for(int i=0; i < 7;i++){
 joint_handles_[index].setCommand(q(i));  // setting commanded joint values (joint pos resource)
 index++
}
for(int i=0; i < 7;i++){
 joint_handles_[index].setCommand(q_stiff(i));  // setting commanded joint stiffness values (joint stiffness resource)
 index++
}

Is my intuition correct ? What should I modify to have all those resources loaded and be able to set them in my controller ?

Thanks a lot again for your help, it is very much appreciated.

[gpldecha]

I have figured out how to do it... in my controller I added:

 // in joint_position_impedance_controller.h
std::vector<hardware_interface::PositionJointInterface::ResourceHandleType> joint_handles_stiffness;

// inside the init function 
KinematicChainControllerBase<hardware_interface::PositionJointInterface>::init(robot, n);
// Get joint handles for all of the joints in the chain
for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin(); it != kdl_chain_.segments.end(); ++it)
  {
       joint_handles_stiffness.push_back(robot->getHandle(it->getJoint().getName() + "_stiffness"));
   }

  // inside the update function
  joint_handles_stiffness[i].setCommand(K_(i));

This solved my problem I can now also change the stiffness values as well as the position values of the joints. I have tried this on the real robot and it works.

[carlosjoserg]

Yes, that was the idea!

That's why also the lwr_hw is tightly related to the urdf model, since there is a name convention to do this.

Regarding previous questions:

If I want to set damping in a similar fashion I should add another dummy joint for damping right ? If this is correct I should have a total number of 7 * 3 resources loaded for the controller.

Yes, exactly.

I assume you use the controller acting on the EffortJointInterface to trigger strategy 30 on the robot, right? But still you can access to the PositionJointInterface handles... that's why all interfaces are registered here

[gpldecha]

Actually I am using PositionJointInterface and I am setting in RHW

    mFRI->SetCommandedJointPositions(newJntPosition);
    mFRI->SetCommandedJointStiffness(newJntStiff);
    mFRI->SetCommandedJointDamping(newJntDamp);

I wanted to try effort at the start but the KUKA went a bit crazy. I have my own (but very similar) RHW, it's a way for me to learn what is going on. I will try the EffortJointInterface way next to do joint impedance. In this case I will not need to set stiffness and damping in FRI since they are used to compute directly the torques if I am not mistaken.

[carlosjoserg]

Ah, ok, that explains then...

Yes, the robot needs to start in strategy 10, and then switch to 30 (or 20), otherwise there can be an unexpected behaviors.

But those commands are only available in strategy 30, so I guess you have that mode fixed and only that one in your RHW.