Articulated motion model support in lattice generator

[ravi-kumaaar]

Feature request

Feature description

Adding support to generate motion primitives for articulated vehicle for the state lattice planner.

Implementation considerations

I've prepared a draft version of the implementation and incorporated the changes after pulling from the main branch as of March 14th.

The draft includes the following:

• Updated config.
• Added trajectory constraints for AV.
• Created new functions to compute articulated path.
• Added vehicle kinematics for AV.
• Precomputes footprint.
• Visualization of primitives overlayed with footprint.

Pros:

1. More variety of vehicles with articulated hinge/pivot mechanism have the possibility to drive autonomous.
2. Precomputing the footprints in the primitives could reduce the computational time to create a path in the state lattice planner.

Cons:

1. When generating path with an arc, It assumes the rear pose of the vehicle based on the goal pose to create a smooth path. However, in real scenarios the rear pose can be in different states limited to its maximum turning angle.

My thoughts are that It might be required to have subsamples of primitives based on the possible start poses of the vehicle. Resulting in creating a additional angle bin to check through when generating a online path.

2. When working with big vehicles with minimum turning radius it takes a long time generating and the primitives are relative close to each other. If i try to generate with grid_res = 0.05m and r=4.6m It takes multiple hours.

When checking for overlapping primitives the threshold is 10% of the grid resolution, I think to include some relationship with the turning radius could be useful to reduce the overlapping primitives.

Appreciate any thoughts on these matters.                            

[SteveMacenski]

When generating path with an arc, It assumes the rear pose of the vehicle based on the goal pose to create a smooth path. However, in real scenarios the rear pose can be in different states limited to its maximum turning angle.

I think that the lattice needs to be setup to be multidimensional: for each pose of the front part of the articulation, having each possible angle of the back part of the vehicle's state. That way as you're planning, you can propagate that. So the states aren't just orientation bins of the vehicle front like in non-articulating vehicles, but also for each of those the possible back-section's angular states. Thus, if you have 16 angular bins, for each one you may have ~8 primitives based on ~8 states of the rear car that is possible at that front-car angular bin (~8 depending on turning radius, self collision, etc of possible states). The front and rear car's angles are baked into the full state so that each time we make a move, both angles are accounted for.

So essentially, for non-articulated, we may have 16 angular states (for example), but for articulated, we may have 16 * 8 = 128 states.

If i try to generate with grid_res = 0.05m and r=4.6m It takes multiple hours.

Have an actual time? I would expect it to take a few minutes, but surprised by hours. There may be ways to optimize this and perhaps there are things this is doing wrong or too greedily optimizing for. For example, there's alot of symmetry that can be leveraged relative to left-turn and right-turn that can just be mirrored without computation. It also may be that your termination criteria is too strict or something. If nothing else, we know that some of these things can be computed independently so it could be multi-threaded which would speed it up by multiples.

Have a visual for the lattices that this generates?

[ravi-kumaaar]

So essentially, for non-articulated, we may have 16 angular states (for example), but for articulated, we may have 16 * 8 = 128 states.

I agree, It should be setup with multiple configured states.

The figures below is to help clarify my thought process. The left figure is the 16 angular bin, and the rest is an example of iteration through the headings in 1.quadrant of the angular bin. We only need to consider the 1.quadrant since it flips the primitives in the other quadrants. Note: The arrows of the rear states are pointed in the opposite way for visual purposes.

For starters, I think the initial heading of the front should also be consider in one of the states of the rear headings. Or else it would not be able to start a maneuver from a steady state. Next we know we are limited by the maximum pivot angle (red arrows), which in these figures are approximate set to -40/40 degrees. Then fill up the gap between them adding four more states, resulting in 7 rear headings per angular bin (112 states). I tried to think of other ways to have 8 rear states, but then we would have to discard one of the rear state that includes the initial heading.

Have a visual for the lattices that this generates?

For reference I created a new workspace with the latest nav2-humle repo. Entered parameters: Ackermann, 4.6m turning radius, grid_res = 0.05m. It took 6 h 33m 36s to generate 1592 primitives. The same results came from the draft I made since generating the minimum control set for ackermann has not been tampered with.

I think that the primitives get denser as the turning radius becomes larger, which is a result of big vehicles operating in a grid of 5cm res.

When generating in articulated mode and with these parameters:

• turning radius = 4.6m
• grid res = 0.05m
• max pivot angle = 40 deg
• max pivot velocity = 20deg/s
• max linear velocity = 1.0m/s

It took 153s to generate 368 primitives.

I increased the grid res to 0.2 in this figure, and it generated 88 primitives in 3.5s.

Here is a video going through every poses of each primitives generated in the 1.quadrant using the last configurations.
These are also used parameters:

• footprint = [[-3.7, -1.3], [0, -1.3], [5.3, -1.3], [5.3, 1.3], [0, 1.3], [-3.7, 1.3]]
• length_from_pivot_to_front = 1.6m
• length_from_pivot_to_rear = 1.6m
  Screencast from 31. mars 2024 kl. 21.10 +0200.webm

It occur to me that it took me hours to generate primitives before I added more trajectory constraints in articulated mode, so it is currently not an issue. However, the density of the primitives tells me something is not as it should be. This might be resolved by including more of the rear states into consideration, which then introduces more strict constraints. Thoughts?

[SteveMacenski]

It took 6 h 33m 36s to generate 1592 primitives.

That's not great, which we both know, but also really not that bad either. Especially if we consider the fact that the initial states can be multithreaded (i.e. if you start at each of the rear car angles possible, that makes N different lattice calculations that are completely independent). If you spin those up as new threads, that should drop it by Nx approximately in time, which should be under an hour. I assume something is wrong here, but honestly if we came out that everything was right and it really just does take a long time for articulated vehicles, we can still get this down to sub-1-hour easily and it just becomes something that crunches over a lunch break.

That also looks incredibly, incredibly dense, but I'm also not 100% sure what I'm looking at there. Is that just for 1 particular rear-car initial and final orientation or is that everything superimposed? If just a single orientation of the rear-car, I think you need to have a more liberal rejection criteria.

It took 153s to generate 368 primitives.

That's perfectly fine. The reason we offline calculate and store them is because they can take a bit to compute. If they were instantaneous, we won't bother and just compute them on initialization of the controller. What's the difference between the 153s and the 6+ hour ones? I don't see the listed difference between them. Is the 153s one more practical/real requirements and the 6+ hour one more notional randomly plugging in parameters? I'm sure you can find parameters that don't represent any vehicle which would be particularly hard, so its useful for me to understand what these two runs practically represent.

I increased the grid res to 0.2 in this figure, and it generated 88 primitives in 3.5s.

That is starting to look like a normal lattice with just a single rear-car orientation heading (i.e. I don't see multiple that are getting to a goal pose/heading that are slightly different). If those previous illustrations were also like this in being only a single rear-car orientation, I think your rejection criteria is too strict, many of those are effectively similar. What does our current primitive solver use for rejection criteria? Consider using that?

Screencast from 31. mars 2024 kl. 21.10 +0200.webm

Shouldn't the car's footprint be changing over a motion primitive? If going from straight configuration to a turn, shouldn't we see the footprint start having a "kink"? Or is that happening and its just slow enough in those short videos I don't see it visually?

---

Note that the search grid size of the primitives does not need to be the same as the costmap resolution. We mostly just want that the trajectories that are outputted to be discretized from their continuous state into states that are proportional to costmap resolution for collision checking. The actual search grid used for checking for new trajectories can be really anything as long as the outputted versions have their via-states at ~about costmap resolution.