search

meco-group / omg-tools

Optimal Motion Generation-tools: motion planning made easy
GNU Lesser General Public License v3.0
565 stars 98 forks source link

Obstacle avoidance with 3D holonomic vehicle not working #74

Closed RianBeck closed 5 years ago

RianBeck commented 5 years ago

Omg-tools does calculate a path, but this is by far not the optimal path around the obstacle. When setting the soft constraint around the vehicle to zero it still calculates a path around the obstacle with a large distance margin. It even ignores the vehicle radius to do so, thereby drawing a path that partly lies on the edge of the room, which is infeasible in practice. This behavior can be simulated in the code snipped attached below.

https://github.com/BosMathias/BosRepo/blob/58f5c3fde6026b9f0ce104a15cebb7da2fa4ed09/p2p_holonomic_3d.py#L20-L57

rubenvanparys commented 5 years ago

Hey Rian, What you observe is a typical behavior of a holonomic system. As its x,y,z motion components are independent, the most constrained component will determine the 'speed' of the motion, while the other components 'can do whatever they want'. In the example, vx is constrained at its bound, but the motion in y,z can have different solutions without increasing the objective. So strictly speaking, you observe an optimal path, but it looks silly. By constraining the total velocity norm in stead of its different components, this behavior is avoided. You can do this by adding the line vehicle.set_options({'syslimit': 'norm_2'})

RianBeck commented 5 years ago

Awesome Ruben, that indeed solves it! Thanks!