Support interpolation of waypoints for Omnidirectional and Ackermann steering

[Yadunund]

• Extend VehicleTraits to support ackermann steered vehicles. Update Holonomic class.
• Update Interpolate::positions()

[luca-della-vedova]

One of the simplest approaches can be using Dubin curves, they basically deconstruct navigation between two arbitrary waypoints in a set of three of any of the following actions:

• Turn left at minimum turning radius
• Turn right at minimum turning radius
• Go straight at maximum speed

However, while they are simple and easy to implement @codebot mentioned that a more realistic model would apply a constant angular acceleration to simulate a steering wheel being turned at constant speed, since a Dubin curve involves every turn being at minimum turning radius that would either create the risk of tipping over vehicles or force them to slow down a lot even for wide bends.

Thus I think we could have the planner model a vehicle through the following parameters:

• Maximum linear velocity / acceleration
• Minimum turning radius
• Maximum angular acceleration

Then deconstruct the trajectory into a series of bends / straight parts following Dubin curves that, instead of being all at minimum turning radius, are at constant angular acceleration. We should probably apply a scaling to the linear speed to make it inversely proportional to the angular speed so that vehicles will slow down as they reduce the bending radius but still take wide bends at relatively high speed.

Afterwards it's a question of what the waypoints information coming from the planner should look like.
I think it could be useful if the planner output two new fields for each waypoint:

• Target linear velocity
• Turning direction (left / right / straight).

The most accurate (but probably most complex) bend model would split each turn into three points and probably look something like:

• Entrance: with a target velocity to enter the bend at (to make sure that we execute it as fast as possible) and the direction (i.e. R). After this point the vehicle will start decelerating at -maximum_linear_acceleration (or other acceleration value) and turning right at maximum_angular_acceleration.
• Mid point, where we start to exit the turn, hence apply maximum_angular_acceleration in the other direction and maximum_linear_acceleration (or other acceleration value)
• Exit, where we plan to be having an angular velocity of zero and a target linear velocity.

Where I say other acceleration value I mean that probably we don't want vehicles to break to their maximum capability in the middle of a bend since it is a common cause of loss of control. We could just output a target velocity at the entrance, middle and end of the bend and do linear interpolation through a constant linear acceleration model with deceleration when entering and acceleration while exiting the bend to calculate the speed throughout.

This modelling implies an infinite jerk trajectory since we are changing both angular and linear acceleration instantly at the three points of the bend, if we want continuity at the acceleration level we can have a maximum_linear_jerk and maximum_angular_jerk parameter but imho that might be unnecessarily complex.

Edit: Note that Dubin curves only work for forward moving vehicles but there is an extension for reversible vehicles through Reeds Shepp curves.

[ddengster]

Here's a video detailing the curves plus more manoeuvers like reversing, and how to plan for those.

This begs the question as to how much rmf_traffic is going to handle with regards to the planning for such vehicles. For example, the ackermann steering plugin right now, given 3 waypoints that make up a bend for a right/left turn, breaks it down to simple SCS manoeuvers. Should this act of breaking down waypoints into SCS manoeuvers be done in rmf_traffic?

Simulation-wise, we'll have to look at the world and see what kind of manoeuvers we want to show off. For now I'm looking at getting vehicles to orientate correctly for their first waypoint.

Tagging in @mxgrey

[mxgrey]

how much rmf_traffic is going to handle with regards to the planning for such vehicles.

My expectation/hope is that we can provide a "close enough" representation of these motions using the existing piecewise cubic spline format that rmf_traffic::Trajectory is already based on. The first step would be creating some utilities for generating piecewise cubic splines that reasonably approximate the Reeds-Shepp curves (ideally with some parameter for error threshold so users can decide how they want to trade-off between accuracy and waypoint density).

After we can generate the right kinds of trajectories, the next step would be supporting it in the planner. I'm thinking we can define a pure abstract interface for an "Interpolator" that can be used by the Planner class. The interface for Interpolator would be something (very roughly) like

class Interpolator
{
public:

  struct Branch
  {
    std::size_t arrival_waypoint;
    std::vector<rmf_traffic::Route> route;
  };

  virtual std::vector<Branch> branch_from(
    const rmf_traffic::agv::Graph& graph,
    std::size_t waypoint_index) const = 0;
};

Essentially the job of Interpolator would be to consider a waypoint on a navigation graph and tell the planner what the different options are for an agent that starts at that point. We can create an implementation of this interface for the existing straight-line steering type and a separate implementation for the new Reeds-Shepp steering type. Users would provide one implementation or the other to set what kind of steering the want. In the future other implementations of the interface could be developed to support yet more types of steering (e.g. holonomic omnidirectional).

[ddengster]

My expectation/hope is that we can provide a "close enough" representation of these motions using the existing piecewise cubic spline format that rmf_traffic::Trajectory is already based on. The first step would be creating some utilities for generating piecewise cubic splines that reasonably approximate the Reeds-Shepp curves (ideally with some parameter for error threshold so users can decide how they want to trade-off between accuracy and waypoint density).

Update: I've setup a prototype in this repo open-rmf/rmf_geometry_testbed#1 approximating the circular arc into rmf::Trajectory using the midpoint of the circular arc and constructing a b-spline. I should be able to put features to reduce the amount of error sometime in the future.

Essentially the job of Interpolator would be to consider a waypoint on a navigation graph and tell the planner what the different options are for an agent that starts at that point.

To add on, we should consider the lane type as well (bidirectional or one-way) because there are are some maneuvers that require reversing. For example, here's a common setup in our projects.

If a nonholonomic vehicle is on AAAAA and wants to go to BBBBB, it cannot turn on the spot like a holonomic/omnidirection robot does and the planner will have to account for that. On a bidirectional lane we can reverse and make the turn, giving us a L|CL reeds-shepp motion.

[ddengster]

Here's another type of motion that I think we could support: a waypoint with a target yaw that is different from the nonholonomic vehicle's current heading. The picture below shows what the motion could be. Dark cyan arrows represent the starting/desired positions and headings.

[mxgrey]

Here's another type of motion that I think we could support: a waypoint with a target yaw that is different from the nonholonomic vehicle's current heading

Right, the job of the interpolator will be to return any number of valid trajectories that can start from the given initial (waypoint, orientation) state. Some of those trajectories might have a final orientation that is the same as the initial orientation, but that's not a requirement; the interpolator should return all valid trajectories, including ones where the robot has turned.

As I think more on this, there could be some combinatorial challenges since there may be closed loops or large detours that would offer valid trajectories but may lead to infinitely many outputs or just very unhelpful outputs. We may also need to give the interpolator some kind of filter where we can tell it "Don't bother trying to reach this subset of waypoints" or "Don't revisit waypoints that were already reached on your current trajectory".

It will certainly be an interesting challenge to nail down these details.