[CI] Cartesio reset marker issue

[EdoardoRomiti]

I found a possible issue related to the reset marker functionality in Cartesio. Whenever, after I put the robot in gravity compensation mode, I want to resume impedance control I would like to update my reference pose to the current one assumed by the robot. If this doesn't happen I would see a jump when putting back stiffness to non-zero value, because the pose error would be big. This seems like the purpose of the reset marker, but instead it seems the reference is updated only after moving the marker again after the reset.

Example:

1. Initial configuration of the robot, with topic /cartesian/L_4_A/state echoed on terminal on the right

2. We move the robot by hand to a different configuration (stiffness are zero) and do reset marker. The references are not updated

3. Plot of the x, y, z references after "touching" one of the arrow of the marker so it would update. Looking at the plots it seems to be doing an interpolation so to create a trajectory between the previous reference and the actual one

4. Final values of the references after touching the arrow, now updated

Is this the expected behavior of reset marker? Did I interpret it wrong? If so, is there another way to reset the reference to the current pose of the robot?

[EnricoMingo]

Ciao Edo, this is a known issue related to the fact that CartesianMarker are intended for position control and does not have concept of Impedance. So, the right way to use the Cartesian marker should be:

0. suppose that you move the end-effector with the marker
1. you change the Cartesian impedance, for example you set to 0 the Kx
2. you move the arm "manually" along the x
3. now suppose you want to raise the impedance again, before you should reset the marker (right button, reset), then increase the stiffness. In this way you reset the reference for the marker and raising the gains the robot should not jump.

Can you try this sequence?

[joernmalzahn]

Thanks Enrico. So there shoul be a way to "click the reset button" through a command, right?

I am thinking of the implimentation of a setter mechanism for the stiffness that could first call this reset when used together with the CartesianMarker. Such an automatism would avoid strange behavior during demos and human mistakes resulting in dangerous jumps.

Any suggestion how to implement such a mechanism?

[EdoardoRomiti]

Ciao Enrico, I tried the sequence you suggested but still get the same problem. I tried also on the 3rd floor arm, but I get the same. Step 3: after the reset when I increase stiffness again the robot jumps.

By looking at the reference in fact it does not change after the reset. It changes only when you move the marker again from the new position. I can provide a video (maybe privately) if it helps explaining better.

[alaurenzi]

I think what we are seeing here is that the marker will not publish anything on reset. It only publishes when you move it around. The reset functionality is just meant to start the marker from the correct pose which matches the current robot state.

In order to reset the whole solver to the current state, you might try the service /cartesian/reset. Give it a try in simulation first though, since I am not very sure if the RT implementation has been done, and if yes, it hasn't been given much testing for sure.

[EnricoMingo]

Yes, this is true, the marker is not going to publish anything before you move it. We can indeed fix this behavior if you want. In that case please mark this as a bug and assign it to me.I will fix it asap.

@joernmalzahn if we fix this we can imagine a mechanism as you suggest implemented inside the plugin, for example.

[alaurenzi]

To me it’s not really a bug.. the current behaviour is nice because you can reset the marker, check visually that everything is ok, and only then move the marker to piblish..to reset the solver there is the service I mentioned

[EnricoMingo]

Yes, but resetting the solver involves the creation of a new solver, tasks ,etc... which may compromise the RT. Is the reset a virtual method which ca be reimplemented? In that case we can reimplement it for the Cartesian Impedance plugin so that the references of the task are updated with the current position and that's it.

[alaurenzi]

This is already the current implementation :D

[framc2]

imho this behaviour can actually create some issues. For example: let's suppose there is a discrepancy between the current robot position (x) and the reference (i.e. the marker position, r): for instance this can happen if we move the robot with null stiffness or if the marker is not reachable by the robot. In this case, if we reset the marker, the reference r is not updated, so there is still an error between the two positions (r - x). When we click on the resetted marker, the simple planner generates a trajectory from r to x, causing the robot moving. It is not a critical issue actually, but sometimes we got some unexpected behaviour from the robot, since we often move the robot without stiffness.

[alaurenzi]

So this is a different behavior than I expect. I expect that if you reset the marker, nothing happens, because the marker should not publish anything on reset. If this is not true, then I misunderstood and it is indeed a bug.

Then there is a slightly different issue which is that the solver (we need here to distinguish between the solver, i.e. the core of the algorithm, and the marker which is a bare interface) has an internal reference for the task which is now far from the real robot pose (because of what Francesco mentioned in the previous post). This is where the /cartesian/reset service comes into play. If you don't call it, there will be automatic interpolation between the old reference and the new, under the specified velocity and acceleration limits.

Am I missing something?

[alaurenzi]

Ok, now maybe I see the issue. The marker resets itself not according to the /cartesian/task_name/state topic, which contains the internal reference of the solver, but according to TF, i.e. the real robot state. This maybe is the real bug.

[EdoardoRomiti]

With @EnricoMingo we found a solution to this problem by using the reset_cb method of the RosServerClass, which was declared but not implemented. (Enrico, if I miss something please correct me, as I cannot see these changes now).

When the service /cartesian/reset gets called, the callback first resets parameters and posture using the other two callbacks: reset_params_cb and reset_posture_cb, and then sends an empty message on a topic "changed_controller_event" which let MarkerSpawner know that the markers should be reset . Also the LockfreeBufferImpl::reset() method was implemented as it was missing. This is on a new branch called issue_26_cartesian_interface. @alaurenzi : what do you think of this solution?

Although we tested this in simulation on Enrico's PC and it seems to work (no jumps when setting back the stiffness to non-zero values), I couldn't test it more as I don't have access to the CartesianInterface and cannot pull these changes. If I want to test this in simulation before going on the robot how can I do it?

I also cannot try this on the plastic-mockup 4-dof robot we have since I neither can pull from the tree com-express.

[EnricoMingo]

Hi @EdoardoRomiti, some corrections: the service /cartesian/reset call the reset() of the CartesianInterface implementation which resets the references of the tasks and the posture of the robot as well (the reset_params_cb is not used). Once this is done, the topic which trigger the reset of external ros nodes, such as markers and joystick, is published.

[alaurenzi]

Good try! However, this solution clashes with the already-existing reset service, which is opened by the ROS main (this is why you don't see it on the RT layer). There is a reason why I was opening the service inside the ROS main and not inside RosServerClass (as you did): the reason is that only the ROS main knows about the robot (the rest of the framework only uses models). Now, because you're working with a feedback controller, your model is always in the same state as the robot. But when we use open-loop kinematic control, the model state is the result of the integration over time of the control variable (e.g. qdot). In such a case, the reset functionality must transfer the robot state to the model. What I will probably do in order to fix this conflict is to replicate the reset service (currenty part of the RosExecutor class) inside the CartesianPlugin.

[liesrock]

@EdoardoRomiti was this then solved?

[EdoardoRomiti]

Yes, we are using the solution by Enrico to reset the reference pose after going to zero gravity and it works well. This is on the issue26_cartesian_markers branch of the Cartesian Interface.

What I will probably do in order to fix this conflict is to replicate the reset service (currenty part of the RosExecutor class) inside the CartesianPlugin.

I guess this conflict @alaurenzi was mentioning still needs to be solved, but the reset service was tested and we are using it