Tutorial 2: Variational Trajectory Planning - Build a motion planner and upload benchmark solutions

This tutorial shows how you can plan trajectories in CommonRoad scenarios by solving an optimization problem and easily create a benchmark solution file for upload the solution to commonroad.in.tum.de. We therefore use the CommonRoad scenario 'ZAM_Tutorial-1_2_T-1.xml' which was created in Tutorial 1.

Task of the Ego Vehicle

The ego vehicle has the following specification in our scenario:

• obstacle type: car
• obstacle shape: rectangle with a width of 1.844m and a length of 4.569m as prescribed in VW Vanagon, Vehicle Type 3 (see also documentation vehicle models)
• initial state:
  • position: (15, 0.0) m
  • orientation: 0.00 rad
  • velocity: 22 m/s
• ID in ZAM_Tutorial-1_2_T-1.xml: 100

The following physical constraints apply to the ego vehicle:

• minimum acceleration: -6 m/s2
• maximum acceleration: 15 m/s2
• minimum jerk: -15 m/s3
• maximum jerk: 15 m/s3

The task of the ego vehicle is to reach its target velocity of 30.0 m/s without colliding with other obstacles.

Exercise 0: Read the created CommonRoad scenario

The CommonRoadFileReader reads the Scenario and the PlanningProblemSet from a CommonRoad XML-file. Please read the scenario from the CommonRoad file.

%matplotlib inline
import os
import matplotlib.pyplot as plt
from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.visualization.mp_renderer import MPRenderer

# load the CommonRoad scenario that has been created in the CommonRoad tutorial
file_path = os.path.join(os.getcwd(), 'ZAM_Tutorial-1_2_T-1.xml')

scenario, planning_problem_set = CommonRoadFileReader(file_path).open()

# plot the scenario for each time step
for i in range(0, 40):
    plt.figure(figsize=(25, 10))
    rnd = MPRenderer()
    scenario.draw(rnd, draw_params={'time_begin': i})
    planning_problem_set.draw(rnd)
    rnd.render()

Helper Functions for visualization

First, make sure that the required packages are installed. The following functions are necessary for this tutorial.

try:
    import matplotlib.pyplot as plt
except ImportError:
    print('Matplotlib not installed. Please use pip(3) to install required package!')

try:
    import numpy as npy
except ImportError:
    print('Numpy not installed. Please use pip(3) to install required package!')

try:
    import vehiclemodels
except ImportError:
    print('commonroad-vehicle-models not installed. Please use pip install to install required package!')

try:
    import pkg_resources
    pkg_resources.require("scipy>=1.1.0")
    pkg_resources.require("cvxpy>=1.0.0")
    from cvxpy import *
except ImportError:
    print('CVXPy not installed or wrong version. Please use pip(3) to install required package!')

class TIConstraints:
    a_min = -8
    a_max = 15
    s_min = 0
    s_max = 150
    v_min = 0
    v_max = 35
    j_min = -30
    j_max = 30


def plot_state_vector(x : Variable, c : TIConstraints, s_obj = None):
    plt.figure(figsize=(10,10))
    N = x.shape[1]-1
    s_max = npy.maximum(150,100+0*npy.ceil(npy.amax((x.value)[0,:].flatten())*1.1/10)*10)

    # Plot (x_t)_1.
    plt.subplot(4,1,1)
    x1 = (x.value)[0,:].flatten()
    plt.plot(npy.array(range(N+1)),x1,'g')
    if s_obj is not None:
        plt.plot(npy.array(range(1,N+1)),s_obj[0],'b')
        plt.plot(npy.array(range(1,N+1)),s_obj[1],'r')
    plt.ylabel(r"$s$", fontsize=16)
    plt.yticks(npy.linspace(c.s_min, s_max, 3))
    plt.ylim([c.s_min, s_max])
    plt.xticks([])

    # Plot (x_t)_2.
    plt.subplot(4,1,2)
    x2 = (x.value)[1,:].flatten()
    plt.plot(npy.array(range(N+1)),x2,'g')
    plt.yticks(npy.linspace(c.v_min,c.v_max,3))
    plt.ylim([c.v_min, c.v_max+2])
    plt.ylabel(r"$v$", fontsize=16)
    plt.xticks([])

    # Plot (x_t)_3.
    plt.subplot(4,1,3)
    x2 = (x.value)[2,:].flatten()
    plt.plot(npy.array(range(N+1)),x2,'g')
    plt.yticks(npy.linspace(c.a_min,c.a_max,3))
    plt.ylim([c.a_min, c.a_max+2])
    plt.ylabel(r"$a$", fontsize=16)
    plt.xticks([])

    # Plot (x_t)_4.
    plt.subplot(4,1,4)
    x2 = (x.value)[3,:].flatten()
    plt.plot(npy.array(range(N+1)), x2,'g')
    plt.yticks(npy.linspace(c.j_min,c.j_max,3))
    plt.ylim([c.j_min-1, c.j_max+1])
    plt.ylabel(r"$j$", fontsize=16)
    plt.xticks(npy.arange(0,N+1,5))
    plt.xlabel(r"$k$", fontsize=16)
    plt.tight_layout()
    plt.show()

Variables of the Optimization Problem

We use the convex optimization package cvxpy to formulate our optimization problem. The following code initializes all necessary variables such as the state and input vector as well as the cost matrices.

# problem data
N  = 40  # number of time steps
n  = 4   # length of state vector 
m  = 1   # length of input vector
dT = scenario.dt # time step


# set up variables
x = Variable(shape=(n,N+1)) # optimization vector x contains n states per time step 
u = Variable(shape=(N)) # optimization vector u contains 1 state

# set up constraints
c = TIConstraints()
c.a_min = -6 # Minimum feasible acceleration of vehicle
c.a_max = 6 # Maximum feasible acceleration of vehicle
c.s_min = 0 # Minimum allowed position
c.s_max = 100 # Maximum allowed position
c.v_min = 0 # Minimum allowed velocity (no driving backwards!)
c.v_max = 35 # Maximum allowed velocity (speed limit)
c.j_min = -15 # Minimum allowed jerk 
c.j_max = 15 # Maximum allowed jerk

# weights for cost function
w_s = 0
w_v = 8
w_a = 2
w_j = 2
Q = npy.eye(n)*npy.transpose(npy.array([w_s,w_v,w_a,w_j]))
w_u = 1
R = w_u

Exercise 1: Consider the vehicle model

In order to obtain kinematically feasible motion plans, the optimization problem needs to respect the constraints of the vehicle model. We use a linear vehicle model, which can be written in the following state-space form: $$x_{k+1} = A x_k + B u_k.$$ Please fill the matrices A and B according to the fourth-order point-mass model, discretized using the time step dT.

A = npy.array([[1,dT,(dT**2)/2,(dT**3)/6],
               [0,1,dT,(dT**2)/2],
               [0,0,1,dT],
               [0,0,0,1]])
B = npy.array([(dT**4)/24,
               (dT**3)/6,
               (dT**2)/2,
               dT]).reshape([n,])

Formulation of the Optimization Problem

We are now able to formulate the optimization problem. Please set the initial state of the ego vehicle with a velocity of 0 m/s and its reference velocity. Subsequently, we create the cost terms and constraints of the optimization problem.

# get the initial state of the ego vehicle from the planning problem set
planning_problem = planning_problem_set.find_planning_problem_by_id(100)
initial_state = planning_problem.initial_state

# initial state of vehicle for the optimization problem (longitudinal position, velocity, acceleration, jerk)
x_0 = npy.array([initial_state.position[0],
                 initial_state.velocity,
                 0.0,
                 0.0]).reshape([n,]) # initial state

# reference velocity
v_ref = 30.0
# Set up optimization problem
states = []
cost = 0
# initial state constraint
constr = [x[:,0] == x_0]

for k in range(N):
    # cost function
    cost += quad_form(x[:,k+1] - npy.array([0,v_ref,0,0],), Q)\
           + R * u[k] ** 2
    # time variant state and input constraints
    constr.append(x[:,k+1] == A @ x[:,k] + B * u[k])

# sums problem objectives and concatenates constraints.
# create optimization problem
prob = Problem(Minimize(cost), constr)

# Solve optimization problem
prob.solve(verbose=True)
print("Problem is convex: ",prob.is_dcp())
print("Problem solution is "+prob.status)

# plot results
plot_state_vector(x, TIConstraints())

===============================================================================
                                     CVXPY
                                    v1.1.13
===============================================================================
(CVXPY) Nov 03 10:08:51 AM: Your problem has 204 variables, 41 constraints, and 0 parameters.
(CVXPY) Nov 03 10:08:51 AM: It is compliant with the following grammars: DCP, DQCP
(CVXPY) Nov 03 10:08:51 AM: (If you need to solve this problem multiple times, but with different data, consider using parameters.)
(CVXPY) Nov 03 10:08:51 AM: CVXPY will first compile your problem; then, it will invoke a numerical solver to obtain a solution.
-------------------------------------------------------------------------------
                                  Compilation
-------------------------------------------------------------------------------
(CVXPY) Nov 03 10:08:51 AM: Compiling problem (target solver=OSQP).
(CVXPY) Nov 03 10:08:51 AM: Reduction chain: CvxAttr2Constr -> Qp2SymbolicQp -> QpMatrixStuffing -> OSQP
(CVXPY) Nov 03 10:08:51 AM: Applying reduction CvxAttr2Constr
(CVXPY) Nov 03 10:08:51 AM: Applying reduction Qp2SymbolicQp
(CVXPY) Nov 03 10:08:51 AM: Applying reduction QpMatrixStuffing
(CVXPY) Nov 03 10:08:51 AM: Applying reduction OSQP
(CVXPY) Nov 03 10:08:51 AM: Finished problem compilation (took 1.104e-01 seconds).
-------------------------------------------------------------------------------
                                Numerical solver
-------------------------------------------------------------------------------
(CVXPY) Nov 03 10:08:51 AM: Invoking solver OSQP  to obtain a solution.
-----------------------------------------------------------------
           OSQP v0.6.2  -  Operator Splitting QP Solver
              (c) Bartolomeo Stellato,  Goran Banjac
        University of Oxford  -  Stanford University 2021
-----------------------------------------------------------------
problem:  variables n = 404, constraints m = 364
          nnz(P) + nnz(A) = 1284
settings: linear system solver = qdldl,
          eps_abs = 1.0e-05, eps_rel = 1.0e-05,
          eps_prim_inf = 1.0e-04, eps_dual_inf = 1.0e-04,
          rho = 1.00e-01 (adaptive),
          sigma = 1.00e-06, alpha = 1.60, max_iter = 10000
          check_termination: on (interval 25),
          scaling: on, scaled_termination: off
          warm start: on, polish: on, time_limit: off

iter   objective    pri res    dua res    rho        time
   1   0.0000e+00   3.00e+01   3.00e+03   1.00e-01   5.76e-04s
  75   8.0046e+03   5.40e-09   7.27e-10   6.86e+01   2.23e-03s
plsh   8.0045e+03   4.55e-11   4.29e-12   --------   2.82e-03s

status:               solved
solution polish:      successful
number of iterations: 75
optimal objective:    8004.5500
run time:             2.82e-03s
optimal rho estimate: 2.39e+02

-------------------------------------------------------------------------------
                                    Summary
-------------------------------------------------------------------------------
(CVXPY) Nov 03 10:08:51 AM: Problem status: optimal
(CVXPY) Nov 03 10:08:51 AM: Optimal value: 8.005e+03
(CVXPY) Nov 03 10:08:51 AM: Compilation took 1.104e-01 seconds
(CVXPY) Nov 03 10:08:51 AM: Solver (including time spent in interface) took 6.158e-03 seconds
Problem is convex:  True
Problem solution is optimal

Exercise 2: Consider additional constraints

The obtained trajectory reaches the desired reference velocity of the ego vehicle. However, the physical constraints are not fulfilled. Please add the physical constraints to the optimization problem in order to obtain kinematically feasible trajectories.

tiConstraints = [x[1,:] <= c.v_max, x[1,:] >= c.v_min] # velocity
tiConstraints += [x[2,:] <= c.a_max, x[2,:] >= c.a_min] # acceleration
tiConstraints += [x[3,:] <= c.j_max, x[3,:] >= c.j_min] # jerk

# Adjust problem
prob = Problem(Minimize(cost), constr + tiConstraints)

# Solve optimization problem
prob.solve()
print("Problem is convex: ",prob.is_dcp())
print("Problem solution is "+prob.status)

# plot results
plot_state_vector(x, c)

Problem is convex:  True
Problem solution is optimal

Exercise 3: Extract the prediction of obstacles

In our scenario, the ego vehicle is surrounded by two other traffic participants -- one following and one leading vehicle. In order to not cause a collision with these two vehicles, we need to consider the positions of the two vehicles in our optimization problem. Please obtain the minimum and maximum feasible position of the ego vehicle for each point in time. Please note that each obstacle has a certain shape.

# extract obstacle from scenario
dyn_obstacles = scenario.dynamic_obstacles

# create constraints for minimum and maximum position
s_min = [] # minimum position constraint
s_max = [] # maximum position constraint

# go through obstacle list and distinguish between following and leading vehicle
for o in dyn_obstacles:
    if o.initial_state.position[0] < x_0[0]:
        print('Following vehicle id={}'.format(o.obstacle_id))
        prediction = o.prediction.trajectory.state_list
        for p in prediction:
            s_min.append(p.position[0]+o.obstacle_shape.length/2.+2.5)
    else:
        print('Leading vehicle id={}'.format(o.obstacle_id))
        prediction = o.prediction.trajectory.state_list
        for p in prediction:
            s_max.append(p.position[0]-o.obstacle_shape.length/2.-2.5)

# plot vehicle motions
plt.plot(range(1,len(s_min)+1),s_min,'b')
plt.plot(range(1,len(s_max)+1),s_max,'r')

Following vehicle id=42
Leading vehicle id=44

[<matplotlib.lines.Line2D at 0x7faf295316a0>]

Exercise 4: Plan the motion of the ego vehicle

The obtained collision constraint allow us to avoid collisions with the following and leading vehicle. Please add the constraints to our optimization problem and obtain a trajectory.

# initial state of ego vehicle
x_0 = npy.array([initial_state.position[0],
                 initial_state.velocity,
                 0,
                 0]).reshape([n,]) # initial state

# reference velocity
v_ref = 30.0

# Set up optimization problem
cost = 0
constr = [x[:,0] == x_0]
for k in range(N):
    # cost function
    cost += quad_form(x[:,k+1] - npy.array([0,v_ref,0,0],), Q)\
           + R * u[k] ** 2
    # single state and input constraints
    constr.append(x[:,k+1] == A @ x[:,k] + B * u[k])
    # add obstacle constraint
    constr.append(x[0,k+1] <= s_max[k])
    constr.append(x[0,k+1] >= s_min[k])

# sums problem objectives and concatenates constraints.
prob = sum(states)
# add constraints for all states & inputs
prob = Problem(Minimize(cost), constr + tiConstraints)


# Solve optimization problem
prob.solve(verbose=True)

print("Problem is convex:",prob.is_dcp())
print("Problem solution is "+prob.status)

# plot results
plot_state_vector(x, c, [s_min,s_max])

x_result = x.value
s_ego = x_result[0,:].flatten()
v_ego = x_result[1,:].flatten()

===============================================================================
                                     CVXPY
                                    v1.1.13
===============================================================================
(CVXPY) Nov 03 10:08:52 AM: Your problem has 204 variables, 127 constraints, and 0 parameters.
(CVXPY) Nov 03 10:08:52 AM: It is compliant with the following grammars: DCP, DQCP
(CVXPY) Nov 03 10:08:52 AM: (If you need to solve this problem multiple times, but with different data, consider using parameters.)
(CVXPY) Nov 03 10:08:52 AM: CVXPY will first compile your problem; then, it will invoke a numerical solver to obtain a solution.
-------------------------------------------------------------------------------
                                  Compilation
-------------------------------------------------------------------------------
(CVXPY) Nov 03 10:08:52 AM: Compiling problem (target solver=OSQP).
(CVXPY) Nov 03 10:08:52 AM: Reduction chain: CvxAttr2Constr -> Qp2SymbolicQp -> QpMatrixStuffing -> OSQP
(CVXPY) Nov 03 10:08:52 AM: Applying reduction CvxAttr2Constr
(CVXPY) Nov 03 10:08:52 AM: Applying reduction Qp2SymbolicQp
(CVXPY) Nov 03 10:08:52 AM: Applying reduction QpMatrixStuffing
(CVXPY) Nov 03 10:08:52 AM: Applying reduction OSQP
(CVXPY) Nov 03 10:08:52 AM: Finished problem compilation (took 1.232e-01 seconds).
-------------------------------------------------------------------------------
                                Numerical solver
-------------------------------------------------------------------------------
(CVXPY) Nov 03 10:08:52 AM: Invoking solver OSQP  to obtain a solution.
-----------------------------------------------------------------
           OSQP v0.6.2  -  Operator Splitting QP Solver
              (c) Bartolomeo Stellato,  Goran Banjac
        University of Oxford  -  Stanford University 2021
-----------------------------------------------------------------
problem:  variables n = 404, constraints m = 690
          nnz(P) + nnz(A) = 1610
settings: linear system solver = qdldl,
          eps_abs = 1.0e-05, eps_rel = 1.0e-05,
          eps_prim_inf = 1.0e-04, eps_dual_inf = 1.0e-04,
          rho = 1.00e-01 (adaptive),
          sigma = 1.00e-06, alpha = 1.60, max_iter = 10000
          check_termination: on (interval 25),
          scaling: on, scaled_termination: off
          warm start: on, polish: on, time_limit: off

iter   objective    pri res    dua res    rho        time
   1   0.0000e+00   9.90e+01   3.00e+03   1.00e-01   4.51e-04s
 125   7.9855e+03   8.67e-04   9.82e-05   1.00e-01   2.00e-03s
plsh   8.0045e+03   4.55e-11   4.29e-12   --------   2.31e-03s

status:               solved
solution polish:      successful
number of iterations: 125
optimal objective:    8004.5500
run time:             2.31e-03s
optimal rho estimate: 1.52e-01

-------------------------------------------------------------------------------
                                    Summary
-------------------------------------------------------------------------------
(CVXPY) Nov 03 10:08:52 AM: Problem status: optimal
(CVXPY) Nov 03 10:08:52 AM: Optimal value: 8.005e+03
(CVXPY) Nov 03 10:08:52 AM: Compilation took 1.232e-01 seconds
(CVXPY) Nov 03 10:08:52 AM: Solver (including time spent in interface) took 3.998e-03 seconds
Problem is convex: True
Problem solution is optimal

Exercise 5

We have successfully planned the motion of the ego vehicle. Please convert the ego vehicle to a dynamic obstacle and visualize the scenario and the motion of the ego vehicle for each time step.

from commonroad.geometry.shape import Rectangle
from commonroad.scenario.obstacle import DynamicObstacle, ObstacleType
from commonroad.scenario.trajectory import Trajectory,State
from commonroad.prediction.prediction import TrajectoryPrediction
from vehiclemodels import parameters_vehicle3

# generate state list of the ego vehicle's trajectory
state_list = [initial_state]
for i in range(1, N):
    orientation = initial_state.orientation
    # compute new position
    # add new state to state_list    
    state_list.append(State(**{'position': npy.array([s_ego[i],0]), 'orientation': orientation,
                               'time_step': i, 'velocity': v_ego[i]*npy.cos(orientation),
                               'velocity_y': v_ego[i]*npy.sin(orientation)}))

# create the planned trajectory starting at time step 1
ego_vehicle_trajectory = Trajectory(initial_time_step=1, state_list=state_list[1:])
# create the prediction using the planned trajectory and the shape of the ego vehicle

vehicle3 = parameters_vehicle3.parameters_vehicle3()
ego_vehicle_shape = Rectangle(length=vehicle3.l, width=vehicle3.w)
ego_vehicle_prediction = TrajectoryPrediction(trajectory=ego_vehicle_trajectory,
                                              shape=ego_vehicle_shape)

# the ego vehicle can be visualized by converting it into a DynamicObstacle
ego_vehicle_type = ObstacleType.CAR
ego_vehicle = DynamicObstacle(obstacle_id=100, obstacle_type=ego_vehicle_type,
                              obstacle_shape=ego_vehicle_shape, initial_state=initial_state,
                              prediction=ego_vehicle_prediction)

# plot the scenario and the ego vehicle for each time step
for i in range(0, 40):
    plt.figure(figsize=(25, 10))
    rnd = MPRenderer()
    scenario.draw(rnd, draw_params={'time_begin': i})
    ego_vehicle.draw(rnd, draw_params={'time_begin': i, 'dynamic_obstacle': {
        'vehicle_shape': {'occupancy': {'shape': {'rectangle': {
            'facecolor': 'g'}}}}}})
    planning_problem_set.draw(rnd)
    rnd.render()

Exercise 6

Now that we have the planned trajectory and motion of our vehicle, we are ready to write the scenario to a benchmark solution file. This can be done by using the Solution Writer provided by Commonroad. A solution file requires a trajectory for the planning problem and IDs for vehicle model, vehicle type, and the scenario ID. Remember, we used a point-mass vehicle model (PM) and the parameters for VW_VANAGON (vehicle type 3). For the cost function, we choose WX1 (see documentation).

After running the following code, you will find a file automatically named in the following format according to the definition from our paper:

solution_{VehicleModel&VehicleType}:{CostFunctionID}:{scenario_id}:{ScenarioVersion}.xml

In this case, the file will be named solution_PM3:SM3:ZAM_Tutorial-1_2_T-1:2020a.xml and it is ready to be submitted on the submissions page.

from commonroad.common.solution import CommonRoadSolutionWriter, Solution, PlanningProblemSolution, VehicleModel, VehicleType, CostFunction

pps = PlanningProblemSolution(planning_problem_id=100,
                              vehicle_type=VehicleType.VW_VANAGON,
                              vehicle_model=VehicleModel.PM,
                              cost_function=CostFunction.WX1,
                              trajectory=ego_vehicle_prediction.trajectory)

# define the object with necessary attributes.
solution = Solution(scenario.scenario_id, [pps], computation_time=prob.solver_stats.solve_time)

# write solution to a xml file
csw = CommonRoadSolutionWriter(solution)
csw.write_to_file(overwrite=True)

Exercise 7

To submit the CommonRoad solution, you need to create an account and afterwards a submission here.

Select the file generated in the previous step and submit your solution. After uploading, you can check your score before publishing your solution.

Finally, you can compare your result against others in the ranking!