no sliding in collision prevention

[baumanta]

This code simplifies and improves the collision prevention algorithm. The current implementation allowed the collision prevention to change the direction of the given stick input (this manifested as a slight sliding along the obstacle). But this feature resulted in some unexpected and non-intuitive behavior. Also the stopping maneuver contained an undesirable roll motion.

This PR takes out this directional change of the setpoint. Now the stick input is only scaled by the max velocity and not modified in direction anymore. This resulted in a much nicer stopping behavior.

This was tested in SITL and in a flight test. The attached logs contain flights where we tried to push the drone into a tree. In the direction of the tree the original setpoint is adapted to prevent the maneuver.

setpoint y current master
https://logs.px4.io/plot_app?log=d6f59ea3-d343-41b1-9970-734de2bcb24a

setpoint y PR:
https://logs.px4.io/plot_app?log=16ac810e-7ebc-4eed-bfc7-a7e2e0a020f0

[jkflying]

A lot easier to read and understand 👍

[mhkabir]

What's 0.94?

[baumanta]

It is just an angluar limit to only consider distance data in the direction where you actually want to fly. In this case sin(70deg) = 0.94. But you made me look into that again, and there was actually a sin/cos mixup. It is very important to check that vel_setpoint_bin >0 there. That means we do not consider distance data in the opposite direction as the setpoint. Any harder constraint is tuning. If we only leave bigger zero there, the algorithm hardly allows flying sideways in front of an obstacle, you actually need to exit the obstacle zone backwards. The value I put there now, allows to back off at a 45deg angle.

[jlecoeur]

Much simpler implementation, for the better.

I made two suggestions

[jlecoeur]

LGTM

comments addressed

[bresch]

@baumanta What's still pending now? Can we merge it?

[baumanta]

@bresch from my side this is ready to merge

[RomanBapst]

@baumanta Could you try to explain this a bit better? Also, wouldn't it be better to define it in degrees here?

[RomanBapst]

@baumanta Just a minor comment. Btw, is the parameter you added there already in the documentation? @hamishwillee FYI

[hamishwillee]

@baumanta Do we need to add MPC_COL_PREV_ANG to http://docs.px4.io/master/en/computer_vision/collision_prevention.html#px4-software-setup ?

Note

• it is not entirely clear to me from the description what this does. I GUESS that, in position mode, you might be commanding to go forward (say). You might be getting data from sensors at some angle offset to forward which you can use to calculate the range (using trig). What I think you're saying is that if the orientation of the sensor is more than MPC_COL_PREV_ANG you will ignore that data for the calculation. Is that bout right?
• It is not clear what you should reasonably set this as? Can you provide some guidance/real use cases?
• Do you want to take first shot at adding above docs?

[baumanta]

@hamishwillee, yes I think it might be a good idea to put the there just below the MPC_COL_PREV_D. I don't really know how to describe the function of it best. That's why I had it hardcoded in the beginning. But essentially this angle determines which sensor data is used for the calculation of the maximum speed. The value needs to be between 0- 90 degrees. If you set it to 90deg, the whole hemisphere of sensor values centered around the commanded direction is taken into account. This means that if all is blocked, go can only move straight back. If you lower the parameter value e.g. to 45deg (which is what I put as default) you can still move sideways along the obstacle. So the effect of this parameter is basically the "exit angle" or at which angle you can move away from obstacles. If you put it too low, you might collide because you are not using enough data and you are too susceptible to noise.

I'm not sure how to formulate that best, but does it make sense to you?

[hamishwillee]

Sorry, it may be that I'm a bit thick, but I don't get it at all. Not even a little bit.

Firstly, let me confirm what you are actually saying. What I think you are saying is that if I approach an infinite wall head on, and the MPC_COL_PREV_D is 45, then I can't slide - I just stick ? To move I have to use a setpoint to that is behind me - presumably at at least 45 degrees from parallel to the wall? If I use 10 then I can use a setpoint at 10 degrees from parallel to the wall, but if I use 90, then I have to move directly backwards.
Is that even close? If so, what if I come at 45degree angle to the wall?

In the "old way" I understand that as you approached a surface the vehicle would accelerate to cancel out the velocity components of your motion perpendicular to the surface in time to stop at the desired distance. You kept the parallel components of velocity, so you would stop if heading straight in, but on a slight angle you would slide along the surface.

Perhaps it might help to understand how things are different than that now? Perhaps a diagram showing this too for a couple of different PREV_D angle and approach angles, showing the PREV_D and the exit angle in each case.

Basically I can't "see" what the algorithm is doing.

[jkflying]

The issue was that before the sliding could potentially send you in an 'uncommanded' direction, which is a neat gimmick but actually very non-intuitive when you're trying to actually do a specific task (eg filming) and not just demo the collision prevention. It would also lead to strange wobbles if the vehicle approaches an obstacle with an uneven surface.

So now, the drone will only attempt to move in the direction you command it. If there is something in the way, then it will slow down and stop at the minimum distance. This new MPC_COL_PREV_ANG, say it is set to 45 (default), only takes into account obstacle information within +-45 degrees of the direction you are trying to go, so you don't clip things on the side. Theoretically in order to guarantee you can't get closer to obstacles you should have MPC_COL_PREV_ANG set to 90 degrees, but flying like that close to obstacles constantly feels like you are getting stuck.

[hamishwillee]

Thanks @jkflying, I think perhaps I am starting to "get" this.
So basically you're saying that you're going to hit an object at any angle then you will stop rather than slide - cancelling all velocity components?

Imagine have the angle set at 45 degrees, that is a spread out beam like a torch in front of you. If you rotate the vehicle so you're sensor is parallel to it, the beam still touches the wall, so you can't move. If you keep rotating, eventually the beam no longer touches the wall, so you can head away?
With a wider beam you have to point further away to stop touching the wall with the beam.

And of course this is a copter, so while I'm talking about "turning" actually I mean just that your planned direction of movement (setpoint) is such that the angle of sensor data you are collecting doesn't touch the wall.

Is that what you mean?

[baumanta]

What julian said. So MPC_COL_PREV_D still is the exact same parameter as before, it defines how close you can get to obstacles (in absolute distance, e.g. keep 5m to anything). If you command a setpoint, the drone now only has the following options: execute as commanded, execute at smaller velocity, stay where you are. It cannot go any other direction than the one commanded. Before it was able to change direction slightly and "drift around" stuff. The new parameter MPC_COL_PREV_ANG influences whether and how much the velocity is limited. Depending on how much of the data you consider in the calculation you might be able to move certain directions or not. Your example with the infinite will is correct.

[baumanta]

[jkflying]

To add, the beam analogy works pretty well. And critically, it points in the direction the sticks command, not where the sensors happen to be. If it sees something (ie. the beam overlaps with sensor data), it will constrain the speed if necessary to keep the minimum distance specified in MPC_COL_PREV_D.

[hamishwillee]

Thanks guys. So @baumanta in your diagrams the blue lines are the MPC_COL_PREV_ANG around the commanded direction. In the first diagram the vehicle can't obey the command because the sensor data is collecting everything in that right right side of the circle - in front there is at least some obstacle sensor data being collected that is in a range where we have to slow down - so we're stopped even though looking at the image it looks like you should be able to move right because there is no obstacle to the right.
In the second diagram the sensor data collected is only 45 degrees. In this case we might be picking up the wall, but at a range of more like 6m, so we can move in the commanded direction (albeit not too fast, because we're detecting "something close".

Is that about it?

[baumanta]

@hamishwillee yes, you got that perfectly right. The maximum speed allowed is calculated from the distance of all the bins in the MPC_COL_PREV_ANG (blue) range. In the left case there is a bin at 4m, which limits the velocity to zero, because we do not ever want to go closer than that. In the right example we only have the closest bin at 6m, which allows just enough forward speed that we would be able to stop again in 2m.

[junwoo091400]

@baumanta This case will always be true, is this here to prevent anomaly the case when the logic above (Line 218 ~ 219) changes?

[baumanta]

the range scan contains values 360 degrees. Depending on the FOV and orientation of your sensor different bins will contain valid data. This statement checks whether the scalar product between a valid bin and your desired direction of movement is positive. This would return false for example if you ask the drone to move backwards but the bin you are currently observing lies in front of it. In this case this bin is irrelevant for the movement you requested and will not be used to limit your speed.