CMPT 419/983 Assignment 2

Due date: October 28, 2019

Submit zip file to CourSys

1) Sequential Quadratic Programming. Use Sequential Quadratic

Programming to solve the NLP, introduced in class. You may use any

software to solve the quadratic subproblems. (The solution will be given

in cvx).

minimize sin(𝜋𝑥) sin(2𝜋𝑦)

subject to −1 ≤ 𝑥 ≤ 1

−1 ≤ 𝑦 ≤ 1

a) Construct a quadratic subproblem that minimizes the quadratic

approximation of the objective, subject to linearization of the constraints,

centred around a given iterate 𝑥.

b) Write code to solve the above quadratic subproblem using cvx.

c) Write code to solve the entire NLP, starting from several different initial

points 𝑥. Make a plot showing several sequences of {𝑥

} starting these initial

points.

2) Differential Flatness. Consider the following simple model of the car (that

is slightly more complex than the one introduced in class:

𝑥̇ = 𝑣 cos 𝜃

𝑦̇ = 𝑣 sin 𝜃

𝜃̇ = 𝜔

𝑣̇ = 𝑎

The states consist of the position (𝑥, 𝑦), the heading 𝜃, and the

longitudinal speed 𝑣. The controls are the turn rate 𝜔 and the longitudinal

acceleration 𝑎.

a) Show that the system is differentially flat by letting 𝑧 = (𝑥, 𝑦), and deriving

the functions 𝛽 and 𝛾 such that

(𝑥, 𝑦, 𝜃, 𝑣) = 𝛽൫𝑧, 𝑧̇, … , 𝑧

()൯,

(𝜔, 𝑎) = 𝛾൫𝑧, 𝑧̇, … , 𝑧

()൯.

b) Consider a maneuver given by the following initial condition at 𝑡 = 0 and final

condition at 𝑡 = 𝑇 = 10:

⎣

⎢

⎢

⎡

𝑥(0)

𝑦(0)

𝜃(0)

𝑣(0)⎦

⎥

⎥

⎤

= ൦

0

0

0

1

൪ ,

⎣

⎢

⎢

⎡

𝑥(𝑇)

𝑦(𝑇)

𝜃(𝑇)

𝑣(𝑇)⎦

⎥

⎥

⎤

=

⎣

⎢

⎢

⎢

⎡

0

0

𝜋

2

1⎦

⎥

⎥

⎥

⎤

Using the basis functions 𝜓

(𝑡) = 1, 𝜓ଵ

(𝑡) = 𝑡, 𝜓ଶ = 𝑡

ଶ

, 𝜓ଷ = 𝑡

ଷ

, 𝜓ସ = 𝑡

ସ

, we

parameterize the flat outputs as follows:

𝑥(𝑡) = 𝑏𝜓(𝑡)

ଷ

ୀ

𝑦(𝑡) = 𝑏ଵ𝜓(𝑡)

ଷ

ୀ

Write a system of equations in terms of the coefficients {𝑏} and {𝑏ଵ} such

that the initial and final conditions are satisfied.

c) Solve the above systems of equations using a software of your choice to

obtain and plot the state and control trajectories.

3) Multiple Shooting with Casadi. Consider a

system involving an inverted pendulum on a

cart. For this system, the state is (𝑥, 𝑣, 𝜃, 𝜔),

representing the position of the cart, speed of

the cart, angle of the pendulum, and angular

speed of the pendulum, respectively. The initial

state at time 𝑡 = 0 is (0,0,0,0).

The problem parameters are as follows: 𝑀 = 0.5

kg is the mass of the cart, 𝑚 = 0.2 kg is the

mass of the pendulum, 𝑙 = 0.3 m is the half-length of the pendulum, 𝐼 =

0.006 kg∙m2

is the moment of inertial of the pendulum.

We would like to apply a force 𝐹(𝑡) such that at time 𝑡 = 5 s, the cart is

stopped at the origin, with the pendulum being stationary at the upright

position, represented by the state (0,0, 𝜋, 0). At the same time, we would

like to minimize the control effort, given by the integral ∫ 𝐹

ଶ(𝑡)𝑑𝑡 ହ

௧ୀ , while

ensuring that the position of the cart stays within 1 m of the origin, |𝑥| ≤

1. The control is also limited by |𝐹(𝑡)| ≤ 0.2 at all times.

The equations of motion are given by

(𝑀 + 𝑚)𝑥̈ + 𝑏𝑥̇ + 𝑚𝑙𝜃̈cos 𝜃 − 𝑚𝑙𝜃̇ଶ sin 𝜃 = 𝐹

(𝐼 + 𝑚𝑙ଶ)𝜃̈+ 𝑚𝑔𝑙 sin 𝜃 = −𝑚𝑙𝑥̈ cos 𝜃

where 𝑏 = 0.1 N/m/s is the coefficient of friction, and 𝑔 = 9.81 m/s/s is

the acceleration due to gravity on Earth. In terms of a first-order ODE, we

have 𝑥̇ = 𝑣, 𝜃̇ = 𝜔. The rest of the system dynamics, for (𝑣, 𝜔), can be

implicitly written as

ቂ 𝑀 + 𝑚 𝑚𝑙 cos 𝜃

𝑚𝑙 cos 𝜃 𝐼 + 𝑚𝑙ଶ

ቃ ቂ𝑣̇

𝜔̇ቃ =

−𝑏𝑣 + 𝑚𝑙𝜔ଶ sin 𝜃 + 𝐹

−𝑚𝑔𝑙 sin 𝜃 ൨

a) Write down the associated optimal control problem.

b) Discretize the optimal control problem using the multiple shooting method

and write down the resulting nonlinear program. Use 𝑁 = 50 time intervals,

forward Euler integration, and left endpoint first order integration (the same

schemes as presented in the lecture).

c) Using the Casadi toolbox, which can be found at

https://github.com/casadi/casadi/wiki, solve the nonlinear program, and plot

(𝑥(𝑡), 𝑣(𝑡), 𝜃(𝑡), 𝜔(𝑡)), and 𝐹(𝑡). A good starting point is the direct multiple

shooting example at https://github.com/casadi/casadi/wiki/examples.

4) Reachability Analysis. Consider a kinematic model of a fixed-wing aircraft

flying in the presence of wind, given as follows:

𝑥̇ = 𝑣 cos 𝜃 + 𝑑௫

𝑦̇ = 𝑣 sin 𝜃 + 𝑑௬

𝜃̇ = 𝑢 + 𝑑ఏ

The state (𝑥, 𝑦, 𝜃) consists of 𝑥- and 𝑦- position, and heading 𝜃. The

aircraft flies at a constant speed 𝑣 = 0.3 km/s, and controls its turn rate 𝑢,

with |𝑢| ≤ 0.5 rad/s. The effect of wind is modeled by the disturbance 𝑑 =

൫𝑑௫, 𝑑௬, 𝑑ఏ൯ with |𝑑௫

|,ห𝑑௬ห ≤ 50 m/s, and |𝑑ఏ

| ≤ 0.005 rad/s.

The aircraft aims to arrive at its next waypoint at 𝑥 = 2 km, 𝑦 = 2 km, 𝜃 =

ଷగ

଼

, with an acceptable tolerance of 200 m in distance and గ

ଵ

in heading.

We now proceed to compute all possible initial states and times that can

reach the setpoint within tolerance at time 𝑡 = 0.

a) What is the target set 𝒯 representing the acceptable set of states for

reaching the waypoint within the tolerances? What is a suitable function

𝑙(𝑥, 𝑦, 𝜃) such that 𝑙(𝑥, 𝑦, 𝜃) ≤ 0 ⇔ (𝑥, 𝑦, 𝜃) ∈ 𝒯?

b) The value function 𝑉(𝑡, 𝑥, 𝑦, 𝜃) representing the set of states from which the

aircraft can reach the target set within 𝒕 seconds is the solution to the HJI

variational inequality

min ൜

𝜕𝑉

𝜕𝑡 (𝑡, 𝑥, 𝑦, 𝜃) + min

௨∈𝒰

max

ௗ∈𝒟

𝜕𝑉

𝜕𝑥 (𝑡, 𝑥, 𝑦, 𝜃)ୃ𝑓(𝑥, 𝑦, 𝜃, 𝑢, 𝑑) , 𝑙(𝑥) − 𝑉(𝑡, 𝑥)ൠ = 0

where 𝑓(𝑥, 𝑦, 𝜃, 𝑢, 𝑑) is given by the system dynamics. Determine the optimal 𝑢

and 𝑑 optimizes the min-max expression. That is find an analytic expression

for 𝑢

∗

and 𝑑

∗

, where

𝑢

∗(𝑡, 𝑥, 𝑦, 𝜃) = arg min

௨∈𝒰

max

ௗ∈𝒟

∇𝑉(𝑡, 𝑥, 𝑦, 𝜃)ୃ𝑓(𝑥, 𝑦, 𝜃, 𝑢, 𝑑)

𝑑

∗(𝑡, 𝑥, 𝑦, 𝜃) = arg max

ௗ∈𝒟

∇𝑉(𝑡, 𝑥, 𝑦, 𝜃)ୃ𝑓(𝑥, 𝑦, 𝜃, 𝑢

∗

, 𝑑)

and 𝑢 ∈ 𝒰 and 𝑑 ∈ 𝒟 represents the bounds on the control and disturbances.

c) Compute 𝑉(𝑡, 𝑥, 𝑦, 𝜃) from 𝑡 = −10 to 𝑡 = 0 using the helperOC toolbox, which

can be found at https://github.com/HJReachability/helperOC. A good starting

point is tutorial.m. Use the visSetIm and proj functions to visualize the

following: 𝑉(𝑡 = −10, 𝑥, 𝑦, 𝜃), 𝑉(𝑡 = −5, 𝑥, 𝑦, 𝜃), 𝑉(𝑡 = −10, 𝑥, 𝑦, 𝜃 = 0), 𝑉(𝑡 =

−10, 𝑥, 𝑦 = 0, 𝜃).

For your computation, use the following recommended grid bounds and

resolution: 𝑥 ∈ [−2,5] with 45 grid points, 𝑦 ∈ [−2.5,4] with 45 grid points, 𝜃 ∈

[−𝜋, 𝜋] with 35 grid points.