Avoiding the: `cartesian_reflex`

[wyqsnddd]

Dear all, @ndehio @gergondet @kheddar

I have encountered two issues that lead to different [franka::ControlException]:

1. Abrupt motion: generates: cartesian_reflex
2. sudden force jump generates: [tau_J_range_violation]

To enable more dynamic motion, I am wondering that if we can fix issue 1? I found that it affects the admittance contact, either with rigid-surface contact (1.1) or rigid-surface contact (1.2).

issue 2 is not fixable, I listed it here for documentation purposes. It might hinder the post-impact state observation. In the figures below, when the 5th joint torque limit [12 Nm] was hit, it throws [tau_J_range_violation] and the log stopped. Hence we may not observe the state jump or the oscillations correctly.

1. Abrupt motion

I got the Franka::ControlException error. From the plot below, we can see that the error was triggered at around 20N.

I applied the SinglePandaTestAdmittanceController with different modified transitions.

  - [InitialPosture, OK, MoveDownStart1]
  - [MoveDownStart1, OK, MoveDownContinue1]
  - [MoveDownContinue1, OK, Apply10ForceAdmittance]
  - [Apply10ForceAdmittance, OK, Apply20ForceAdmittance]
  - [Apply20ForceAdmittance, OK, InitialPosture]

1.1 Hard contact case:

Set the contact with 0.02 m/s, and then apply the admittance task. While the contact sustains, I switched the reference from 10 N to 20N. I am not sure either it is the force or the velocity that triggered the exception. The data is here

[warning] [mc_franka] panda_default missed control data (previous: 33368, current: 33370, expected: 33369
[warning] [mc_franka] panda_default missed control data (previous: 33370, current: 33370, expected: 33371
[info] Completed taskAdmittance (timeout)
[success] Completed Apply10ForceAdmittance - output: OK
[success] Starting state Apply20ForceAdmittance
[info] Removed task taskAdmittance
[info] Added task taskAdmittance
[warning] [mc_franka] panda_default missed control data (previous: 33438, current: 33440, expected: 33439
[warning] [mc_franka] panda_default missed control data (previous: 33440, current: 33440, expected: 33441
terminate called after throwing an instance of 'franka::ControlException'
  what():  libfranka: Move command aborted: motion aborted by reflex! ["cartesian_reflex"]
control_command_success_rate: 0.99
Aborted (core dumped)

1.2 Soft contact case:

I enabled the admittance task and interacted via the "Foot" surface by hand. The data is here

arning] [mc_franka] panda_default missed control data (previous: 9128, current: 9131, expected: 9129
[warning] [mc_franka] panda_default missed control data (previous: 9131, current: 9131, expected: 9133
terminate called after throwing an instance of 'franka::ControlException'
  what():  libfranka: Move command aborted: motion aborted by reflex! ["cartesian_reflex"]
control_command_success_rate: 0.98
Aborted (core dumped)

2 Joint torque jump:

Although it seems appealing, reset the joint torque limit via the setCollisionBehavior does not help. According to Niels @ndehio and the data sheet the maximum hard joint torque limits are:

• 12 Nm for the last three
• 87 Nm for the first four.

FE is not going to modify it. Hence, we can not generate impact motions more than that. The data is here

[info] Removed task body6d_panda_foot_panda_foot
[warning] [mc_franka] panda_foot missed control data (previous: 1152, current: 1152, expected: 1153
[warning] [mc_franka] panda_foot missed control data (previous: 1229, current: 1233, expected: 1230
[warning] [mc_franka] panda_foot missed control data (previous: 1233, current: 1233, expected: 1236
terminate called after throwing an instance of 'franka::ControlException'
  what():  libfranka: Move command aborted: motion aborted by reflex! ["tau_J_range_violation"]
control_command_success_rate: 0.97
Aborted (core dumped) 

[ndehio]

The admittance task is basically a motion-controller. It is of course not a good idea to suddenly change its force-reference (for any robot). This will lead to jumps in the control input and panda is very sensitive in this regard...

So far I know, mc_rtc / tasks is currently missing a feature that would allow to easily specify something like a smooth force trajectory for a task like admittance task.

[wyqsnddd]

Suppose we are working on impact, of course there is a sudden change of something, either the velocity or the force, either on-purpose or not.

If we can not handle this cartesian_reflex, either by avoiding it or lifting the dependent internal limits. I am afraid that the controller can not finish a entire cycle.

[kheddar]

I suggest that we contact the new guys from FE to see if the cartesian_reflex can be cancelled. I am wondering if we will have the same problems or not with the UR robot? Because if the Panda is limited, we cannot do much about it actually!?
I would like to have clear numbers w.r.t to what is the max velocities where these reflex is triggered?
I agree that at this stage of investigations, we shouldn't switch to admittance and we need to work in open-loop. Basically to make sure we have good post-impact prediction model, we need to operate the impact in 'open-loop'. What I mean by open-loop is the following:

1. set a position target lightly the plan to touch, the how much under depends on the speed one wants to impact the plan. We can talk about this tomorrow because this is very tricky
2. monitor contact if impact happens we must stop the servo or set a safe target point to reach slightly above the surface (but do not switch to admittance control)
3. the reason is that we want to monitor simply whether the post-impact prediction in terms of post-impact velocity and torque matches our model (hence I want simply to monitor the post impact state and not implement any recovery strategy or switching for now.