"Fake" velocity control issues

[omarrayyann]

So, we are interested on doing joint velocity control, but since (it seems) FRI does not provide a velocity interface directly, what we are doing right now is the following closed loop scheme:

(a) Measure current joint position q
(b) Compute joint velocity qdot = f(q)
(c) Integrate it numerically: q_next = q + dt*qdot, in which dt is the inverse of the frequency we are using.
(d) Send the next joint position q_next using Position mode.

Sometimes there is a weird behavior in which, from exactly the same initial configuration and with exactly the same parameters, it does an unexpected movement from a while and then, a couple of seconds later, it performs the expected motion (i.e, the one that we would expect from simulations). Furthermore, this unexpected motion seems to be always the same: it appears randomly once we start the loop but when it appears, it is always the same motion. So, to summarize, it seems that every 4 trials or so it works as expected 3 times and once it does a weird motion (always the same!) for a while and then it goes back to the normal behavior.

We are running a simple resolved rate controller for pose control and the function f(q) is deterministic, so from the same initial configuration we would expect mostly the same motion. It is important to mention that we tried the same scheme using the SmartServo interface and it was working as expected. We wanted to switch from SmartServo to FRI in order to be able to control at higher frequencies.

We are wondering if there is something wrong with the loop (a)-(d) described above. Maybe we need to change some configuration? We tried different stiffness, and the behavior persists.

Thanks.

[costashatz]

Are you controlling via python or C++? This "smells" like a variable issue; like a variable is not initialized or it is changed non-intentionally. This is very easy to do in python because of pass by reference nature, but it can also happen in C++. Is it possible to give us some code (at least a small reproducible example) to investigate more on what's happening?

By the way, can you also share a video of the "weird behavior"? If qdot is very small, KUKA joints can struggle to move because of the friction of joints and it could create some "weird" behaviors. This is less likely though.

Maybe @matthias-mayr or @fkhadivar have some more ideas?

[CodingCatMountain]

So, we are interested on doing joint velocity control, but since (it seems) FRI does not provide a velocity interface directly, what we are doing right now is the following closed loop scheme:

(a) Measure current joint position q (b) Compute joint velocity qdot = f(q) (c) Integrate it numerically: q_next = q + dt*qdot, in which dt is the inverse of the frequency we are using. (d) Send the next joint position q_next using Position mode.

Sometimes there is a weird behavior in which, from exactly the same initial configuration and with exactly the same parameters, it does an unexpected movement from a while and then, a couple of seconds later, it performs the expected motion (i.e, the one that we would expect from simulations). Furthermore, this unexpected motion seems to be always the same: it appears randomly once we start the loop but when it appears, it is always the same motion. So, to summarize, it seems that every 4 trials or so it works as expected 3 times and once it does a weird motion (always the same!) for a while and then it goes back to the normal behavior.

We are running a simple resolved rate controller for pose control and the function f(q) is deterministic, so from the same initial configuration we would expect mostly the same motion. It is important to mention that we tried the same scheme using the SmartServo interface and it was working as expected. We wanted to switch from SmartServo to FRI in order to be able to control at higher frequencies.

We are wondering if there is something wrong with the loop (a)-(d) described above. Maybe we need to change some configuration? We tried different stiffness, and the behavior persists.

Thanks.

Hey, Sir. You said the velocity control can be utilized in iiwa_stack?? How could it happens? I try to publish the joint velocity via the related topic, but it doesn't move.

[matthias-mayr]

You said the velocity control can be utilized in iiwa_stack?? How could it happens? I try to publish the joint velocity via the related topic, but it doesn't move.

iiwa_stack uses KUKA's Smartservo and can in principal do that. However when I tried using it with position control, it did not work. Furthermore, it's unmaintained for some time.

@costashatz: Why does this driver expose a joint velocity interface at all? It's not sending joint velocity commands to the robot in any case.

@CodingCatMountain: I have no particular idea for your problem either. If I were you, I'd do the standard debugging thing. Record some rosbags and/or configure some multiplots and see if I can find differences.

One alternative, that needs a lot of tuning though is to use a joint velocity controller that converts it to efforts/torques. However, you would most likely spend some more time with the position control before going down that road.

[costashatz]

@costashatz: Why does this driver expose a joint velocity interface at all? It's not sending joint velocity commands to the robot in any case.

Good question. No idea! We should remove the velocity interface!