-
Notifications
You must be signed in to change notification settings - Fork 10
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Robot is driving a bit backward, if the target is exactly straight forward #36
Comments
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! 🙂 |
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:
In cf48f33 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. |
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.
afterwards I calculate a path with the path_planner:
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:
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
The text was updated successfully, but these errors were encountered: