Intent to implement: simple car paths

[jwnimmer-tri]

I intend to add simple car path and trajectory scripting support in C++ for something at the level of the drake/drake/examples/SimpleCar current example. (This may be a step along the road to #1764, though it's not entirely clear if the actor tag will be the end-all and be-all.)

Unlike some other trajectory-type code in matlab or otherwise, I posit that the passage of time t should only be loosely coupled into the System model. Yes, for a given car velocity, the car should move at the appropriate rate -- but, the time when the car starts accelerating, slows down, or holds constant is more likely dictated by the position along a path (slowing down before turning), rather than the current absolute simulation time. Thus, most state is phrased in terms of distance_along_path and not directly dependent on time.

I am considering creating dynamical systems that look like this:

CarPath (System) /// A mechanism to lay out a path, based on distance along it. /// The contents of the path are populated at construction time.

• input u = { distance_along_path }
• state x = none
• dynamics xdot = none
• output y = { position_x, position_y, heading, curvature }

CarProgress (System) /// Integrate velocity into path distance.

• input u = { velocity }
• state x = { distance_along_path }
• dynamics xdot = { velocity }
• output y = { distance_along_path, velocity }

Those two should be enough to get basic scripting working (just feed in a constant, non-zero velocity), and then also support some basic velocity-control inputs like in SimpleCar with throttle and brake.

To specify a velocity profile for a CarPath (e.g., scripting out the typical or maximum speed for a car following that path), we could then later add:

CarVelocityProfile (System) /// A mechanism to overlay a velocity profile on a path. /// The velocity at a given path distance is determined at construction time.

• input u = { distance_along_path }
• state x = none
• dynamics xdot = none
• output y = { velocity, acceleration }

Other velocity profiles could be created later that model reactionary car behaviours, such as (adaptive) cruise control.

By choosing a t0 where the path-following begins, a hybrid system could transition to starting the path at a pre-determined moment. Alternatively, path-begin transitions could have a more complicated start condition based on other factors (e.g., stop-sign negotiation).

If anyone has comments / critique of this, I'm happy to hear it.

[jwnimmer-tri]

I'm specifically looking for input from @RussTedrake @maddog-tri. Optionally input from @bmartin-tri @rpoyner-tri also. (Anyone in the wider community can comment of course, too. That's why it's up here on github, instead of a private list.)

[liangfok]

@jwnimmer-tri, can you make this a Google Doc? That would allow us to add in-line comments.

[liangfok]

The word "dynamics" is a loaded word in robotics -- it typically refers to forces / torques and thus accelerations. Thus, labeling a velocity "dynamics" could potentially be confusing to a roboticist. Instead of dynamics xdot, I would call it xdot (velocity) or velocity xdot or just xdot or something like that.

[liangfok]

Unlike some other trajectory-type code in matlab or otherwise, I posit that the passage of time t should only be loosely coupled into the System model. Yes, for a given car velocity, the car should move at the appropriate rate -- but, the time when the car starts accelerating, slows down, or holds constant is more likely dictated by the position along a path (slowing down before turning), rather than the current absolute simulation time. Thus, most state is phrased in terms of distance_along_path and not directly dependent on time.

The above statement is generally true but I don't think it should be true for the "simple" car path level of sophistication. The only situation when the passage of time is not tightly coupled with the system model is if the model can explicitly adapt to randomness in the simulator. For example, if the car unexpectedly encounters an obstacle and has the option to slow down to avoid collision with the obstacle, then its path progression would indeed be loosely coupled with time. If, however, the car is "dumb" and its path 100% scripted, it should not adapt to a potential collision and thus should plow forward. In such a simulation, the car's trajectory will be perfectly correlated with the passage of time.

Until we're ready to do fancier simulations where both sides of a potential collision are willing / able to adapt, I suggest making the "simple" car path not perform any adaptation and relegate all adaptation to the autonomous / semi-autnomous vehicle.

[liangfok]

output y = { position_x, position_y, heading, curvature }

What is the difference between "heading" and "curvature"?

[liangfok]

I think the data structures should be:

Simple car path specification:

• sequence of triplets: [time, x position, y position]
• spline parameters

Simulation input:

• current time t

Simulation output:

• [x position, y position, xdot velocity, ydot velocity] at time t

[liangfok]

To add first-order adaptive behavior where the path is 100% fixed but the location along the path at a particular time is not, we would simply remove time from the path specification:

1st order adaptive car path specification:

• sequence of 2-tuples: [x position, y position]
• max acceleration / deceleration
• spline parameters

We would then allow the planner logic determine the exact position / velocity of the car over time.

To achieve 2nd order adaptive behavior where neither time or position is fixed, add a "max distance divergence term" to the path specification:

2nd order adaptive car path specification:

• sequence of 4-tuples: [x position, y position, max x divergence, max y divergence]
• max acceleration / deceleration
• spline parameters

The above would provide a funnel in which the car must reside, but the exact position / velocity within the funnel is determined by a planner.

[amcastro-tri]

Notice that the CarProgress system is the typical Symulink integrator block. I would therefore call it just that, an Integrator (1/s) so that it can also be used in more general applications.

The CarVelocityProfile system is a vector valued function with a scalar (the distance) as an input. Mathematically it is an object from R (distance) to R^2 (velocity, acceleration) (the reals to the two dimensional space). Therefore I would suggest making a general system called something like VectorValuedFunction that can also be used in other applications.

I love the CarPath system, I think it does what is appropriate.

[liangfok]

Another question: What is the Car system? Is it dynamics (mass, force, acceleration) based or kinematics (position, velocity, max acceleration) based? For this initial "simple" car path design, my assumption is the car system is kinematics based. Is this correct?

[jwnimmer-tri]

Short responses first. Longer to follow later.

The word "dynamics" is a loaded word in robotics...

It is the Drake System API. See also Appendix D.

What is the difference between "heading" and "curvature"?

By "heading" I mean the direction a car is facing. By "curvature" I mean Signed curvature of the Path.

What is the Car system? Is it dynamics (mass, force, acceleration) based or kinematics (position, velocity, max acceleration) based?

The car system is totally and only what is shown above. The CarPath will be cascaded into visualization without any other System. Take a look at simple_car_demo.cc:93. I guess that means kinematics.

[bmartin-tri]

I agree with the proposed design.

@liangfok I don't think anyone is proposing any intelligence for the NPC cars at this time. Although you are correct that analytically there is no effective difference between expressing things as a function of time vs a function of distance since distance is a straightforward function of time, I think @jwnimmer-tri is asserting that practically speaking, these systems will be easier to implement when expressed as a function of distance, and be less prone to integration error throwing off the absolute position of the car, and I agree.

+1 to @amcastro-tri 's suggestion to rename CarProgress to just a generic integrator. Disagree however about trying to express CarVelocityProfile as some generic vector function system. Having seen Jaybridge's implementations of VelocityProfile and how complicated they can be, I think that deserves to be its own thing, and the code to be shared by factoring out a generic R->R^2 function system seems to be minimal at best.

[liangfok]

These systems will be easier to implement when expressed as a function of distance, and be less prone to integration error throwing off the absolute position of the car, and I agree.

@bmartin-tri: Can you clarify the above statement? I don't see how expressing a time-based trajectory will result in higher absolute position error when integrating over time since at every time step, the absolute position is re-computed based on the time-based position trajectory, not from its past position. If anything, I would assert that computing the next position based on the present position will result in greater error w.r.t the ideal position trajectory.

[liangfok]

FTR, I'm always in favor of going away from time as the underlying independent variable. In my previous research group, we did this when we switched to phase-space locomotion planners. I just want to make sure we are doing so for the right reasons.

Doing this, however, does indeed make it less predictable of where the robot's going to be at a particular point in time. We accepted that trade-off since doing things in real-time using general purposes PCs is not easy. In fact, we claimed that doing so makes the system more "robust" against timing errors like estimating when the foot will actually hit the ground.

[liangfok]

@jwnimmer-tri:

The word "dynamics" is a loaded word in robotics...

It is the Drake System API. See also Appendix D.

Drake fundamentally uses a dynamics model (see the mass, center of mass, and inertia matrix member variables starting on RigidBody.h :112). Thus, I believe it is justified in using the dynamics keyword. I'm not so sure about the NPC planner though. It clearly doesn't reason about the reason why, from a physics perspective, things move the way they do and thus I would hesitate to use dynamics when describing its state variables.

[maddog-tri]

The word "dynamics" is a loaded word in robotics...

It is the Drake System API. See also Appendix D.

Drake fundamentally uses a dynamics model (see the mass, center of mass,

The System concept requires a method named "dynamics()", and that method is what jwnimmer-tri is describing/defining. (Perhaps the "dynamics()" method should be named "derivatives()" instead, but that is another issue altogether.)

[liangfok]

+1 for changing the name of method System::dynamics() to be System::derivatives().

[jwnimmer-tri]

+1 for changing the name of method System::dynamics() to be System::derivatives().

The System 2.0 design already doesn't use the 'dynamics' terminology, and I guess I don't think renaming everything in System 1.0 is worth the effort.

[liangfok]

@jwnimmer-tri: Good point. Just to confirm, Framework 1.0's System::dynamics() is replaced with the following two methods in Framework 2.0 right?

template <typename ScalarType> class ContinuousSystemInterface 
    : public SystemInterface<ScalarType> {  
  // Returns the derivatives of the first-order continuous state.
  VectorInterface<ScalarType> GetFirstOrderContinuousDerivatives(
      const Context& context, Cache* cache) const = 0;  

  // Returns the derivatives of the second-order continuous state.
  VectorInterface<ScalarType> GetSecondOrderContinuousDerivatives(
      const Context& context, Cache* cache) const = 0;

  //...
}

[jwnimmer-tri]

Unlike some other trajectory-type code in matlab or otherwise, I posit that the passage of time t should only be loosely coupled into the System model...

The above statement is generally true but I don't think it should be true for the "simple" car path level of sophistication...

I disagree. For the case of my cars, I want to define the paths (roads) ahead of time, and then separately specify when and how quickly the cars traverse them. I very much want to just say "Car A should follow road B (a CarPath), beginning at t=##.### (a hybrid system), accelerating at #.#m/s2 until reaching #.#m/s (a CarVelocityProfile). In various scenarios, I will have agent cars begin moving at different times, or loop forever around the same CarPath, etc. and each one will have its own prescription for how quickly to traverse its path along the road.

I could possibly imagine cases where you want the CarPath's independent variable be a relative t versus some t_0, but never the absolute simulation t that marks the global passage of time. The simulation will run for hours of driving time, but my CarPath systems will not be hours-long trajectories.

FTR, I'm always in favor of going away from time as the underlying independent variable... Doing this, however, does indeed make it less predictable of where the robot's going to be at a particular point in time.

Not really? The distance = velocity * time relationship is really trivial and our simulator arithmetic should handle it exceedingly well. Note that the simulation t has nothing to do with the PC clock; it is the abstract evolution of the simulated world's time, and totally under our control.

[liangfok]

@jwnimmer-tri

FTR, I'm always in favor of going away from time as the underlying independent variable... Doing this, however, does indeed make it less predictable of where the robot's going to be at a particular point in time.

Not really? The distance = velocity * time relationship is really trivial and our simulator arithmetic should handle it exceedingly well. Note that the simulation t has nothing to do with the PC clock; it is the abstract evolution of the simulated world's time, and totally under our control.

Your proposed definition of CarProgress is:

CarProgress (System)
/// Integrate velocity into path distance.

input u = { velocity }
state x = { distance_along_path }
dynamics xdot = { velocity }
output y = { distance_along_path, velocity }

Since distance_along_path is a state variable that is updated in a cumulative manner (i.e., the new distance_along_path depends on the current distance_along_path), won't there be cumulative error over time? In other words, wouldn't it be difficult to predict where the car will be at a certain point in time? I'm OK with this. Just wanted to highlight the fact.

[maddog-tri]

To clarify my (and others) understanding: The primary reason for making things a function of path-position rather than time is that it makes by-hand scripting easier: script-writers typically want to say "behave like this when you get to this place", not "behave like this after this much time has elapsed".

Back to the big picture: overall, looks fine. I am not too concerned about generalization at this point, because such things can/should evolve as this is actually used --- the code isn't even written yet, and has no consumers yet, much less "rule of three". Another reason I'm not concerned about generalization: drake already has the crippling problem of "zero semantics" --- everything is a vector of doubles with no units, no labels, no syntax or mechanism for meaningfully composing a system state from subsystems. Even if there were a generic "Integrator" system, I'd prefer to see a "CarProgress" alias for it with a description of what the variables meant, how they relate to CarPath and CarVelocityProfile, and why the integration behavior is correct/intended. Heck, you could generalize CarPath as "VectorValuedFunction", too, but that wouldn't help anyone accomplish anything.

Also: I prefer _pathposition and _pathvelocity (and _pathacceleration) as concise names that make it clear we are talking about "along the path" (e.g., alluding to path integral).

[maddog-tri]

Since distance_along_path is a state variable that is updated in a cumulative manner (i.e., the new distance_along_path depends on the current distance_along_path), won't there be cumulative error over time?

Doesn't this concern apply to all of drake?

[liangfok]

script-writers typically want to say "behave like this when you get to this place", not "behave like this after this much time has elapsed"

Doesn't the "behave like this when you get to this place" imply some level of planning? I'm afraid we are conflating path specification from planner logic. Ideally these two concepts should be decoupled.

[liangfok]

Since distance_along_path is a state variable that is updated in a cumulative manner (i.e., the new distance_along_path depends on the current distance_along_path), won't there be cumulative error over time?

Doesn't this concern apply to all of drake?

No since Drake contains controllers and a servo loop that (hopefully) will eliminate error accumulation over time.

[liangfok]

By choosing a t0 where the path-following begins, a hybrid system could transition to starting the path at a pre-determined moment. Alternatively, path-begin transitions could have a more complicated start condition based on other factors (e.g., stop-sign negotiation).

Path transitions are definitely a valid concern. Any discontinuity between paths will result in vehicle occupant discomfort at best and complete system instability at worst. Typically some form of ramps or cubic splines are used to blend the end of one path with the beginning of the other. Again, I believe this should be done by a higher-level planner and not specified in the path specification itself.

[sherm1]

The distance = velocity * time relationship is really trivial and our simulator arithmetic should handle it exceedingly well.

(This assumes velocity is constant; otherwise this relationship is more complicated.)

[RussTedrake]

sorry i've had trouble keeping up with the volume of comments. :)

my initial reaction was (and continues to be) that a path is a very useful object to have in c++, but is not a system. probably just a class (perhaps even just a spline). A trajectory -- which in my lingo is a path that is governed by time -- would be a dynamical system with time as the input and the e.g. euclidean point over time as an output (no state/dynamics).

[jwnimmer-tri]

Ah. I guess I thought that to support analysis, the Path had to be more glass-boxy. I am more than happy to just C++ class everything except for the final pose that comes out of some System.

[RussTedrake]

templating it is still good. and the trajectory system can use the path’s methods (when that makes sense).

[jwnimmer-tri]

Right. Always ScalarType; never double.

[jwnimmer-tri]

I think this is fair to close. There's some code in-tree, and the next step is road networks, not additional bespoke trajectories.