Cornering with motion primitives

[Wimll]

Hi there!

Thanks for all the effort on these packages, it makes life much easier when developing a global planner for my robot!

I am currently using the SBPL lattice planner together with the TEB local planner to steer my robot. I am running into problems when cornering using my motion primitives. I use short feasible motions to steer the robot (bakcwards, forwards, forwards left corner and forwards right corner).

However, my control point is the front axle of the tricycle. Since only the front wheel is steered and the back wheels follow this trajectory, the planned path can cause the robot to collide with obstacles when cornering (i.e. the back wheels follow a smaller radius than the planned path for the front wheel).

To my knowledge, these holonomic constraints cannot be specified in the motion primitives, is this correct?
The only way to deal with this behavior is then to increase the inflation radius of the global costmap and tune this together with the cost scaling factor to make sure the back of the robot does not collide with obstacles when cornering.

Is this correct, or am I missing something w.r.t. the motion primitives of such a non-holonomic tricycle robot?

Thanks!

[mintar]

No, you can definitely use any motion model you want.

The input to SBPL is just a *.mprim file. For example, have a look at unicycle_highcost_5cm.mprim.

Here is a visualization of this file (all starting angles overlaid):

That file was created using the genmprim_unicycle_highcost_5cm.py Python script, which was converted from the genmprim_unicycle_highcost_5cm.m Matlab script.

The Python/Matlab scripts use the unicycle motion model to generate the motion primitives. However, there is nothing preventing you to modify those scripts to employ the tricycle motion model instead. The only thing that you need to ensure is that the generated motion primitives meet the following conditions:

1. SBPL works on a discretized grid. Its resolution has to match the grid size of the map (5cm in the example). Each primitive has to start and end on a grid cell.
2. SBPL works with a discretized set of angles (16 angles in the example, so 22.5° each). Each primitive must start and end on one of these discretized angles.

These conditions are only required for the start and end poses of each primitive; for the intermediate poses, anything is allowed.

Here's an extract from the figure above, showing only the motion primitives for starting angle 1 (=22.5°). There are 7 motion primitives shown (some a bit hard to see), with the resulting angle shown as a number at the end pose:

1. slow forward (dark blue)
2. fast forward (green)
3. slow backward (red)
4. forward turn left (light blue)
5. forward turn right (purple)
6. turn in place clockwise (black)
7. turn in place counterclockwise (black)

The dots are the intermediate poses (10 for each primitive).

You can verify for yourself that the conditions (1) and (2) hold for these primitives.

So the next step for you would be to edit the Python/Matlab script above and replace the unicycle model with a tricycle/simple car/Reeds-Shepp car/Dubins car motion model, whatever fits your purpose best.

[mintar]

Oh, and of course you should also think carefully about where you place the body frame of your robot that SBPL uses for planning. I believe the convention is to place it in the center of the rear axle, not the front axle.

[Wimll]

Hi mintar,

Thanks for your elaborate answer, this helps a lot! I tried incorporating the tricycle model, but I have a hard time translating the model to actual motion primitives of the robot (dx,dy,dtheta,cost). But I will look into this again.

My robot is steered by the TEB local planner from the front wheel, but then the global plan will be planned from the back wheels using the SBPL lattice planner. I suspect that this gives some problems when the local planner tries to follow the path.

[mintar]

My robot is steered by the TEB local planner from the front wheel, but then the global plan will be planned from the back wheels using the SBPL lattice planner. I suspect that this gives some problems when the local planner tries to follow the path.

Both local and global planners must use the same frame, and it should be the center of the rear axis. This probably causes 95% of your problems; you could probably get away with the unicycle model if you change both frames to the rear axis.

[Wimll]

My robot is steered by the TEB local planner from the front wheel, but then the global plan will be planned from the back wheels using the SBPL lattice planner. I suspect that this gives some problems when the local planner tries to follow the path.

Both local and global planners must use the same frame, and it should be the center of the rear axis. This probably causes 95% of your problems; you could probably get away with the unicycle model if you change both frames to the rear axis.

Thanks mintar! Is it just common practice to use the center of the rear wheels to steer the robot? Or why is this preferred over any other frame? I will try to incorporate this in my current setup!

Furtermore, when using the centre of the back wheels for both planners, how do I prevent the front of the robot hitting a wall when cornering? Should I enable the inflation layer for the local planner to prevent this?

[mintar]

Is it just common practice to use the center of the rear wheels to steer the robot? Or why is this preferred over any other frame?

I think this is because in a car-like robot, it is easiest to express the motion model this way.

Imagine a tricycle that is performing an in-place rotation (steering angle = 90°, so the front axle is perpendicular to the rear axle). A configuration can be expressed as (x, y, theta). In this example, the tricycle rotates around the center of the rear axle. If you pick the mounting point of the front wheel as the body frame, an in-place rotation is actually a circle, so x, y and theta change. If you pick the center of the rear axle as the body frame, x and y remain constant and only theta changes. See this link and the rest of that chapter for details.

TL;DR: It's possible to describe the motion model equations using any body frame other than the center of the rear axle, but it gets very complicated.

Furthermore, when using the center of the back wheels for both planners, how do I prevent the front of the robot from hitting a wall when cornering? Should I enable the inflation layer for the local planner to prevent this?

You always need an inflation layer, no matter which body frame you pick. All local and global planners (including TEB and SBPL) perform a footprint check to ensure that the robot doesn't collide with the environment. See #33 (comment) and #33 (comment) for information how to properly configure the inflation layer.

I believe if you set the body frame to the center of the rear axle and configure the footprint and inflation layer properly, all your problems will go away, and you don't need to implement the tricycle motion model. Especially this quote is encouraging:

Note that the unicycle can also simulate the simple-car model. Therefore, the tricycle and unicycle models are similar.

(from http://planning.cs.uiuc.edu/node660.html)

[Wimll]

TL;DR: It's possible to describe the motion model equations using any body frame other than the center of the rear axle, but it gets very complicated.

This actually makes sense, it was just because the ROS stack was configured this way that I got confused. I will make the appropriate changes to negate this and plan from the centre of the back wheels.

You always need an inflation layer, no matter which body frame you pick. All local and global planners (including TEB and SBPL) perform a footprint check to ensure that the robot doesn't collide with the environment. See #33 (comment) and #33 (comment) for information how to properly configure the inflation layer.

Thanks, I already saw these comments by reading through the previous issues. I will incorperate this and properly tune my costmaps again with the approriate footprint in the TEB local planner.

I believe if you set the body frame to the center of the rear axle and configure the footprint and inflation layer properly, all your problems will go away, and you don't need to implement the tricycle motion model. Especially this quote is encouraging:

Thanks, this makes sense! Thank you very much for these insightfull thoughts (:

[Wimll]

Hi mintar,

I derived the appropriate kinematic model for my robot. However, I have a hard time understanding how these translate to the motion primitives.

For instance, for the unicycle model we have these kinematics:

ẋ = u_s * cos(θ)
ẏ = u_s * sin(θ)
θ_dot = u_w

These are according to http://planning.cs.uiuc.edu/node660.html

If I understand correctly, the motion primitives of genmprim_unicycle_highcost_5cm.m with heading angle of 0 degrees w.r.t. the x axis can then be derived in the following manner:

• Set u_s to any value (for instance, 8 in the script)
• This sets x = 8 * cos(0) = 8 (40 cm move with 0.05m/pix resolution) and y = 8 * sin(0) = 0
• u_w can be arbitrarly chosen, for a straight move we keep it at 0

We then do this for a heading angle of 0, 22.5 and 45 degrees for instance:

x = 8 * cos(22.5) = 7.4
y = 8 * sin(22.5) = 3.06
θ = 0 (because we move in a straight line)

And we just round these numbers to 7,3 and 0? Is this correct? Because in the genmprim_unicycle_highcost_5cm.m file this is configured as (6,3,0)

Or are these just arbitrarily chosen to get a straight path at 22.5 degrees while keeping the resolution into account?

Furthermore, what happens if we do a forward move with turning?
The change in θ is just 1/16 of the maximal steering angle of the robot?

I tried looking into the tutorials as to how the motion primitives are calculated, but I still have my doubts, that is why I reopened this issue.

The tricycle model would then only affect the heading angle equation:

θ_dot = (u_w*sin(rho))/L

Where rho is the steering angle and L the wheelbase of the robot. I could simply use these to calculate the proper theta change for all motion primitives?

Thanks!