Task-Space Trajectories via Cubic Spline Optimization
J. Zico Kolter and Andrew Y. Ng
Computer Science Department, Stanford University, Stanford, CA 94305{kolter,ang}@cs.stanford.edu

Abstract— We consider the task of planning smooth trajectories for robot motion. In this paper we make two contributions.
First we present a method for cubic spline optimization; thistechnique lets us simultaneously plan optimal task-space trajectories and fit cubic splines to the trajectories, while obeying
many of the same constraints imposed by a typical motion
planning algorithm. The method uses convex optimization
techniques, and is therefore very fast and suitable for real-time
re-planning and control. Second, we apply this approach to thetasks of planning foot and body trajectory for a quadruped
robot, the “LittleDog,” and show that the proposed approach
improves over previous work on this robot.

I. I NTRODUCTION
In this paper we consider the task of planning smooth
trajectories for robot motion. This is one of the fundamental
tasks of robotics, and has received a great deal of attention
over the past several decades. One strategy that has provenparticularly effective for this task is the use of smooth,parametrized splines to describe trajectories, either in jointspace or task-space. Cubic splines in particular are ubiquitousin robotic applications [1], as they provide a simple meansof generating smooth (twice differentiable) trajectories for
robot motion.
Cubic splines for robot trajectories are typically employed
as follows. First, one uses a high-level planning algorithm togenerate a series of kinematically feasibly waypoints that the
robot should pass through on its way to the goal. Next, onefits the parameters of a cubic spline that passes through allthese points; the smoothness of the resulting cubic spline
leads to a smoother motion of the robot than would beobtained, for example, by a linear spline that just interpolated
between the waypoints. We refer to this as the “two-phase”
approach, since the planning and spline fitting are done inseparate phases.
However, despite their advantages, cubic splines also suffer from a number of drawbacks. The chief problem is thatin the typical two-phase application of cubic splines, thehigh-level waypoint planning is done separate from the cubic
spline fitting procedure, which can lead to poor trajectories.
To convey this intuition, consider the simple planning task
shown in Figure 1 (a): the objective is to move a double
pendulum, actuated at both joints, from the start to the goal,
while avoiding the obstacle. Figure 1 (b) shows a possible
output from a typical planner (for example, a randomized
tree planner [2]) and the corresponding cubic spline fit
to these waypoints. Due to the stochastic nature of theplanner, the waypoints do not lead to a particularly nice final
trajectory. Existing trajectory optimization techniques [3] canhelp mitigate this problem to some degree, but they usually



(a)




    (b)       (c)           Fig. 1. Simple planning task used to demonstrate the potential advantage
of cubic spline optimization. See text for details.

involve a slow search process, and still typically do not take
into account the final cubic spline form of the trajectory.
The basic insight of the method we present in this paper
is that if we initially parametrize the trajectory as a cubic
spline, then in many cases we can accomplish both theplanning and trajectory fitting simultaneously. That is, we candirectly optimize the location of the cubic spline waypoints
while obeying many of the same constraints (or approximations thereof) required by a typical planning algorithm.
Specifically, in this paper we show how to plan smoothtask-space trajectories — that is, trajectories where we careprimarily about the position of the robot’s end effector —while maintaining kinematic feasibility, avoiding collision,and limiting velocities or accelerations, all via a convex
optimization problem. Convex optimization problems are
beneficial in that they allow for efficiently finding global
optimums [4] — this allows us to solve the planning tasks ina few milliseconds using off-the-shelf software, suitable forreal-time re-planning and control. This approach is illustrated
in Figure 1 (c). Because the waypoints and cubic splineand optimized simultaneously, the resulting trajectories are
typically much smoother than those obtained by the twophase approach.
We implement this proposed algorithm on a quadruped,
the “Little Dog” robot, to demonstrate its usefulness; indeed,
the task of planning foot and body trajectories for thisrobot originally motivated our approach. The cubic spline
optimization approach that we present in this paper is a
crucial element of our complete system on this robot, andas we demonstrate, the method substantially improves thequality of trajectories.
The remainder of this paper is organized as follows. In
Section II we review the standard means of fitting cubic
splines to a set of waypoints. Section III contains the chiefalgorithmic contribution of the paper: here we present our
method for optimizing cubic spline trajectories using convex
programming. In Section IV we discuss the application ofthis algorithm to a quadruped robot, and present empiricalevaluations. Finally, in Section V we discuss related work,and conclude the paper in Section VI.
II. C UBIC S PLINES
Here we review the standard methods for fitting cubic
splines to a series of waypoints output by a planner. We
suppose that we have access to a high-level planner that plansa feasible path from a start location to a goal location. This
path is represented as a series of T + 1 desired time-location
pairs:(t0 , x⋆0 ), (t1 , x⋆1 ), . . . , (tT , x⋆T )(1)where x⋆i ∈ Rn denotes the desired location of the robot attime ti ∈ R, specified in task space.
Given these waypoints, there is a unique piecewise-cubic
trajectory that passes through the points and satisfies certain
smoothness criteria. Specifically, we model the trajectory
between times ti and ti+1 , denoted xi (t) : R → Rn , asa cubic functionxi (t) = ai + bi (t − ti ) + ci (t − ti )2 + di (t − ti )3(2)

where ai , bi , ci , di ∈ Rn are parameters of the cubic spline.
The final trajectory x(t) : R → Rn is a piecewise-cubic
function that is simply the concatenation of these different
cubic trajectories

if t0 ≤ t < t1

 x0 (t).
.
(3)x(t) =.

xT −1 (t) if tT −1 ≤ t ≤ tT

where we can assume that the trajectory is undefined fort < t0 and t > tT .
Given the desired waypoints, there exists a unique set of
coefficients {ai , bi , ci , di }i=0,...,T −1 such that the resulting
trajectory passes through the waypoints and has continuous velocity and acceleration profiles at each waypoint.1
To compute these coefficients, we first define the matrices
x, a, b, c, d ∈ RT +1×n ⋆Tx0 x⋆1 · · · x⋆T
x =
(4)

T
a0 a1 · · · aT
(5)a =with b, c, and d defined similarly (we define T + 1 sets ofparameters in order to simplify the equations, though we will
1 Technically, in order to ensure uniqueness of the spline we also need to
impose a constraint on the velocity or acceleration of the endpoints, but we
ignore this for the time being.

ultimately only use the 0, . . . T − 1 parameters, as describedabove). Given the x matrix, we can find the parameters of
the cubic splines using the following set of linear equations
a
H1 b
c
d

=
=
=
=

x
H2 x
H3 x + H4 b
H5 x + H6 b

(6)
(7)
(8)
(9)

where the Hi ∈ R(T +1)×(T +1) matrices depend only (nonlinearly) on the times t0 , . . . , tT . We present the complete
definition of these matrices, as well as a derivation of theequations, in Appendix A. However, the important point togleam from this presentation is that the the parameters of the
cubic splines are linear in the desired locations x.
III. C UBIC S PLINE O PTIMIZATIONIn this section we present our primarly algorithmic contribution: a method for optimizing task-space cubic spline
trajectories using convex programming. As before, we assume that we are given an initial plan, now denoted(t0 , x̂0 ), (t1 , x̂1 ), . . . , (tT , x̂T ).

(10)

However, unlike the previous section, we will not require that
our final cubic trajectory pass through these points. Indeed,most of the real planning is performed by the optimization
problem itself, and the initial plan is required only for someof the approximate constraints that we will discuss shortly;
an initial “plan” could simply be a straight line from the startlocation to the goal location.
The task of optimizing the location of the waypoints while
obeying certain constraints can be written formally as
min

f (x)

subject tox∈Cx

where x is the optimization variable, representing the location of the waypoints, f : R(T +1)×n → R is the optimization
objective, and C represent the set of constraints on the
waypoints. In the subsequent sections, we discuss several
possible constraints and objectives that we use in order toensure that the resulting trajectories are both feasible andsmooth. The following is not meant to be an exhaustive list,
but conveys a general idea of what can be accomplished inthis framework. We will present more concrete examples ofsuch optimization problems when we discuss the quadruped
robot in Section IV.
A. Additional Variables and Constraints
Spline derivatives at the waypoints. Often times wewant objective and constraint terms that contain not only the
position of the waypoints, but also the velocity, acceleration,
and/or jerk (derivative of acceleration) of the resulting cubic
spline. Using (6) – (9), these terms are linear functionsof the desired positions, and can therefore be included in
the optimization problem while maintaining convexity. For
instance, since ẋi (ti ) = bi , we can add ẋ variables that
correspond to the velocity at each waypoint by introducing
the constraint ẋ = b — or rather, since to actually include     

 

(a)



(b)

Fig. 2. Illustration of kinematic feasibility constraints. (a) Kinematicallyfeasible region for q1 ∈ [−π/2, π/2], q2 ∈ [−2.6, 2.6]. (b) Convex subsetof the feasible region.

the b variables in the optimization problem, we can justdirectly include the constraint
H1 ẋ = H2 x.
Adding variables in this manner allows us to include objectives and constraints that depend on the velocity terms,
and the same procedure can be used to create variables
representing the acceleration or jerk at each waypoint.
Spline position and derivatives at arbitrary times. Often
times we want to constrain the position, velocity, etc, of the
splines not only at the waypoints, but also at the intermediatetimes. Using the cubic spline formulation, such variables arealso a linear function of the waypoint locations. For example,
suppose we wanted to add a variable x(t′ ) representing theposition of the trajectory at time ti < t′ < ti+1 . Using
equations (2) and (3),x(t′ ) = ai + bi (t′ − ti ) + ci (t′ − ti )2 + di (t′ − ti )3 .
But from (6)–(9), ai , bi , ci and di are all linear in the desired
positions x, so the variable x(t′ ) is also linear in thesevariables. The same argument applies to adding additionalvariables that represent the velocity, acceleration, or jerk at
any time.2 Space constraints prohibit providing the complete
definition of the following matrices, but it should be clear
from the discussion above that if use use x′ ∈ RN ×n to
denote the spline positions at a variety of intermediate times,
we can solve for these positions via a linear system
x′ = G1 x + G2 ẋ
and similarly for other derivatives.
Kinematic feasibility constraints. Since we are specifically focused on planning trajectories in task-space, a key
requirement is that points on the spline must be kinematicallyfeasible for the robot. While the kinematic feasibility regionof an articulated body with joint stops is not typically aconvex set, we can usually find a suitable subset of thiskinematic region that is convex.
For example, consider the double pendulum shown in
Figure 2. The kinematically feasible region, when joint one is
2 In theory, if we wanted to ensure that the entire spline obeys a position
or derivative constraint, we would have to add an infinite number of suchvariables. However, as we will show, in practice we can obtain goodresults by introducing a very small number of additional variables, greatly
increasing the practicality of the approach. This same consideration applies
to the kinematic feasibility and collision constraints.

      
 

(a)

(b)

(c)

Fig. 3. Illustration of collision constraints. (a) Initial (infeasible) plan. (b)Height constraints imposed to avoid collision with obstacle. (c) Resulting
optimized cubic spline trajectory.

restricted to the range [−π/2, π/2] and joint two is restricted
to the range [−2.6, 2.6], is show in Figure 2 (a). Although thisregion is not convex, we can easily find a convex subset, suchas the region shown in Figure 2 (b). This convex subset canbe expressed as the intersection of a sphere and a half-plane,and so we can efficiently constrain the spline waypoints (orspline positions at intermediate times) to lie within this set,
while maintaining convexity of the optimization problem.
Collision constraints. Although general collision constraints can be quite difficult to handle in our framework,in many simple cases we can approximate such constraints
using a simple method shown in Figure 3. Here Figure 3(a) shows the initial plan used by the cubic spline optimizer(recall from above such a plan need to be feasible). Two waypoints on this initial trajectory violate the collision constraint,
so we simply add the constraint, at each of these times, thatthe resulting waypoint must lie above the the obstacle bysome margin; Figure 3 (b) shows this constraint, and Figure
3 (c) shows the resulting trajectory. This technique is anapproximation, because 1) it only constrains the end-effector
position and still could lead to a collision with the articulatedbody, 2) it assumes that the x-position of the waypoints after
optimization doesn’t change, when in fact it can and 3) as
mentioned in Footnote 2, these collision constraints are only
imposed at a finite number of points, so we have to insurethat the “resolution” of these points is smaller than any thinobstacles. Nonetheless, as we show in the subsequent section,
this simple approximation works quite well in practice, andallows us to maintain convexity of the optimization problem.
Finally, we note that for some planning problems, adding
enough constraints of any of the preceeding types can leadto an infeasible optimization problem. We mention thisprimarily to emphasize that this approach is not suited to
all planning situations; if plans must traverse through nonconvex, narrow “corridors” in the robot’s configuration space,then slower, traditional motion planning algorithms may be
the only possible approach. However, for situations where
our method can be applied, such as the LittleDog planningtasks we describe below, our method can produce highlyoptimized trajectories extremely quickly.
B. Optimization Objectives
Given the variables and constraints described above, we
lastly need to define our final optimization objective. While
we have experimented with several different possible optimization objectives, one that appears to work quite well is topenalize the squared velocities at the waypoints and at a few



Fig. 6.

Fig. 4. The LittleDog robot, designed and built by Boston Dynamics, Inc.

Fig. 5.

Planning task, taking two steps over rough terrain.

intermediate points between each waypoint. More formally,
we use the optimization objectiveTf (ẋ, ẋ′ ) = tr ẋT ẋ + tr ẋ′ ẋ′
where ẋ represents the velocity at each waypoint and ẋ′represents the velocities at the midpoint between each waypoint. This objective has the effect of discouraging verylarge velocities at any of the spline points, which leads totrajectories that travel minimal distances while keeping fairlysmooth. However, while this objective works well for our
settings, there are many other possible objective functions
that might function better in other cases such as minimizing
the maximum velocity, average or maximum acceleration,
average distance between spline points, or (using approximations based on the Jacobians along the initial trajectory)
average or maximum joint velocities or torques.
One objective that cannot be easily minimized is thetotal time of the trajectory. Recall that while the position,
velocity, etc terms all linearly related by equations (6)–(9),these equations also involve non-linear, non-convex functionsof the times. However, there has been previous work inapproximately optimizing the times of cubic splines [5], [6],[7], and if the total time of the trajectory is ultimately themost important objective, these techniques can be applied.
IV. A PPLICATION TO A Q UADRUPED ROBOTIn this Section we present the primary empirical result of
this paper: the successful application of these methods toa quadruped, the “LittleDog” robot, shown in Figure 4. In
particular, we use cubic spline optimization to plan foot andbody trajectories for statically stable locomotion over roughterrain. The system we present here is used in our software

Foot planning task, and initial trajectory.

architecture for the quadruped, submitted to DARPA for the
Learning Locomotion project. Due to space constraints weonly give a brief description of the quadruped task in general;
we refer the reader to [8] for a more complete descriptionof our system, excluding the cubic spline optimization.
The basic planning task that we consider in this paper
is as follows: given a current position of the robot, andtwo upcoming footsteps plan trajectories for the feet androbot center of gravity (COG) that achieve these footsteps;
Figure 5 illustrates this planning task. Successfully achieving the footsteps requires that the trajectories obey several
constraints, such as ensuring that the COG and foot locations
are jointly kinematically feasible and collision free, andensuring that the robot is statically stable as it moves its
feet. In the subsequent section, we show how to apply thetechniques from the previous section to plan the foot and
COG trajectories respectively.
A. Planning Foot Trajectories
The chief aim in planning foot trajectories is to achievesmooth motions that avoid collision with the terrain. An
illustration of this task is shown in Figure 6, along with
the initial plan we supply to the cubic spline optimization.
The initial plan is a simple trapezoid, with three waypoints
allocated for the upward and downward “ramps”, and theremaining waypoints in a line, spaced in 2cm intervals. We
use the following optimization problem to plan the foottrajectories:
minx,x′ ,ẋ,ẋ′ ,ẍTtr ẋT ẋ + tr ẋ′ ẋ′

subject to H1 ẋ = H2 x
1ẍ = (H3 x + H4 ẋ)2x′ = G1 x + G2 ẋ
ẋ′ = G3 x + G4 ẋ
x0,: = x̂0 , xT,: = x̂T , ẋ0,: = 0, ẋT,: = 0ẍt,x ≥ 0, ẍt,y ≥ 0, t = 0, 1ẍt,x ≤ 0, ẍt,y ≤ 0, t = T − 1, Tẍt,x ≥ ẍt+1,xt = 2, . . . , T − 3ẍt,y ≥ ẍt+1,yẍt,z < 0, t = 2, . . . , T − 2(xt,x − xt+2,x )2 + (xt,y − xt+2,y )2≤ (3cm)2 , t = 0, T − 2′
x i,z ≥ x̂z (t′i ) + 2cm, i = 1, . . . , N
xt,z ≤ max x̂z (t′i ), t = 1, . . . T.
i=1,...,N

(11)
(12)
(13)
(14)
(15)
(16)
(17)
(18)
(19)
(20)
(21)
(22)
(23)



       






         

(a)

(b)

Fig. 7.

(a) COG planning task. (b) Initial trajectory for the COG plan.

While there are many different terms in this optimization
problem, the overall idea is straightforward. The optimization
objective (11) is the squared velocity objective we discussed
earlier; constraints (12)–(15) are the standard cubic spline
equations for adding additional variables representing respectively the velocity at the waypoints, the acceleration
at the waypoints, additional position terms, and additional
velocity terms;3 (16) insures that the spline begins and ends
at the start and goal, with zero velocity; (17) and (18)ensure that the x, y accelerations are positive (negative)at the start (end) ramp, which in turn ensures that the
trajectory will never overshoot the start and end locations;4(19) extends the previous constraint slightly to also ensurethat the x, y accelerations during the main trajectory portion
are monotonic; (20) forces the z accelerations during themain portion of the trajectory to be negative, which ensuresthat the spline moves over any obstacles in one single arch;(21) ensures that the last waypoints in the ramp don’t deviate
from the start and end positions by more than 3cm; finally,(22) and (23) ensure that the z position of the spline is 2cm
above any obstacle, and that no waypoint is more than 2cm
higher than the tallest obstacle.
Note that this precise optimization problem is not meant tobe a general purpose algorithm. This particular objective andthe constraints were developed specifically for the quadrupedfoot trajectory planning task, and many of the constraints
were developed over time in response to specific situations
that caused simpler optimization problems to produced suboptimal plans. As such, the main purpose of describing the
optimization problem explicitly is as a demonstration of thetechniques presented in the previous section, and to give a
concrete example of the method.
B. Planning COG Trajectories
The aim of planning a trajectory for the COG is twofold:maintaining stability of the robot while allowing the feet toreach their targets. This planning task is illustrated in Figure
7 (a), and the initial trajectory supplied to the cubic spline
optimizer is shown in Figure 7 (b). The triangle in Figure 7(b) denotes the double supporting triangle, described more
3 In greater detail, we add four additional velocity terms, in the midpointsof the waypoints on the “ramp” portion of the initial trajectory. We add N
additional position terms, one at each 1cm interval along the top portion of
the initial trajectory.
4 This constraint and next assume that the x̂
0,x ≤ x̂T,x and x̂0,y ≤ x̂T,y .
In the case that these inequalities are reversed, the corresponding inequalities
in the constraints are also reversed.

fully in [8]; as long as the COG is within this triangle, therobot can take either of the two steps while maintaining
stability. Therefore, the initial plan proceeds in two phases,
first moving into the supporting triangle, and then moving
forward in the triangle while taking the two steps. Although
not shown entirely in Figure 7 (b) for the sake of clarity,
we always use nine waypoints in the COG trajectory splines:
three to move the COG into the supporting triangle, and three
for each foot movement. Since planning the COG trajectory
requires knowledge of the foot locations, we first use themethod above to plan trajectories for the moving feet, which
we denote xf1 (t) and xf2 (t). We then use the following
optimization problem to plan the COG trajectory:T

tr ẋT ẋ + tr ẋ′ ẋ′P
min +λ 5i=3 feas(xi,: , xf1 (ti ), f1 )x,ẋ,ẋ′P7+λ i=5 feas(xi,: , xf2 (ti ), f2 )subject to H1 ẋ = H2 xẋ′ = G3 x + G4 ẋ
x0,: = x̂0 , ẋ0 , x2,: = x̂2 , x8,: = x̂8ẋ0,: = ẋinit
xi,: ∈ S, i = 3, . . . , 7

(24)
(25)
(26)
(27)
(28)
(29)

where
feas(xbody , xfoot , f )denotes the squared distance from xfoot to the kinematicallyfeasible region of foot f , given that the COG is positioned
at point xbody , and where S denotes the support triangle.
Intuitively, the optimization objective (24) is a weighted
combination of the kinematic infeasibility of the moving
feet plus the velocity terms we discussed earlier — inpractice, we choose λ = 100 to try to make the system as
close to kinematically feasible as possible, and only later
try to minimize the velocities; constraints (25) and (26)are again the standard cubic spline equations for velocity
terms — here we add eight additional velocity terms, one at
the midpoint of each two waypoints; (27) ensures that the
trajectory begins, enters the supporting triangle, and ends at
the initially specified waypoints, while (28) ensures that theinitial velocity of the spline is equal to the COG’s current
velocity; finally, (29) ensures that the COG waypoints will
be inside the supporting triangle during the times that thefeet are moving.
C. Experimental Results
We begin with a qualitative look at the trajectories generated by this method. Figure 8 shows a typical footstep
trajectory generated by this method. As we can see, thetrajectory moves the foot from its initial location to thedesired location in one fluid motion, stepping high enoughto avoid any obstacles. Likewise, Figure 9 shows a typically
COG trajectory generated by the algorithm. Notice that the
trajectory inside the supporting triangle is not just a straight
line: the algorithm adjusts the trajectory in this manner tomaximize the kinematic feasibility of the moving feet.
Of course, while examining the splines in this manner canhelp give an intuition about kind of trajectories generated

0.06
0.05
0.04
0.03

z (m)

0.02
0.01
0

TerrainDesired Trajectory−0.01

Trajectory Waypoints

−0.02
−0.03
−0.04
0

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

0.18

xy (m)

x (m)

0.6
0.5
0.4
0.3
0.1

0.2

0.3

0.4

0.5

0.6

0

0.1

0.2

0.3

0.4

0.5

0.6

0

0.1

0.2

0.3

0.4

0.5

0.6

y (m)

−0.06
−0.08

Fig. 10. Terrains used for evaluating the system. The top terrain is fairly
trivial to navigate, while the bottom terrain is significantly more challenging.

−0.1
−0.12

z (m)

0.1

0.05

0

Time (s)Fig. 8. Typical example of a foot trajectory generated by the algorithm.
The top figure shows the resulting trajectory in 3D space, while the bottom
shows each component as a function of time.

0

Desired Trajectory
Trajectory Waypoints−0.02

Support Triangle

y (m)

−0.04

−0.06

−0.08

−0.1

−0.12
−0.02

0

0.02

0.04

0.06

0.08

0.1

0.12

x (m)

x(m)

0.1

0.05

0

0

0.2

0.4

0.6

0.8

1

1.2

1.4

0

0.2

0.4

0.6

0.8

1

1.2

1.4

0

0.2

0.4

0.6

0.8

1

1.2

1.4

y (m)

0
−0.02
−0.04
−0.06

z (m)

0.04
0.03
0.02
0.01
0

Time (s)Fig. 9.

Typical example of a COG trajectory generated by the algorithm.

by our algorithm, we are ultimately interested in whether
or not the method actually improves performance on the
LittleDog. To evaluate this, we tested the system on two
terrains of varying difficulty, shown in Figure 10. We compare the cubic spline optimization approach to the trajectory
planning method described in [8], which uses simple boxshapes to step over terrain, and linear splines for moving the
COG. While, as mentioned previously, the entire system forplanning and control of the quadruped is quite complex, allother elements of the system remain fixed in the experiments
here.
Table I shows the performance of the quadruped bothwith and without the cubic spline optimization. We ran 10

trials on each of the terrains, and evaluated the systems
using 1) fraction of successful runs, 2) speed over terrain,3) average number of “recoveries”5 , and 4) average tracking
error (i.e., distance between the planned and actual location)
for the moving foot. Perhaps the most obvious benefit of
the cubic spline optimization method is that the resulting
speeds are faster; this is not particularly surprising, since
the splines output by our planner will clearly be more
efficient than a simple box pattern over obstacles. However,equally important is that the cubic spline optimization also
leads to more robust behavior, especially on the challenging
terrain: the previous method only succeeds in crossing theterrain 70% of the time, and even when it does succeed
it typically needs to execute several recoveries, whereas
the cubic spline optimization method crosses the terrain in
all cases, and executes much fewer recoveries. We surmisethat this is because the cubic spline method attempts tomaintain kinematic feasibility of the moving foot at all times.
This is seen in Table I from the fact that the cubic spline
optimization approach has lower tracker error, implying moreaccurate placement of feet, and therefore greater robustness
is challenging environments.
Finally, we want to briefly mention issues of running
time. We solve the convex optimization problems using
MOSEK, a general purpose convex solver, available at
http://www.mosek.com. Using this software, solving
for a foot spline takes an average of 4.4 milliseconds (with
1.1 millisecond standard deviation), while solving for the
COG path takes an average of 5.4 ms (0.4 ms standard
deviation). These quantities remain approximately the same
regardless of the terrain, and are fast enough to allow forreal-time re-planning and control on the quadruped.
V. D ISCUSSION AND R ELATED W ORK
As mentioned in the introduction, cubic splines are very
common in robotic applications [1]. This paper, as well as[5], [6], [7] discuss methods for approximately optimizing
the time of the trajectory, but do not consider optimizing
the waypoints themselves. The work of Schlemmer et al.
5 A recovery is required whenever the COG falls outside the supporting
triangle. Again, see [8] for details.

Metric% Successful Trials
Speed (cm/sec)
Avg. Tracking Error (cm)Avg. # Recoveries

Easy Terrain
Spline Optimization
Previous Method
100%
100%
7.02 ± 0.10
5.99 ± 0.07
1.28 ± 0.03
1.40 ± 0.06
0.0
0.0

Challenging Terrain
Spline Optimization
Previous Method
100%
70 %
6.30 ± 0.12
5.59 ± 0.31
1.27 ± 0.05
1.55 ± 0.09
0.5 ± 0.37
2.22 ± 1.45

TABLE IP ERFORMANCE OF CUBIC SPLINE OPTIMIZATION ON THE TWO QUADRUPED TERRAINS . T ERMS INCLUDE 95% CONFIDENCE INTERVALS .

[9] perhaps bears the most resemblance to our own, as they
do consider the modification the cubic spline parameters
themselves. However, the resulting techniques are quite
different, since they break the trajectories into multiple
segments with constant acceleration, not the continuous
acceleration profiles that we consider here; furthermore, they
focus on splines in joint space, whereas the most interesting
constraints (namely the kinematic feasibility and collision
constraints) in our setting are precisely those that arise dueto the task-space planning.
Free-knot splines [10], [11], typically used in the statistics
literature, also relate to our method, as they involve choosing
both the waypoints and times of parametric splines, including
cubic splines. However, these methods typically focus on
fitting splines to data, not creating splines that relate to
any kind of trajectory planning, so the actual optimization
approaches are very different.
Our method also relates to the trajectory optimization
literature in robotics [12], [13], [14], [15], though these methods typically focus on improving a pre-supplied (feasible)
trajectory via gradient or higher-order methods. Although inthis paper we focus on cases where the initial “plan” supplied
to the cubic spline optimization problem is very simple andpossibly infeasible, there is nothing that prevents the methodfrom being used to optimize a feasible trajectory generated
by a planner, as in the approaches above: in this setting thealgorithm could also function as a trajectory optimization
approach, specifically using a cubic spline parametrizationof the trajectory, and using general convex optimization
techniques rather than gradient methods alone.
Planning trajectories for the quadrupeds robot in particular
has been considered in [8], [16], [17]. Although a full comparison between the different approaches is beyond the scopeof this paper, we did compare to our previous controller, [8]and demonstrated that cubic spline optimization improves
performance. The work by Mistry et al, [18] also considers
a quadruped and specifically looks at the trade-off between
balance and kinematic feasibility, but this work is generally
orthogonal to our own, as it primarily considers control
strategies for achieving a given trajectory.
VI. C ONCLUSION
In this paper we presented a cubic spline optimization
method, for planning smooth trajectories for robot motion.
The algorithm uses convex optimization methods to efficiently plan task-space trajectories, while obeying several
constraints, such as kinematic feasibility and avoiding contact. We apply this method to the task of planning trajectories
for a quadruped robot, and demonstrate significant improvement over past work.

R EFERENCES[1] C.-S. Lin, P.-R. Chang, and J. Luh, “Formulation and optimizationof cubic polynomial joint trajectories for industrial robots,” IEEE
Transactions on Automatic Control, vol. 28, no. 12, pp. 1066–1074,
1983.
[2] S. M. LaValle, Planning Algorithms. Cambridge University Press,
2006.
[3] J. Betts, “Survery of numerical methods for trajectory optimization,”
Journal of Guidance, Control, and Dynamics, vol. 21, no. 2, pp. 193–
207, 1998.
[4] S. Boyd and L. Vandenberghe, Convex Optimization. Cambridge
University Press, 2004.
[5] B. Cao, G. Dodds, and G. Irwin, “Constrained time-efficient smoothcubic spline trajectory generation for industrial robots,” in Proceedingsof the IEE Conference on Control Theory and Applications, 1997.
[6] J.-H. Park, H.-S. Kim, and Y.-K. Choi, “Trajectory optimization andcontrol for robot manipulator using evolution strategy and fuzzy logic,”in Proceedings of the IEEE International Conference on Systems, Man,
and Cybernetics, 1997.
[7] A. I. F. Vaz and E. M. Fernandes, “Tools for robotic trajectory planning using cubic splines and semi-infinite programming,” in Recent
Advances in Optimization, A. Seeger, Ed. Springer, 2006, pp. 399–
413.
[8] J. Z. Kolter, M. P. Rodgers, and A. Y. Ng, “A control architecture forquadruped locomotion over rough terrain,” in Proceedings of the IEEE
International Conference on Robotics and Automation, 2008.
[9] M. Schlemmer, R. Finsterwalder, and G. Grubel, “Dynamic trajectory
optimization in real time for moving obstacles avoidance by a ten
degrees of freedom manipulator,” in International Conference onIntelligent Robots and Systems, 1995.
[10] C. de Boor, A Practical Guide to Splines. Springer-Verlag, 1978.
[11] D. Jupp, “Approximations to data by splines with free knots,” SIAM
Journal of Numerical Analysis, vol. 15, pp. 328–343, 1978.
[12] D. W. Miles and S. M. Rock, “Real-time dynamic trajectory optimization,” in Proceedings of the AIAA Guidance, Navigation and Control
Conference, 1996.
[13] S. Fleury, P. Soueres, J. Laumound, and R. Chatila, “Primitives forsmoother mobile robot trajectories,” IEEE Transactions on Robotics
and Automation, vol. 11, no. 3, pp. 441–448, 1995.
[14] F. Lamiraux, D. Bonnafous, and O. Lefebvre, “Reactive path deformation for non-holonomic mobile robots,” IEEE Transactions on
Robotics, vol. 20, no. 6, pp. 967–977, 2004.
[15] S. Lee, J. Park, F. M. Kim, and J. Bobrow, “Newtow-type algorithms
for dynamics-based robot movement optimization,” IEEE Transactions
on Robotics, vol. 21, no. 4, pp. 657–667, 2005.
[16] D. Pongas, M. Mistry, and S. Schaal, “A robust quadruped walking gaitfor traversing rough terrain,” in Proceedings of the IEEE International
Conference on Robotics and Automation, 2007.
[17] J. R. Rebula, P. D. Neuhaus, B. V. Bonnlander, M. J. Johnson, andJ. E. Pratt, “A controller for the littledog quadruped walking on roughterrain,” in Proceedings of the IEEE International Conference on
Robotics and Automation, 2007.
[18] M. Mistry, J. Nakanishi, and S. Schaal, “Take space control withprioritization for balance and locomotion,” in Proceedings of the IEEE
International Conference on Robotics and Automation, 2007.
[19] R. H. Bartels, J. C. Beatty, and B. A. Barsky, An Introduction to Splines
for Use in Computer Graphics and Geometric Modelling. Morgan
Kaufmann, 1989.

A PPENDIX
A. Derivation of the Cubic Spline Equations
In this section we derive the standard equations for cubic
splines, presented in Section II. In particular, we show that

given desired locations and final and initial velocities ofthe spline, we can solve for the cubic spline parameters
using the linear equations (6)–(9). We reiterate that this is astandard derivation, but we include it for completeness andto solidify our notation. The derivation follows that in [19],but generalizes it to the case of arbitrary times for the spline
points.
Recall that the trajectory from time ti to ti+1 is given by
the cubic function
2

3

xi (t) = ai + bi (t − ti ) + ci (t − ti ) + di (t − ti )(30)

In order to achieve a smoother, continuous trajectory, we
impose four conditions on the spline: 1) at ti it must havethe value x⋆i , 2) at ti+1 it must have the value x⋆i+1 , 3) its
derivative (velocity) must be continuous at ti+1 , and 4) its
second derivative (acceleration) must be continuous at ti+1 .
These four conditions can be written formally as
x⋆i
x⋆i+1

xi (ti )xi (ti+1 )ẋi (ti+1 )ẍi (ti+1 )=== ẋi+1 (ti+1 )= ẍi+1 (ti+1 ).

=
=
=
=

2ci + 6di ∆ti

∆t0

 0

 0

H4 =  .
 ..

 0
0
 2

∆t30

 0


 0
H5 = 
 ..
 .

 0
0
 1

∆t20

 0

 0

H6 = 
 ..
 .

 0

−1
∆t0
−2
∆t1

0
..
.
0
0
−2
∆t30
2
∆t31

0
..
.
0
0
1
∆t20
1
∆t21

0
..
.
0

0

Using the cubic function definition (30), these conditions canbe rewritten in terms of the cubic spline parameters:ai
ai + bi ∆ti + ci ∆t2i + di ∆t3i
bi + 2ci ∆ti + 3di ∆t2i

 −2

x⋆i
x⋆i+1
bi+1
2ci+1

(31)
(32)
(33)
(34)

where

0

a = x.
Next, we use equations (32) and (33), to solve for ci and diin terms of the xi ’s and bi ’s, giving:3(x⋆i+1 − x⋆i ) − (2bi + bi+1 )∆ti
(35)
ci =
∆t2i
2(x⋆i − x⋆i+1 ) + (bi + bi+1 )∆ti
.
(36)
di =
∆t3i
This leads immediately to the third and fourth linear equations (8) and (9)

where
 0

 0

H3 = 
 ..
 .

 0
0

0
..
.
0
0

0

..
.
0

···
···
···
..
.
···

0

···

3
∆t21
−3
∆t22

−2
∆t31
2
∆t2

..
.
0
0

0

−2
∆tT −1−1∆tT −1

0
0
0
..
.

0
0
0
..
.

2
∆tT −1−2∆tT −1

0
0
0
..
.

0
0
0
..
.

1
∆t2T −1

1∆t2T −1

0

···
···
···
..
.
···
···

..
.
0

···
···
···
..
.
···

0

···

1
∆t21
1
∆t22

0
0
0
..
.



0

0

0

(38)



0

0



















(39)







.





(40)

These equations, along with the additional constraints thatcan be expressed as the second set of linear equalities (7):

0
0
0
..
.

0
0
0
..
.

−3
∆t2T −13∆t2T −10

where
0 0 0 0α0 β0 α1 0
 0 α1 β1 α2 0 0 α2 β2H1 = 
..
..
..
 ..
.
.
.
 .

0 0 0 0
0 0 0 0
0 0 0 0


αi ≡

0












(37)

···
···
···
···
..
.

0
0
0
0
..
.

0
0
0
0
..
.

0
0
0
0
..
.

· · · βT −3 αT −2· · · αT −2 βT −2
···
0
0








 (42)0 
αT −1 0442
, βi ≡
+
∆ti
∆ti
∆ti+1

and
0
0 0 0
−γ0 η0 γ1 0
 0 −γ1 η1 γ2
0 −γ2 η2
 0
H2 = 
..
..
..
 ..
.
.
.
 .

0 0 0
 0
 0
0 0 0
0
0 0 0


c = H3 x + H4 b
d = H5 x + H6 b
3
∆t20
−3
∆t21

0

0
0
0
..
.

H1 b = H2 x

Now we use simple variable elimination to get find explicit
equations for the cubic spline parameters. Specifically, equation (31) immediately leads to the first linear equation (6)

∆t20

..
.
0
0

···
···
···
..
.
···
···

Finally, substituting (35) and (36) into (34) we obtain
4bi+1 + 2bi+2
2bi + 4bi+1
+
=
∆ti
∆ti+1
(41)
6(x⋆i+1 − x⋆i ) 6(x⋆i+2 − x⋆i+1 )+.
∆t2i∆t2i+1∆ti ≡ ti+1 − ti .

 −3

0
−1
∆t1
−2
∆t2

γi ≡

···
···
···
···
..
.

0
0
0
0
..
.

· · · ηT −3· · · −γT −2
···
0

0
0
0
0
..
.


0
0 

0 

0 
.. 
 (43). 
γT −2 0 

η
γ
T −2

T −1

0

0

6
6
6
, ηi ≡−.
∆t2i∆t2i+1∆t2i