AA 274: Principles of Robotic Autonomy

Problem Set 1
Due January 22

Starter code for this problem set has been made available online through github; to get started download the
code by running git clone https://github.com/StanfordASL/AA274_HW1.git in a terminal window.
For your final submission, you will submit a single pdf with your answers for written questions (denoted by
the symbol) and the python files that contains your code for code questions (denoted by the symbol).
Stay tuned for more information about “how-to-submit”.

Introduction
The goal of this problem set is to familiarize you with some of the techniques for controlling nonholonomic
wheeled robots. The nonholonomic constraint here refers to the roll without slip condition for the robot
wheels which leads to a nonintegrable set of differential constraints. In this problem set we will consider the
simplest nonholonomic wheeled robot model, the unicycle, shown below in Figure 1.

Figure 1: Unicycle robot model

The kinematic model we will use reflects the roll without slip constraint, and is given below in eq. (1).
ẋ(t) = V cos(θ(t))
ẏ(t) = V sin(θ(t)) (1)
θ̇(t) = ω(t)
The robot state is x = (x, y, θ), where (x, y) is the Cartesian location of the robot center and θ is its alignment
with respect to the x-axis. The robot control inputs are u = (V, ω), where V is the velocity along the main
axis of the robot and ω is the angular velocity, subject to the control constraints: |V (t)| ≤ 0.5 m/s and
|ω(t)| ≤ 1.0 rad/s.
1
Stanford Aeronautics & Astronautics Winter 2018

In the first part of the problem set you will explore different methods of generating feasible trajectories and
associated open-loop control histories. The presence of un-modeled effects, external disturbances, and/or
time sampling will undoubtedly lead to the need for closed-loop control and will be the focus for the second
part of this problem set.
The nonholonomic constraint raises some challenging controllability issues. Chiefly, it can be shown that the
unicycle (and more generally, the class of underactuated driftless regular systems) cannot be stabilized around
any posture (position & orientation) using smooth (or even continuous) time-invariant feedback control laws
[1] (Brockett’s Theorem). This negative result has prompted extensive research in various control strategies
such as smooth time-varying control laws, discontinuous control laws, and feedback linearization. In contrast,
the trajectory tracking problem is significantly simpler (under some mild conditions). In this problem set
you will experiment with a (discontinuous) kinematic and a (smooth) dynamic controller for both posture
and trajectory feedback control, thereby imbuing your robot with some robustness.
Note that many of the control laws discussed in this problem set can also be extended to related nonoholo-
nomic robot models, e.g., a rear/front wheel drive robot with forward steering and a trailer/car combination.
Interested students are referred to [2].

Problem 1: Optimal Control

Consider the kinematic model of the unicycle given in (1). The objective is to drive from one waypoint to
the next waypoint with minimum time & energy, i.e., we want to minimize the functional:
Z tf
λ + V (t)2 + ω(t)2 dt,
 
J=
0

where λ ∈ R≥0 is a weighting factor and tf is free. Consider the following initial and final conditions:
x(0) = 0, y(0) = 0, θ(0) = −π/2,
x(tf ) = 5, y(tf ) = 5, θ(tf ) = −π/2.

(i) Derive the Hamiltonian and conditions for optimality and formulate the problem as a 2P–BVP.

(ii) Edit the script P1_optimal_control.py to solve the 2P–BVP with the largest possible value of
λ such that the resulting optimal control history satisfies the constraints. That is, do not use the
constrained control version of the optimality conditions, i.e., use the conditions in Slide 9 in
Lecture 3 and vary the value of λ yourself. Experiment with different initial guesses for the
solution.

(iii) Explain the significance of using the largest feasible λ.

(iv) Extract the trajectory (x(t), y(t)) and the control history (V (t), ω(t)) using a sample time of 5ms.
Generate the following two plots (verify that the control constraints and boundary conditions are met
and include these plots in your writeup; plotting code has already been provided):
(a) The trajectory (x(t) vs y(t)).
(b) Control histories for V (t) and ω(t).

(v) Validate your work. We will now simulate the car with the computed control history with and
without the presence of disturbances (modeled as input noise). Run sim_traj.py using the filename
containing your saved control history as: python sim_traj.py traj_data_optimal_control.npy
<x_0> <y_0> <th_0> 1 open, where (x0 , y0 , th0 ) are the initial conditions from above (feel free to
write −π/2 ≈ −1.57). By generating the appropriate plots, confirm that the “disturbance-free” tra-
jectory arrives at the goal while the “perturbed” trajectory deviates.
2
Stanford Aeronautics & Astronautics Winter 2018

Note: You will need the scikits.bvp_solver package (which is already installed in the virtual machine
we provide - see question 5 below). See https://pythonhosted.org/scikits.bvp_solver/. Try installing
directly using sudo pip install scikits.bvp_solver. If this fails for some reason, update/reinstall gcc
(make sure you include gfortran), download the source code for the package, and build yourself).
Hint: You will need to re-formulate the problem into “standard” form - see Reformulation of boundary value
problems into standard form,” SIAM review, 23(2):238-254, 1981.

Problem 2: Differential Flatness

Consider the dynamically extended form of the robot kinematic model:

ẋ(t) = V cos(θ(t))
ẏ(t) = V sin(θ(t))
(2)
V̇ (t) = a(t)
θ̇(t) = ω(t)

where the two inputs are now (a(t), ω(t)). Differentiating the velocities (ẋ(t), ẏ(t)) once more yields
      
ẍ(t) cos(θ) −V sin(θ) a u
= := 1
ÿ(t) sin(θ) V cos(θ) ω u2
| {z }
:=J

Note that det(J) = V . Thus for V > 0, the matrix J is invertible. Hence, the outputs (x, y) are the
flat outputs for the unicycle robot and we may use the virtual control inputs (u1 (t), u2 (t)) to design the
trajectory (x(t), y(t)) and invert the equation above to get the control history a(t) and ω(t). We will design
the trajectory (x(t), y(t)) using a polynomial basis expansion of the form:
n
X n
X
x(t) = xi ψi (t), y(t) = yi ψi (t)
i=1 i=1

where ψi , i = 1, . . . , n are the basis functions, and xi , yi are the coefficients to be designed.

(i) Take the basis functions ψ1 (t) = 1, ψ2 (t) = t, ψ3 (t) = t2 , and ψ4 (t) = t3 . Write a set of linear
equations in the coefficients xi , yi , i = 1, . . . , n to express the following initial and final conditions:

x(0) = 0, y(0) = 0, V (0) = 0.5, θ(0) = −π/2,

x(tf ) = 5, y(tf ) = 5, V (tf ) = 0.5, θ(tf ) = −π/2,

where tf = 15. Why can we not set V (tf ) = 0?

(ii) Edit the script P2_differential_flatness.py to solve the system of equations above and compute
the state-trajectory (x(t), y(t), θ(t)), and control history (V (t), ω(t)). (You may find the function
linalg.solve useful).

(iii) Since there are only 4 basis polynomials and 4 constraints (for x and y at each endpoint), we get
a unique solution. However, the trajectory may not necessarily satisfy the control constraints. Some
ways to circumvent this are to (1) increase the number of basis polynomials and solve a constrained
trajectory optimization problem, (2) increase the value of tf until the control history becomes feasible
(this can be very suboptimal), or (3) re-scale the velocity trajectory while keeping the geometric aspects
of the trajectory the same. We will adopt option (3).

3
Stanford Aeronautics & Astronautics Winter 2018

Define a path parameter s (i.e., arc-length) for the trajectory by ṡ(t) = V (t), s(0) = 0. That is,
s(t) denotes the distance traveled along the path up to time t. Thus, we can refer to the path as
(x(s), y(s), θ(s)) = (x(s(t)), y(s(t)), θ(s(t))) which gives us the state (x, y, θ) now parameterized by the
arc-length s. Let’s consider choosing an alternative velocity control as a function of s, call this new
ds
velocity profile Ve (s). This corresponds to a new time history τ (s) specified by dτ = Ve (s), τ (0) = 0,
which we may rearrange and integrate to yield
Z τ Z s
0 ds0
τ (s) = dτ = .
0 0 Ve (s0 )

The alternative angular velocity control input is given by

dθ dθ dt ds 1
ω
e (s) = = = ω(s) · · Ve (s)
dτ dt ds dτ V (s)
where ω denotes the original angular velocity control input produced in part (ii).
e (s) satisfy the control saturation constraints |Ve (s)| ≤ 0.5 m/s and |e
Select Ve (s) so that it and ω ω (s)| ≤ 1
rad/s. In order to get all of the state and control histories in terms of uniform timesteps in τ we’ll
have to do a little interpolation (this is already implemented in the code).

(iv) Generate the following plots (plotting code has been provided):
(a) The trajectory (x(t), y(t)). (Original and scaled trajectories should be identical!)
(b) The arc-length s(t) for the original and scaled trajectories on the same plot.
(c) The V and ω histories for the original and scaled trajectories on the same plot.

(v) Validate your work. Run sim_traj.py using the filename containing your saved control history as:
python sim_traj.py traj_data_differential_flatness.npy <x_0> <y_0> <th_0> 1 open. By
generating the appropriate plots, confirm that the “disturbance-free” trajectory arrives at the goal
while the “perturbed” trajectory deviates.

Problem 3: Closed-loop Control I

We will now study closed-loop control for posture stabilization, i.e., stabilize the robot around some desired
position and pose. For consistency, we will set the desired goal position to be (xg , yg ) = (5, 5) and desired
pose as θg = −π/2. One way to get around Brockett’s controllability result which prevents the use of smooth
invariant state-feedback control laws is to use a change of variables. Consider Figure 2.
Here, (ρ, δ) represent the robot’s polar coordinate position with respect to the goal, and α is the bearing
error. In these new coordinates our kinematic equations of motion become:

ρ̇(t) = −V (t) cos(α(t))

sin(α(t))
α̇(t) = V (t) − ω(t)
ρ(t) (3)
sin(α(t))
δ̇(t) = V (t)
ρ(t)
The posture stabilization problem is now equivalent to driving the state (ρ, α, δ) to the unique equilibrium
(0, 0, 0). In these coordinates, Brockett’s condition no-longer applies so we will use the simple (smooth)
control law:
V = k1 ρ cos(α)
sin(α) cos(α) (4)
ω = k2 α + k1 (α + k3 δ),
α
4
Stanford Aeronautics & Astronautics Winter 2018

Figure 2: New coordinate system

where k1 , k2 , k3 > 0. Note (1) the kinematic equation (3) is undefined at ρ = 0. However, the control
law above drives the robot to the desired goal asymptotically (i.e., as time goes to infinity) so we never
have to worry about the equations becoming undefined. (2) The control law is discontinuous in the original
coordinates, as necessary due to Brockett’s result.

(i) Edit the control function ctrl_pose in the file P3_pose_stabilization.py and program the
control law above.

(ii) Validate your work. Explore the results of your controller by running the script sim_parking.py
with a variety of start locations and poses using the syntax: python sim_parking.py x_0 y_0 th_0
t_f where (x0 , y0 , th0 ) is the initial position and pose of the robot, and tf is the length of time to
simulate. Generate the following plots:

(a) Trajectories for forward, reverse, and parallel parking scenarios.

(b) Control histories for (V (t) and ω(t))

HINTS: (1) You should use the function wrapToPi also present in utils.py to ensure that α and δ remain
within the range [−π, π]. This is needed since the control law has terms linear in α and δ. (2) Use the
function np.sinc to handle values α ≈ 0 in the control law for ω above. (3) You are free to choose the
gains k1 , k2 , and k3 (try staying between (0, 1.5]), however, do not be too aggressive since we have saturation
limits!
Note: It is possible to extend this method to allow path tracking [3] however we will adopt a simpler and
more elegant approach to this problem.

Problem 4: Closed-loop Control II

We will now address closed-loop control for (i) trajectory tracking using the differential flatness approach,
and (ii) parking using the non-linear kinematic controller from Problem 3. Consider the trajectory designed
in Problem 2 (we will refer to the center coordinates of this desired trajectory as (xd , yd )). We will implement

5
Stanford Aeronautics & Astronautics Winter 2018

the following virtual control law for trajectory tracking:

u1 = ẍd + kpx (xd − x) + kdx (ẋd − ẋ)

(5)
u2 = ÿd + kpy (yd − y) + kdy (ẏd − ẏ)

where kpx , kpy , kdx , kdy > 0 are the control gains.

(i) Write down a closed form ODE system for the true control inputs (V, ω) in terms of the virtual
controls (u1 , u2 ) = (ẍ, ÿ).

(ii) Edit the function ctrl_traj in the file P4_trajectory_tracking.py and program in the virtual
control law Eq. (5) and integrate your answer from (i) to convert (u1 , u2 ) into the actual control inputs
(V, ω). (HINT: At each timestep you may consider the current velocity to be that commanded in the
previous timestep. WARNING: You must be careful when computing the actual control inputs due to
the potential singularity in velocity discussed in Problem 2 — although we took care to ensure that
the nominal trajectory did not have V ≈ 0 it is possible that noise might cause this to singularity to
occur. In this case you should use a “reset” strategy to the nominal velocity if the true velocity gets
too small.)

(iii) Modify this control function to switch to the pose stabilization controller from Problem 3 when
you are “sufficiently close” to the goal position (5, 5).

(iv) Validate your work. Run sim_traj.py using the filename containing your saved trajectory his-
tory as: python sim_traj.py <filename> <x_0> <y_0> <th_0> 1 closed. Confirm that both the
“disturbance-free” and “perturbed” trajectories arrive at the goal. Experiment with different initial
conditions.

Note: It is possible to modify the flatness-based trajectory tracking controller to also allow posture stabi-
lization. See [4] for details.

Problem 5: Robot Operating System

As discussed during lecture, we will be using ROS throughout the semester. ROS stands for Robot Operating
System. Effectively it allows us to break down a robot software stack in different modules called nodes
(each of them a process) that seamlessly communicate with each other over topics (using TCP/UDP-like
connections). Every week we will implement a piece of the software stack (most likely a node) that we will
use to accomplish a complex task with a real robot at the end of the semester.
For this homework, you will familiarize yourself to the ROS environment. Namely, the goals of this ROS
question are:

• Setting up the VM/ROS.

• Creating a package in your catkin workspace and building it.
• Writing publisher/subscriber nodes and executing them.
• Use the rostopic tool.

• Familiarize yourself with the Gazebo environment.

There is a lot to read, please read the entire text first before starting on the problem!

6
Stanford Aeronautics & Astronautics Winter 2018

Setting up

In order to facilitate your experience with ROS, we are providing you with a virtual machine (located at
https://stanford.box.com/v/aa274vm2018) that already has most of the required software installed. The
virtual machine’s password is the same as its username (aa274).
Stanford provides access to a good virtualization solution (VMware) for free. Go to https://stanford.
onthehub.com/WebStore/Welcome.aspx, login (top right), search ”VMware” in the top box, select ”VMware
Fusion 10.0” if you are using Mac, select ”VMware Workstation 14” otherwise.
Open VMWare and click on ”Open a Virtual Machine” and select the .ovf file from the zip file. If an error
appears, just click retry and it should work the second time around.
For reference, the virtual machine provided was built using the software described here http://emanual.
robotis.com/docs/en/platform/turtlebot3/pc_software_setup/, here http://emanual.robotis.com/
docs/en/platform/turtlebot3/simulation and the course repository https://github.com/StanfordASL/
asl_turtlebot was installed in your catkin workspace. In the virtual machine, if you look at the end of
~/.bashrc you will also see that we call a script called setup.bash contained in your workspace every
time we launch a bash shell. This then allows bash to find the different packages contained in your catkin
workspace.
Feel free to recreate the virtual machine on your own and even dual boot your computer for that purpose.
However, note that we will only officially support the virtual machine (in the sense of providing help with
your setup). The dual boot might, however, give you much better performance when it comes to graphic
rendering.
If you find that your VM is running too slow, you can increase the amount of memory allocated to your VM.
(Virtual Machine > Settings > Processors and Memory) Further, you can disable hardware acceleration. Go
to settings of the AA274 VM > Display > uncheck Accelerate 3D graphics.
Lets now create a package in your workspace!

The Basic Publisher and Subscriber

Go to the src folder in your catkin workspace.

$ cd ∼/ c a t k i n w s / s r c
$ c a t k i n c r e a t e p k g hw1 s t d m s g s r o s c p p r o s p y
$ cd ∼/ c a t k i n w s
$ catkin make

Now create a scripts directory in your new hw1 package. Then create a file called publisher.py in the
scripts directoy and add the following code to the file.
#! / u s r / b i n / env python

import r o s p y
from s t d m s g s . msg import S t r i n g

def p u b l i s h e r ( ) :
pub = r o s p y . P u b l i s h e r ( ’ r a n d o m s t r i n g s ’ , S t r i n g , q u e u e s i z e =10)
r o s p y . i n i t n o d e ( ’ p u b l i s h e r ’ , anonymous=True )
r a t e = r o s p y . Rate ( 1 )
while not r o s p y . i s s h u t d o w n ( ) :
### YOUR CODE HERE ###
# your code t o p u b l i s h a s t r i n g
# goes here

7
Stanford Aeronautics & Astronautics Winter 2018

### END OF YOUR CODE ###

rate . sleep ()

if name == ’ m a i n ’ :
try :
publisher ()
except r o s p y . ROSInterruptException :
pass

Fill in the section we commented out. The code should take one or two lines and use the pub.publish()
command.
Make the script executable using the following command (you need to do this for every new python node you
write).
$ chmod a+x p u b l i s h e r . py

Once your script is written, build your package using catkin_make in the catkin_workspace directory
(required even for python scripts because of the occasional messages you will add so a good habit to have).
Then run your script by first launching master in one terminal (roscore) and then in another terminal
starting your node (rosrun hw1 publisher.py).
You should be able to see the strings you are publishing using the rostopic tool. Make sure you do,
otherwise you will not be able to move forward in the homework. In a separate terminal, type:
$ r o s t o p i c echo r a n d o m s t r i n g s
data : HELLO WORLD!
−−−
data : HELLO WORLD!
−−−
data : HELLO WORLD!
−−−
...

Note that if your script produces errors, they will not necessarily display if you run your node using rosrun.
This is because ROS will capture the error messages. You can get them by running your script directly
instead.
$ cd ∼/ c a t k i n w s / s r c /hw1/ s c r i p t s
$ python p u b l i s h e r . py

Now let’s try to write a node that will subscribe to our topic. Copy and complete the following code into a
file called subscriber.py. Make sure to make the script executable again (the command is written above).
#! / u s r / b i n / env python

import r o s p y
from s t d m s g s . msg import S t r i n g

def c a l l b a c k ( data ) :
r o s p y . l o g i n f o ( ’ r e c e i v e d : %s ’ , data . data )

def s u b s c r i b e r ( ) :
r o s p y . i n i t n o d e ( ’ s u b s c r i b e r ’ , anonymous=True )
### YOUR CODE HERE ###
# f i l l in the construction
# of the subscriber

8
Stanford Aeronautics & Astronautics Winter 2018

### END OF YOUR CODE ###

rospy . spin ( )

if name == ’ main ’:
subscriber ()

Your code should use the Subscriber constructor rospy.Subscriber() as well as the topic name you used
in the publisher (i.e. random_strings).
Keep your publisher node running, and run the subscriber node in another terminal (so you should now have
three things running). It should receive the messages you are publishing on the topic.
$ r o s r u n hw1 s u b s c r i b e r . py
[ INFO ] [ 1 4 8 3 8 9 6 4 9 4 . 7 3 3 0 8 1 ] : r e c e i v e d : HELLO WORLD!
[ INFO ] [ 1 4 8 3 8 9 6 4 9 5 . 7 3 3 6 3 2 ] : r e c e i v e d : HELLO WORLD!
[ INFO ] [ 1 4 8 3 8 9 6 4 9 6 . 7 3 3 5 7 7 ] : r e c e i v e d : HELLO WORLD!
...

Simulation in Gazebo

One of the most important (if not the most important) tool when developing robotic software is simulation.
A good simulator is one that accurately represents the physics of your system as well as lets you seamlessly
execute the same code you would run on the real system.
There is already a package in your workspace called asl_turtlebot. This package is a clone of the repository
that is available online here: https://github.com/StanfordASL/asl_turtlebot. We encourage you to
explore this package. Start by pulling any new changes we may have published there.
$ cd ∼/ c a t k i n w s / s r c / a s l t u r t l e b o t
$ git pull

Lets try running a simulation of our turtlebot using roslaunch (make sure you include the 3 or you will be
running last year’s version!).
$ roslaunch a s l t u r t l e b o t t u r t l e b o t 3 s i m . launch

You should see Gazebo start running a simulation that looks like this:

9
Stanford Aeronautics & Astronautics Winter 2018

Now let’s try to run the parking controller you designed in problem 3 on the simulated turtlebot.
In the asl_turtlebot package, you should have a node called controller.py. Modify it so that it imple-
ments the controller you designed in problem 3. You should more or less be able to just copy and paste your
code from problem 3 into the get_ctrl_output method. Make sure you have a look at the rest of the code
too. This is representative of what we expect you to be able to implement later this quarter.

(i) Make your publisher node publish your first and last name as one single string (e.g., ’marco
pavone’. Then use the following command to record a rosbag of it and keep that file for submission
with your homework.
$ rosbag record random strings

(ii) You can verify the contents of your rosbag. What is the command to play back a rosbag with
filename ‘filename.bag’ ?

(iii) In Gazebo, run a simulation with the target state x = 1.0, y = 1.0 and θ = 0.0. Take a rosbag
recording of both the state and control topics. You will submit this rosbag as part of your homework.
$ r o s b a g r e c o r d / gazebo / m o d e l s t a t e s / cmd vel

WARNING: Depending on your computer, it might have a hard time rendering Gazebo’s environment from
inside a virtual machine. You can opt to run the problem set without the Gazebo graphics by passing an
argument to the launch file.
$ r o s l a u n c h a s l t u r t l e b o t t u r t l e b o t 3 s i m . l a u n c h g u i := f a l s e

We also implemented for you a simple visualizer so that you can still see your turtlebot moving. It should
leave a ’trail’ as it moves around.
$ r o s r u n a s l t u r t l e b o t g a z e b o p l o t . py
10
Stanford Aeronautics & Astronautics Winter 2018

References
[1] R. W. Brockett, “Asymptotic stability and feedback stabilization,” Differential geometric control theory,
vol. 27, no. 1, pp. 181–191, 1983.
[2] A. De Luca, G. Oriolo, and C. Samson, “Feedback control of a nonholonomic car-like robot,” in Robot
motion planning and control. Springer, 1998, pp. 171–253.

[3] M. Aicardi, G. Casalino, A. Bicchi, and A. Balestrino, “Closed loop steering of unicycle like vehicles via
lyapunov techniques,” IEEE Robotics & Automation Magazine, vol. 2, no. 1, pp. 27–35, 1995.
[4] G. Oriolo, A. De Luca, and M. Vendittelli, “Wmr control via dynamic feedback linearization: design,
implementation, and experimental validation,” IEEE Transactions on control systems technology, vol. 10,
no. 6, pp. 835–852, 2002.

11