First of all, in the current version, it is not possible to guarantee that the underlying optimizer
chooses forward solutions only.
The configuration is much easier for differential-drive robots than for carlike robots.
Some important notes for selecting a penalty for backward motions (parameter weight_kinematics_forward_drive:
local minima (sensitive w.r.t. initialization): The initial trajectory (especially its orientation part) is very important since our optimizer seeks for the next local minimum. In the case of a differential drive robot, the robot can always turn in place which can be easily handled during optimization. For a car-like robot, the trajectory usually consists of backward and forwards arc segments which introduce further local minima.
feasibility: according to your radius of 0.5 m, I assume that you have a carlike robot. The feasible set of possible configurations is significantly reduced if you prohibit backward motions (especially in cluttered environments).
You might check Reeds and Chepp curves to get an idea.
However, this does not solve your problem at all. Sometimes it is possible to bypass this behavior, e.g. we could rotate the robot towards the goal before starting the actual trajectory planning. But I prefer to keep the number of additional rules on top of the optimization low if possible. Or we can implement different initialization strategies.
If you have possible solutions or suggestions in mind, I would be happy to discuss them with in our issue tracker.
Some side notes: (hard) constraints are currently treated as penalties and giving their corresponding terms appropriate weights might enforce them (extremely large weights can cause slower convergence due to a bad matrix condition).
However, since the optimization problem size is medium, penalties should be sufficient (maybe we can change the solver backend to some fast sqp or interior point solver someday to handle hard constraints).
