For instructions on how to run these tutorial notebooks, please see the index.
Please refer to the MathematicalProgram Tutorial for constructing and solving a general optimization program in Drake.
A Nonlinear Programming (NLP) problem is a special type of optimization problem. The cost and/or constraints in an NLP are nonlinear functions of decision variables. The mathematical formulation of a general NLP is
$\begin{aligned} \min_x&\; f(x)\\ \text{subject to }& g_i(x)\leq 0 \end{aligned}$
where $f(x)$ is the cost function, and $g_i(x)$ is the i'th constraint.
An NLP is typically solved through gradient-based optimization (like gradient descent, SQP, interior point methods, etc). These methods rely on the gradient of the cost/constraints $\partial f/\partial x, \partial g_i/\partial x$. pydrake can compute the gradient of many functions through automatic differentiation, so very often the user doesn't need to manually provide the gradient.
The user can call AddCost
function to add a nonlinear cost into the program. Note that the user can call AddCost
repeatedly, and the program will evaluate the summation of each individual cost as the total cost.
We can define a cost through a python function, and then add this python function to the objective through AddCost
function. When adding a cost, we should provide the variable associated with that cost, the syntax is AddCost(cost_evaluator, vars=associated_variables)
, which means the cost is evaluated on the associated_variables
.In the code example below, We first demonstrate how to construct an optimization program with 3 decision variables, then we show how to add a cost through a python function.
from pydrake.solvers import MathematicalProgram, Solve
import numpy as np
# Create an empty MathematicalProgram named prog (with no decision variables,
# constraints or costs)
prog = MathematicalProgram()
# Add three decision variables x[0], x[1], x[2]
x = prog.NewContinuousVariables(3, "x")
def cost_fun(z):
cos_z = np.cos(z[0] + z[1])
sin_z = np.sin(z[0] + z[1])
return cos_z**2 + cos_z + sin_z
# Add the cost evaluated with x[0] and x[1].
cost1 = prog.AddCost(cost_fun, vars=[x[0], x[1]])
print(cost1)
Notice that by changing the argument vars
in AddCost
function, we can add the cost to a different set of variables. In the code example below, we use the same python function cost_fun
, but impose this cost on the variable x[0], x[2]
.
cost2 = prog.AddCost(cost_fun, vars=[x[0], x[2]])
print(cost2)
A more compact approach to add a cost is through a lambda function. For example, the code below adds a cost $x[1]^2 + x[0]$ to the optimization program.
# Add a cost x[1]**2 + x[0] using a lambda function.
cost3 = prog.AddCost(lambda z: z[0]**2 + z[1], vars = [x[1], x[0]])
print(cost3)
If we change the associated variables, then it represents a different cost. For example, we can use the same lambda function, but add the cost $x[1]^2 + x[2]$ to the program by changing the argument to vars
cost4 = prog.AddCost(lambda z: z[0]**2 + z[1], vars = x[1:])
print(cost4)
cost4 = prog.AddQuadraticCost(x[0]**2 + 3 * x[1]**2 + 2*x[0]*x[1] + 2*x[1] * x[0] + 1)
print(cost4)
If the user knows the matrix form of Q
and b
, then it is faster to pass in these matrices to AddQuadraticCost
, instead of using the symbolic quadratic expression as above.
# Add a cost x[0]**2 + 2*x[1]**2 + x[0]*x[1] + 3*x[1] + 1.
cost5 = prog.AddQuadraticCost(
Q=np.array([[2., 1], [1., 4.]]),
b=np.array([0., 3.]),
c=1.,
vars=x[:2])
print(cost5)
This function adds a cost of the form $(x - x_{des})^TQ(x-x_{des})$.
cost6 = prog.AddQuadraticErrorCost(
Q=np.array([[1, 0.5], [0.5, 1]]),
x_desired=np.array([1., 2.]),
vars=x[1:])
print(cost6)
This function adds a quadratic cost of the form $(Ax-b)^T(Ax-b)$
# Add the L2 norm cost on (A*x[:2] - b).dot(A*x[:2]-b)
cost7 = prog.Add2NormSquaredCost(
A=np.array([[1., 2.], [2., 3], [3., 4]]),
b=np.array([2, 3, 1.]),
vars=x[:2])
print(cost7)
Drake supports adding constraints in the following form \begin{aligned} lower \leq g(x) \leq upper \end{aligned} where $g(x)$ returns a numpy vector.
The user can call AddConstraint(g, lower, upper, vars=x)
to add the constraint. Here g
must be a python function (or a lambda function).
## Define a python function to add the constraint x[0]**2 + 2x[1]<=1, -0.5<=sin(x[1])<=0.5
def constraint_evaluator1(z):
return np.array([z[0]**2+2*z[1], np.sin(z[1])])
constraint1 = prog.AddConstraint(
constraint_evaluator1,
lb=np.array([-np.inf, -0.5]),
ub=np.array([1., 0.5]),
vars=x[:2])
print(constraint1)
# Add another constraint using lambda function.
constraint2 = prog.AddConstraint(
lambda z: np.array([z[0]*z[1]]),
lb=[0.],
ub=[1.],
vars=[x[2]])
print(constraint2)
Once all the constraints and costs are added to the program, we can call the Solve
function to solve the program and call GetSolution
to obtain the results. Solving an NLP requires an initial guess on all the decision variables. If the user doesn't specify an initial guess, we will use a zero vector by default.
## Solve a simple nonlinear
# min -x0
# subject to x1 - exp(x0) >= 0
# x2 - exp(x1) >= 0
# 0 <= x0 <= 100
# 0 <= x1 <= 100
# 0 <= x2 <= 10
prog = MathematicalProgram()
x = prog.NewContinuousVariables(3)
# The cost is a linear function, so we call AddLinearCost
prog.AddLinearCost(-x[0])
# Now add the constraint x1-exp(x0)>=0 and x2-exp(x1)>=0
prog.AddConstraint(
lambda z: np.array([z[1]-np.exp(z[0]), z[2]-np.exp(z[1])]),
lb=[0, 0],
ub=[np.inf, np.inf],
vars=x)
# Add the bounding box constraint 0<=x0<=100, 0<=x1<=100, 0<=x2<=10
prog.AddBoundingBoxConstraint(0, 100, x[:2])
prog.AddBoundingBoxConstraint(0, 10, x[2])
# Now solve the program with initial guess x=[1, 2, 3]
result = Solve(prog, np.array([1.,2.,3.]))
print(f"Is optimization successful? {result.is_success()}")
print(f"Solution to x: {result.GetSolution(x)}")
print(f"optimal cost: {result.get_optimal_cost()}")
Some NLPs might have many decision variables. In order to set the initial guess for these decision variables, we provide a function SetInitialGuess
to set the initial guess of a subset of decision variables. For example, in the problem below, we want to find the two closest points $p_1$ and $p_2$, where $p_1$ is on the unit circle, and $p_2$ is on the curve $y=x^2$, we can set the initial guess for these two variables separately by calling SetInitialGuess
.
import matplotlib.pyplot as plt
prog = MathematicalProgram()
p1 = prog.NewContinuousVariables(2, "p1")
p2 = prog.NewContinuousVariables(2, "p2")
# Add the constraint that p1 is on the unit circle centered at (0, 2)
prog.AddConstraint(
lambda z: [z[0]**2 + (z[1]-2)**2],
lb=np.array([1.]),
ub=np.array([1.]),
vars=p1)
# Add the constraint that p2 is on the curve y=x*x
prog.AddConstraint(
lambda z: [z[1] - z[0]**2],
lb=[0.],
ub=[0.],
vars=p2)
# Add the cost on the distance between p1 and p2
prog.AddQuadraticCost((p1-p2).dot(p1-p2))
# Set the value of p1 in initial guess to be [0, 1]
prog.SetInitialGuess(p1, [0., 1.])
# Set the value of p2 in initial guess to be [1, 1]
prog.SetInitialGuess(p2, [1., 1.])
# Now solve the program
result = Solve(prog)
print(f"Is optimization successful? {result.is_success()}")
p1_sol = result.GetSolution(p1)
p2_sol = result.GetSolution(p2)
print(f"solution to p1 {p1_sol}")
print(f"solution to p2 {p2_sol}")
print(f"optimal cost {result.get_optimal_cost()}")
# Plot the solution.
plt.figure()
plt.plot(np.cos(np.linspace(0, 2*np.pi, 100)), 2+np.sin(np.linspace(0, 2*np.pi, 100)))
plt.plot(np.linspace(-2, 2, 100), np.power(np.linspace(-2, 2, 100), 2))
plt.plot(p1_sol[0], p1_sol[1], '*')
plt.plot(p2_sol[0], p2_sol[1], '*')
plt.axis('equal')
plt.show()