Robot is driving a bit backward, if the target is exactly straight forward

[AndyPermaRobotics]

Hi there!

I noticed an interesting behavior in my simulation environment. I thinks in reality it will happen rarely, but not never.

My szenario: With a button click, I create a target pose that is exactly n meters in front (x-axis) of the current robots pose.

new_point = current_pose.transform(Point(x=5, y=0))
goal_pose = Pose(x=new_point.x, y=new_point.y, yaw=current_pose.yaw)

afterwards I calculate a path with the path_planner:

path = await path_planner.search(
    start=current_pose, goal=goal_pose, timeout=30
)

What happens is, that the robot is driving a bit backward, before it drives forward and after driving backward, the carrot is constantly behind the hook. This results in driving beyond the goal pose and afterwards driving backward again, to reach the goal_pose.

Some screenshots:

1. Start:

   

2. Drive Backwards:

   

3. Afterwards the carrot is behind the hook, while the robot is driving forward:

   

4. Drivings beyond the goal_pose:

   

5. Drives backward, until the goal_pose is reached:

   

The interesting part: If I shift the goal_pose just a little bit to the side (e.g. 1cm), everything works as expected and the bot drives forward only.

As far as I could debug the behavior, the Delaunay Path Planner says: "INFO:rosys.delaunay_planner:found single spline to reach goal" I printed out the PathSegment that the path_planner returns: PathSegment(spline=Spline(0.000,0.000 ~ -2.500,0.000 ~ 7.500,0.000 ~ 5.000,0.000), backward=True)

I looks like the forward PathSegment "is not healthy".

but I don't know why.

Best regards Andreas

[falkoschindler]

Hi Andreas,

That's an interesting bug! First I thought it's all about the internal workings of the path planner, which approximates the world with a triangular grid and searches a graph-based path from one node to another. Due to the grid resolution, the path might deviate from the theoretical optimum. But as the debug log says, a special case of a single spline from start to goal was found. So the problem might be caused by this single spline.

Here is a minimum reproduction to experiment with:

import logging
import rosys
from nicegui import ui

logging.basicConfig(level=logging.DEBUG)

shape = rosys.geometry.Prism.default_robot_shape()
wheels = rosys.hardware.WheelsSimulation()
robot = rosys.hardware.RobotSimulation([wheels])
odometer = rosys.driving.Odometer(wheels)
driver = rosys.driving.Driver(wheels, odometer)
path_planner = rosys.pathplanning.PathPlanner(shape)

with ui.scene():
    rosys.driving.robot_object(shape, odometer, debug=True)

async def drive_straight():
    start_pose = odometer.prediction
    goal_pose = start_pose.transform_pose(rosys.geometry.Pose(x=1))
    path = await path_planner.search(start=start_pose, goal=goal_pose)
    logging.info(path)
    await driver.drive_path(path)

ui.button('Drive straight', on_click=drive_straight)

ui.run()

And there it is! The path segment is very unexpected:

[PathSegment(spline=Spline(0.000,0.000 ~ -0.500,0.000 ~ 1.500,0.000 ~ 1.000,0.000), backward=True)]

The spline goes backwards from (0,0) via control points (-0.5,0) and (1.5,0) to (1,0). We don't see the robot driving backwards, because the carrot is basically pushed perfectly straight, so the vehicle doesn't start to turn. But as we can see from the data, the path segment is actually supposed to be driven backwards.

So the question is why the single-spline solution is flipped. I'll look into it! 🙂

[falkoschindler]

Oh, now I understand...

At first the path planner is trying out some simple solutions, like simply driving a smooth spline from start to goal - either backwards or forwards. If either of them works, this is the solution.

But there's a subtle problem: First it tries the backward variant. Since there are no obstacles in our simple example, there is no collision. The next criterion is curvature, which needs to be below some limit. Usually the backward spline should be filtered out because of an infinite curvature. But if start, goal and control points all lie on a straight line, the formula for the maximum curvature seems to break and yields 0. So the curvature criterion is fulfilled and the path planner simply returns this degenerate spline.

Here is an experiment demonstrating the maximum curvature for increasingly straight splines:

for dy in [1, 0.1, 0.01, 0.001, 0]:
    start = rosys.geometry.Pose(x=0, y=0, yaw=0)
    goal = rosys.geometry.Pose(x=1, y=dy, yaw=0)
    spline = rosys.geometry.Spline.from_poses(start, goal, backward=True)
    print(dy, spline.max_curvature())

Result:

1 36.107260595463906
0.1 -3231.6667866986413
0.01 -322752.795930885
0.001 32274865.402173076
0 -0.0

In https://github.com/zauberzeug/rosys/commit/cf48f334eb67aa9111ce8e4c1463ccb2ce93a73e I improved the procedure to always check both variants, forward and backward, and pick the shortest spline. This seems to resolve this issue, because the degenerate spline is usually longer.