Closed RianBeck closed 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'})
Awesome Ruben, that indeed solves it! Thanks!
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