torque controller is not working properly with real robot

[Elise-J]

The torque controller is not working properly anymore with real robot.
When launching the torque controller with roslaunch iiwa_driver iiwa_bringup.launch the robot starts to move when it shouldn't,

The issue seems to be the dithering implementer to trigger the friction observer (commit e293093 ).
I'm not sure I've understood it clearly, but a sinusoidal signal is added to the position command. This sinusoid is bounded by 0.1 rad (which seems quite high) and results in the robot moving on its own.

@matthias-mayr
Do you think we could add a flag to choose to run or not those lines?
Also, shouldn't the signal added be smaller than 0.1 rad ?

[costashatz]

The issue seems to be the dithering implementer to trigger the friction observer (commit e293093 ).

This has nothing to do with the movement of your robot. It's complicated why we need this, but in torque control mode this shouldn't affect your commands/movement.

When launching the torque controller with roslaunch iiwa_driver iiwa_bringup.launch the robot starts to move when it shouldn't,

Giving it zero torque, leaves only the gravity compensation, which does not work very well on each own for all cases. So it is "normal" that the robot might move. What type of movement do you get?

[niederha]

Hi!

I'm working with @Elise-J. We checked together, here are the facts we collected.

• With the latest commit on master when we start FRIOverlay on the IIWA: Robot doesn't stand still, makes a wired downward trajectory but doesn't fall in a straight line at all. It doesn't look like grav comp is failing or at least it is not the only cause.

20230512_140326_1_1.mp4

• By removing the dithering (i.e: changing 0.1 to 0.0 at line 372 of iiwa.cpp) we are back to a normal compliant behavior.
  

Note that we observe the faulty behavior only when testing on our real IIWA for some reason it does not appear in simulation.

This is the reason that makes us think commit e293093 is at fault. From what I see I think the assumption that the dithering constant is too high makes sens. Could it interfer with grav comp or generate some kind of unstable control? Or do you have any other leads?

[matthias-mayr]

This is the reason that makes us think commit e293093 is at fault. From what I see I think the assumption that the dithering constant is too high makes sens. Could it interfer with grav comp or generate some kind of unstable control? Or do you have any other leads?

If you compare the performance of the robot system with and without this commit, you would notice a significant difference in the joint stiction. Of course as a side effect, this also makes calibration errors more noticeable.

My experience here is in line with the Stanford researchers that have a closer contact with KUKA and when those researchers complained about the high joint stiction, the KUKA engineers told them to use this technique to keep the friction compensation running.

This joint position dithering is expected to be used when people turn the stiffness of the KUKA internal joint impedance controller off. Therefore we are expecting that it does not make any difference since it would be multiplied with a 0. (Although there was at least one person who did not turn the stiffness off and actually saw it dithering).

If you want to use your robot with much larger joint stiction and this gives you a good-enough performance, you can of course turn it off. For us, this small feature is a huge improvement in accuracy and we would not want to go back to the old behavior.

[niederha]

Alright, after a bit of investigation, I understand you point. However some of us still need to use non-null stiffness with the joint impedance controller. Although it is easy to fix by hand, as a matter of source control, it is a bit annoying having to patch the repo at every clone (or pull if we wanna keep up with main). Could we have dithering as a flag or something?

I am not very knowledgeable in this repo, but it seems to me that it might not be optimal to have a commit that breaks some functionalities on master.

Also I did note that striction is lower which is great. However, (and this is just a thought) couldn't we add the dithering on the torque command instead of the position to yield similar results without breaking the impedance controller with non-zero joint stiffness?

[matthias-mayr]

Great to hear.

However some of us still need to use non-null stiffness with the joint impedance controller.

Of course this change expects that there is no use-case that configures stiffness of the joint impedance controller and use torque control since these two modes "interfere" somewhat.
It is not possible to update both joint positions and joint torques from ROS and if someone is actually doing this combination, it only makes sense to me if the position references of the joint impedance controller are changed as well. Of course this can be done with KUKA's Java libraries, but I am not aware of anyone combining that with torque control from ROS.

Note that this position dithering is just applied if someone wants to do torque control from ROS. There is still the use-case of people wanting to do position control in which case the dithering is not needed and also not added.

However, (and this is just a thought) couldn't we add the dithering on the torque command instead of the position to yield similar results without breaking the impedance controller with non-zero joint stiffness?

I have some master students who are investigating this right now. The short answer is that it's not that easy and by doing the position dithering we are just "enabling" a KUKA functionality that they just did not document anywhere.

I am not very knowledgeable in this repo, but it seems to me that it might not be optimal to have a commit that breaks some functionalities on master.

Reminds me a bit of https://xkcd.com/1172/
From what I could see reduced stiction and friction is almost always a benefit. Of course there is the use-case in which people rely on the degraded performance of the robot system.
If this is actually the case, an option to operate it with more stiction and friction can be added. E.g. via some parameter.

[niederha]

My apologies, I think I might not have made myself clear but we are getting there.

So to be sure... now if I start the "FRIOverlay" program on the IIWA and select torque for control, choosing any stiffness other than 0 on the IIWA screen will result in actually observing the dithering in the robot?

Reminds me a bit of https://xkcd.com/1172/

This is funny and made me laugh. Regardless I say this update is breaking things not because it reduces friction but because I have the option to choose a non-zero stiffness in the dialogue box of "FRIOverlay" and choosing anything other than 0 is actually resulting in an unwanted behavior (i.e. I see the robot dithering). It is my understanding that some stiffness in the joint control is pretty standard, I have the option to pick it, and I believe that some of the controller developed in our lab are mathematically unstable with null stiffness.

So does the user now have to compute stiffness? Should we remove the dialog box for the stiffness choice in the java app? Or am I still completely missing the point?

[matthias-mayr]

So to be sure... now if I start the "FRIOverlay" program on the IIWA and select torque for control, choosing any stiffness other than 0 on the IIWA screen will result in actually observing the dithering in the robot?

Yes, that would be the expected behavior. tbh, I never did that, because it felt like a not very-smart combination. But there was one person who did and he saw it moving.

This is funny and made me laugh.

:-)

Regardless I say this update is breaking things not because it reduces friction but because I have the option to choose a non-zero stiffness in the dialogue box of "FRIOverlay" and choosing anything other than 0 is actually resulting in an unwanted behavior (i.e. I see the robot dithering).

This is totally correct. I think the issue here is more the FRIOverlay program. With iiwa_ros it does not really make sense to choose any other stiffness than 0 when doing torque control mode. In my own adaption, I removed that dialogue when selecting torque control and it is directly set to 0.

I can give you an example on why this combination does not make sense for me. Let me know if I am missing sth.:
If we do torque control via FRI we are not able to update the positions anymore. For example you initialize torque control at with a joint impedance stiffness of 100 at all 0 joint position. In torque control mode, these joint position would always stay the reference values for the joint impedance control mode.
So if you do not send any torque commands from ROS, your robot behaves as joint impedance controlled. Right now this would mean that it does this dithering, so let's assume this is not there not there. If you then perturb your robot by some angle alpha, you would feel a repelling force that is proportional to the angular error (joint impedance control) and as soon as you release it would move to the 0 position.

Now you have the option to send additional torque commands from ROS, so e.g. you want it to rotate around joint 0 and you command 5nm to the joint. Now it would move into that direction until the "repelling" force of the joint impedance controller is also 5nm and a new equilibrium is established. When you send 0nm from ROS it would move back.

The issue is that you can not correct these reference joint positions anymore if you want to send torque command through FRI. So I do not really see an option to bring the robot to a very different configuration.

It is my understanding that some stiffness in the joint control is pretty standard, I have the option to pick it, and I believe that some of the controller developed in our lab are mathematically unstable with null stiffness.

My take would be that if you really want to use joint impedance control, you either establish an FRI connection for joint position (and not torque) updates (in which there would be no dithering) and then you can easily update the position references for KUKA's joint impedance controller. For example this RL code does that.
Or your other option is to not use the KUKA joint impedance controller and run your own, e.g. in ROS and send the torque commands.

So does the user now have to compute stiffness?

I am not quite for what you mean with "the user now have to compute the stiffness". There are many torque controllers available in the literature that you could use. Most of them would have some sort of stiffness.

Should we remove the dialog box for the stiffness choice in the java app? Or am I still completely missing the point?

It would indeed be an option to remove the joint-impedance stiffness dialogue if the user set the FRI connection to torque control. As explained above, setting a stiffness with torque control enabled does not really make sense to me.

[matthias-mayr]

@niederha I found a way to look at the video. It did not really show in the Github issue.

The behavior is a bit surprising actually. I expected it to make smaller, slower movements, but it moves quite fast. It's not what my robots are doing even when I send 0 torques.
You are using the FRIOverlay file, right? I have a couple of questions then:

1. Which controller do you select on the ROS side?
2. What are the tool values on the Smartpad? Mostly weight?
3. Which external torques does it show before starting the FRI connection?
4. This behavior in the video really happens only when this joint-position dithering is on in torque mode?

[niederha]

1. Which controller do you select on the ROS side?

I'm using the torqueController, which is the defualt controller when launching iiwa_bringup

2. What are the tool values on the Smartpad? Mostly weight?

I'm using 'FRIOverlay' and not 'FRIOverlayGripper' so, if I read the code correctly, no tool values are used. Notice, there is no tool mounted on the robot.

3. Which external torques does it show before starting the FRI connection?

In zero position, the torque on each joint is within [-1.0; 1.0] Nm

4. This behavior in the video really happens only when this joint-position dithering is on in torque mode?

Yes. This is the point of this issue.

Now, I have an explanation for this behavior. In the internal impedance controller the stiffness term is formulated as (q-q_desired)*K. In normal operation, before the commit, q_desired is updated with the current pose of the robot in the control loop. After the commit q_desired includes the dithering. That should just make the robot oscillate. However, grav comp is not that good, so overall the robot falls. Combine that with the fact that I actually had chosen a fairly high stiffness coefficient for the robot in this video and that seems like a reasonable explanation for this movement pattern. Note that I tried later with lower stiffness. I have to manually hold the robot up to help grav comp but the I do see the dithering.

Does that seem a likely explanation to you?

[niederha]

Can give you an example on why this combination does not make sense for me. Let me know if I am missing sth.:

I think it might be the case.

If we do torque control via FRI we are not able to update the positions anymore.

If my reading of the code is correct, that is not the case. We it seems that the position in the robot command sent in Iiwa:_write¹ Is the desired position we take for stiffness computation. And this is the reason we observe physically the dithering when the stiffness is non-zero.

Footnotes

1. Unrelated note, for further reference, starting method names with an underscore is a python convention, it might lead to issues in c++ ↩

[matthias-mayr]

I'm using 'FRIOverlay' and not 'FRIOverlayGripper' so, if I read the code correctly, no tool values are used. Notice, there is no tool mounted on the robot.

Totally correct. My bad.

In zero position, the torque on each joint is within [-1.0; 1.0] Nm

That sounds alright. It's about in the same ballpark of what we see as well.

Now, I have an explanation for this behavior. In the internal impedance controller the stiffness term is formulated as (q-q_desired)*K. In normal operation, before the commit, q_desired is updated with the current pose of the robot in the control loop. After the commit q_desired includes the dithering. That should just make the robot oscillate.

Correct. It would make it oscillate if one chooses a stiffness for the KUKA joint impedance controller.

For our robots, since we want to do "pure" torque control, we select a joint impedance stiffness of 0 and then it works. Both for us and also other people. So I am wondering what's different in your setup.

I assume that you also started the FRIOverlay program in that 0 joint position? There is the issue that it always needs to be completely cancelled (unticking it from the programs list) and start it again from scratch.

However, grav comp is not that good, so overall the robot falls. Combine that with the fact that I actually had chosen a fairly high stiffness coefficient for the robot in this video and that seems like a reasonable explanation for this movement pattern. Note that I tried later with lower stiffness. I have to manually hold the robot up to help grav comp but the I do see the dithering.

How do you want to control your robot? With joint position references or joint torque references?
My understanding is that you want to do torque control on top of KUKA's joint impedance controller. This is a really interesting use-case that I haven't seen anyone doing before. People either want to do "pure" torque control or position control. The latter can actually be combined with the KUKA joint impedance controller as I said earlier, but one still actively sends joint position references then. In iiwa_ros this means to load the position control interface.
Right now you are in the torque interface mode in which you would send joint torque references.

If my reading of the code is correct, that is not the case. We it seems that the position in the robot command sent in Iiwa:_write1 Is the desired position we take for stiffness computation. And this is the reason we observe physically the dithering when the stiffness is non-zero.
Does that seem a likely explanation to you?

You are totally right that you would observe this oscillation.

In the iiwa_ros torque control mode, we are just sending the current position references so the KUKA joint impedance controller does not interfere with the torque commands. This is necessary because KUKA decided that to send torque commands, one also needs to send a position reference that is sufficiently close to the current position. Even if the stiffness of the joint impedance controller is set to 0.

All torque controllers I have seen so far do not expect the "internal" robot controller to interfere. But if one configures some stiffness for KUKA's joint impedance controller, it might actually do that and it can be quite challenging to estimate how the joint impedance controller would behave. Without this dithering, we would send the last read joint positions as a new reference at the FRI frequency, so 200Hz-1000Hz.

Anyway, I don't want to be the grumpy person sitting on top of some code and questioning what people want to do (Reddit /Stackoverflow Reference).
I am mostly asking and writing things, because I can't fit into my head that disabling this "stiction hack" could be beneficial for some use-case. We can make such a switch either way, but it makes more sense if one understands why there should be a switch.

[niederha]

Alright. I think the issue is quiet clear now. I understand how everything works now. I was a bit confused I think because all control done on the robot side and FRIOverlay uses the Impedance controller denomination, thus I thought it would be expected that uses this to implement the impedance controller's friction and damping (which is also likely biased by the dithering but I couldn't do a conclusive experiment on it).

Alright so as I see it, as it is the repo might give a few headaches to a new user. Here is some suggestion I have that could fix it.

1. Remove the option to choose damping and stiffness on the IIWA side (I think this is the best course of action, and thus the torque controller is moving away from being an onboard impedance controller and becomes just a pure torque controller. Yet this breaks backwards compatibility, this might be an issue for some user of this repo. I at least would like to check who it might impact in my lab before proceeding)

2. Implement a switch to turn off or on the dithering depending on the use case. IMO I agree with you that it makes less sense but at least it keeps backward compatibility and once again this might be important for some users.

3. Rework both sides of the app to be able to get the damping and stiffness parameters on the computer side and only apply dithering if both are zero. I think for an end user this is the most seamless option, doesn't break backward compatibility but is more work. (I'm actually not sure it is possible)

I like the first option, I just would like to make sure I'm not breaking any old demo from our side.

I'm happy to build the MR for it. Just would appreciate your input and maybe the one from @fkhadivar (Who was the most knowledgeable person in the lab about this repo) before moving forward with it.

[costashatz]

Hello @niederha and @matthias-mayr ,

When I first created iiwa_ros, the only reason I had stiffness/damping selection in the smartpad when TorqueControl was selected is to allow for intuitive and easy kinesthetic teaching (I honestly cannot think of any other "useful" use case). Grav compensation alone (i.e., TorqueControl with zero stiffness and damping) is not enough to allow intuitive kinesthetic teaching, whereas one could find a nice combination of stiffness/damping gains such that the robot would not move when releasing it, but also it would be easy to move when grabbed.

That being said (and for being backward compatible), I think that option 2 is the most reasonable one. We "assume" that when in TorqueControl users only select zero stiffness (we can add something in the README as well), and thus the dithering is enabled by default. But we give the choice to the users to disable it in case they want to do something different or simply nice kinesthetic teaching.

Thanks both for the exchange of ideas ;)

What do you think?

[matthias-mayr]

If we do torque control via FRI we are not able to update the positions anymore.

If my reading of the code is correct, that is not the case. We it seems that the position in the robot command sent in Iiwa:_write is the desired position we take for stiffness computation.

I was a bit imprecise and also had a though error here. So yes, the reference positions are updated to be the current ones (with or without dithering).
What I actually meant is that from a user or task perspective one can not actively update the reference joint positions when in iiwa_ros torque control mode.

to allow intuitive kinesthetic teaching, whereas one could find a nice combination of stiffness/damping gains such that the robot would not move when releasing it, but also it would be easy to move when grabbed.

Okay, that I did not expect anyone to do. I would have expected you to make a proper control solution :-)
It mostly felt unintuitive, because then one couldn't really continue to run anything after it without switching control modes. But now it makes sense.

@niederha: Thanks for the suggestions.

1. Remove the option to choose damping and stiffness on the IIWA side (I think this is the best course of action, and thus the torque controller is moving away from being an onboard impedance controller and becomes just a pure torque controller.

I would actually vote for that. I think that the use-case of having torque control and do some sort of joint impedance control is quite niche, unless @niederha, you want to keep this way of kinesthetic teaching running.

2. Implement a switch to turn off or on the dithering depending on the use case.

I would again depend it on your way of kinesthetic teaching. If you need it, sure we can quickly add a switch. It's faster to do it than to spend more time discussing it.

3. Rework both sides of the app to be able to get the damping and stiffness parameters on the computer side and only apply dithering if both are zero.

As you already noted, I am afraid that the FRI connection can not submit such information.

Thanks for the discussion. I learned something & I hope you too.

[matthias-mayr]

We just had someone else to do torque control, select some joint impedance stiffness and observe a similar behavior.

It would make sense to document how torque control mode should be used.

[niederha]

Yeah, sorry, things piled up and I never got to doing an proper fix for this issue... I'll see if I can do it within the month as I have some work with the IIWA coming up