"cmd.cartPos not properly initialized" error with cartesian impedance controller

[marcoesposito1988]

Hey Carlos,

I am trying the cartesian impedance controller from the vito-devel branch and I cannot make it work yet. I found out a problem and solved it in #66 but I still get the following error: "cmd.cartPos not properly initialized".

Any idea about the reason?

[teiband]

Hi Marco, I am also working on a version to use the native cartesian impedance mode since I mailed you and I am facing the same problem. Even if I mirror the position in friRemote::doCartesianImpedanceControl() with: cmd.cmd.cartPos[i] = msr.data.msrCartPos[i] + msr.data.cmdCartPosFriOffset[i]; or: cmd.cmd.cartPos[i] = msr.data.msrCartPos[i];

I found out that the coordinates displayed on the KRC side are in world coordinates, as the msrCartPos are in the robot's coordinate system. So do I need to command the world coordinates or the measured ones?

Regards, Thomas

[marcoesposito1988]

Hi Thomas,

while debugging I found out that "short-circuiting" lwr_hw_fri to just pass getMsrCartPosition while in cartesian mode, I seem to maybe get past that error; however I get immediately another one, just as enigmatic: "cmdFlags changed during cmd mode". Since I just do always the same thing in that for loop, this sounds quite unjustified. I will take a look at the internals of friRemote to find what could possibly change the flags.

However, this sounds like a problem during the switch: possibly a write is done before a read. It is very possible that the first iteration after switching sends garbage to the controller, which would then justifiably be angry. I will try to hack a simple synchronization to be sure to read before writing, and in case that solves the problem I think we may have a lead.

[marcoesposito1988]

No luck: there appears to be no write before a read. So the controller should never receive an unplausible value, in theory. I start to run out of ideas, the code looks correct.

[teiband]

Hi Marco, disabling the reset of the command flags seems like it prevents "cmdFlags changed during cmd mode" // cmd.cmd.cmdFlags=0; Anyway I got the "cmd.cartPos not properly initialized", which occurs sporadic after some seconds or minutes. Actually I test it with a cycle time of 5ms and the robot is running forever, while it is just feeding back msr.data.msrCartPos. But still not able to send changing position commands.

I suspect a communication timing issue in cartesian impedance mode within the 2ms cycle time.

[teiband]

I made some print outs in doCartesianImpedanceControl() and it seems that the function is called with the initial values of cart_poscommand directly after the controller switch. So they are not overwritten with the current robot position before doCartesianImpedanceControl() gets called the first time.

Any Idea how to solve this?

[teiband]

I added the setCommand to the init() and start() functions of the cartesian_impedance_controller in order to initialize the commands and fixed a typo: 1ab5b02d74a4d087d2930e34ea29c2817d1f2688

Now it runs for me.

[marcoesposito1988]

Awesome! Somehow I missed that. Integrating the hack you mentioned (commenting out cmdFlags=0), I also manage to get the controller running.

Still, without the hack I get the infamous error. This bugs me a lot. I will try to understand the reason for that, and if eventually we have to live with it.

[marcoesposito1988]

Well, the robot seems to be able to switch back and forth between cartesian impedance and other controllers. So I am committing the hack.

If my understanding is correct, not resetting this flag could give problems if one of the inputs of doCartesianImpedanceControl that was sent stops being sent to the robot. Then the flag remains set, and old data could continue to be sent. While this should not give problems with the cartesian impedance controller here, which does the same thing over and over, another more advanced controller could have problems.

[carlosjoserg]

Hey Marco and Thomas, sorry for my slow reply...

It's great you come back with such good enhancement on the KUKA FRI side... I started it but couldn't test/finish it...

I assume you are using the forked ros-control due to this change CentroEPiaggio/ros_control@809f70b9e9691c64bd254147c2102719b6817ef6, otherwise it wouldn't compile, right? Don't think they will accept that in ros-control, since looks weird, however, when one uses ros-controls as an intermediate layer like here, where you have access to directly send Cartesian commands, is kind of useful. Suppose with drones, there must be some high level API's like that too.

While this should not give problems with the cartesian impedance controller here, which does the same thing over and over, another more advanced controller could have problems.

It's ok by me, first thing is to support the native one as it is.

---

Recall @marcoesposito1988 that you are collaborator of the project with push access, so feel free to commit to the either vito-devel or, if possible, to ros-i-specs, which is already big, I know, but the idea is to merge it to master... someday!

I will dedicate efforts in completing the Stanford FRI then, I'd like to leave this one as default with the RT opt-in.

Many thanks !

[marcoesposito1988]

@carlosjoserg: thanks! Since I was out of the loop for a long time I wanted to hear from you first. As soon as I am confident I will start poking around.

I will do some more testing and then merge the fixes for this controller.

[hamalMarino]

Just for reference purposes, working with @manuelbonilla on the JointImpedanceController control directly using the Kuka interface for commanding the robot (i.e. doJntImpedanceControl), we needed to hack the same instruction

// cmd.cmd.cmdFlags=0;

in order to have that interface working. This, in turn, led the switch to Cartesian Impedance Control mode to no longer work properly. We thus gave a shot at resetting those flags only when a switch in control mode (e.g. here) happens, i.e. when a call of the type setToKRLInt(0, NEW_CONTROL_MODE) is made. The fix is the following (will be included in a PR later on, with fixes regarding the JointImpendanceController):

void  setToKRLInt(int index, int val) { cmd.krl.intData[index]=val; }

becomes

void  setToKRLInt(int index, int val) { cmd.krl.intData[index]=val; 
   if(index == 0) cmd.cmd.cmdFlags = 0;
}

Though it will be further tested in the following days, it behaved well till now.

[marcoesposito1988]

This appears to be fixed now. Thanks!