fix controller

[tonynajjar]

1- Fixes convert_trans_rot_vel_to_steering_angle formulat
2- Adds slowing down until steering angle target is reached
3- Adds extreme angles conditions

[tonynajjar]

@jplapp I added the requirement for an approval to merge. Now that this repo will be very important, feel free to set the necessary PR requirements

[jplapp]

looks good, some comments on the math

[jplapp]

• why devide it by 2?
• you should limit this to positive value, otherwise it will be scaled in the negative direction, which will create unexpected results, e.g. sth like Ws_write *= max(0.01, cos(alpha_delta/2)); . We used 0.01 here, to have some wheel motion already so that flexisoft wakes up.

[tonynajjar]

My goal was to find a function that for x between [0, pi] would give y between [0,1] to keep it as one mathematical function and not add more checks like max().

I found cos(x/2) to do the job.

alpha_delta is between [0, pi] given that alpha_write and alpha_read are normalized (above discussion)

[jplapp]

ah, I see.
But with this scaling, if input is "turn right" (pi/2), and current angle is 45 degrees left (-pi/4), this would already give 38% (update: corrected) of the speed. In the previous implementation, the idea was to have 0% speed if angle is incorrect by more than 90 degrees

[tonynajjar]

We should probably add "last minute" checks in some places to make it safer but shouldn't make it too redundant if we are sure of our inputs' restrictions

[tonynajjar]

In the previous implementation, the idea was to have 0% speed if angle is incorrect by more than 90 degrees

I see, I missed this in the initial implementation, wasn't very explicit. I'll implement it as std::max(0.01, cos(alpha_delta)) then but we should do something about this 0.01 magic value for the generic controller. (and make it more explicit)
That's the profile then

[tonynajjar]

We can already make it more explicit. What do you think of:

  double scale;
  if (alpha_delta < M_PI_4) {
    scale = 1;
  } else if (alpha_delta > M_PI_2) {
    scale = 0;
  } else {
    scale = cos(alpha_delta);
  }
  Ws_write *= scale;

Replace M_PI_4 and M_PI_2 with whatever you'd like. I think that's a good example where explicit is better than implicit

P.S: The reason for the first condition is that there will always be a small error but at some point we want to run with full speed

[jplapp]

agree with all your points! I would use pi / 6 as the first threshold, but we'd need to give that a try in a real-world situation then to finetune.
Also, instead of scale=0 -> use that magic value scale=0.01

[tonynajjar]

Also, instead of scale=0 -> use that magic value scale=0.01

Will do.
Are you sure there is not a more direct way to keep flexisoft awake? e.g setting a certain bit

[jplapp]

currently, we use jetson_speed_cmd, that is in turn based on whether we have some command or not. This, in turn, is calculated based on whether we have some speed. Of course we could change this calculation to use some different input.

Flexisoft then runs some timer once this bit is false.
Speaking of this, I'm not sure if you set this bit anymore (it's set in can_bus_communicator).

[jplapp]

when subtracting angles, always ensure that they are normalized, as e.g. an angle of 0 is equivalent to 2*PI. Can we assume that the input angles (so alpha_write and the steering joint state) are normalized?

[tonynajjar]

My assumption were that they are always normalized. alpha_write comes from process_twist_command's arctan which is between -pi/2 and pi/2. The read alpha comes from read() in the hardware interface which is scaled also between -pi/2 and pi/2.

[jplapp]

ok, good then

[tonynajjar]

I ran uncrustify on it, will need to find a way to run our pre-commits here also

[tonynajjar]

Anything left to do here from your side @jplapp?