trajOptLib package

Subpackages

• trajOptLib.io package
  • Submodules
  • trajOptLib.io.argument module
  • trajOptLib.io.fileio module
  • trajOptLib.io.printio module
  • trajOptLib.io.stringio module
  • Module contents
• trajOptLib.plot package
  • Submodules
  • trajOptLib.plot.common module
  • trajOptLib.plot.compare module
  • trajOptLib.plot.plot3d module
  • Module contents

Submodules

trajOptLib.classBuilder module

classBuilder.py

This module provides versatile functions that helps user build up classes quickly. Specifically, it allows fast prototype of problems. However, the users have to write functions that are autograd compatible. Most basically, import autograd.numpy instead of numpy

trajOptLib.classBuilder.blockIndex(i, j, rows, cols, order='C')

For a matrix block, we return the index of row and columns.

For a matrix we choose a block using the upper left corner positioned at (i, j) and size (row, col). Each element of the block has row and col index, they are returned in two arrays. The order controls we use row or column major order

For example, blockIndex(1, 3, 2, 3, ‘C’) returns (array([1, 1, 1, 2, 2, 2]), array([3, 4, 5, 3, 4, 5]))

Parameters:

• i (int) – the row of the upper left corner
• j (int) – the column of the upper left corner
• rows (int) – number of rows of the block
• cols (int) – number of columns of the block
• order (char) – (‘C’/’F’) if we return row or column major

class trajOptLib.classBuilder.daeSystemWrapper(fun, nx, nu, np, nf, *args)

Bases: trajOptLib.trajOptBase.daeSystem

This class takes a function and returns a system.

__init__(fun, nx, nu, np, nf, *args)

Constructor for the problem.

Parameters:

• fun (callable) – a function that implements f(t, x, u, p, *args) = 0 but does not depend on t
• nx (int) – length of x
• nu (int) – length of u
• np (int) – length of p
• nf (int) – length of output of f
• args (kwargs) – additional parameters

dyn(t, x, u, p, y, G, row, col, rec, needg)

Override class method

class trajOptLib.classBuilder.nonLinearPointConstrWrapper(fun, nx, nu, np, nc, index, lb=None, ub=None, args=None)

Bases: trajOptLib.trajOptBase.nonLinearPointConstr

This class takes a function and wrap it as a point constraint.

__callg__(X, F, G, row, col, rec, needg)

override a function

__init__(fun, nx, nu, np, nc, index, lb=None, ub=None, args=None)

class trajOptLib.classBuilder.systemWrapper(fun, nx, nu, np, *args)

Bases: trajOptLib.trajOptBase.system

This class takes a function and returns a system.

Jdyn(t, x, u, p)

__init__(fun, nx, nu, np, *args)

Constructor.

Parameters:

• fun (callable) – a function that implements dot{x}=f(t, x, u, p, *args) but does not depend on t
• nx (int) – length of x
• nu (int) – length of u
• np (int) – length of p
• args (kwargs) – additional parameters to function

dyn(t, x, u, p)

trajOptLib.oopInterface module

oopInterface.py

Construct a object oriented interface for problem solving.

class trajOptLib.oopInterface.AbstractSolver(system, N, tf, ip=False, config=None)

Bases: object

A class that can provides abstract methods for a general solver.

__init__(system, N, tf, ip=False, config=None)

Constructor for the problem.

Parameters:

• system (a dae system) –
• N (knot point size) –
• tf (float/array-like, final time) –
• ip (bool, if use interior point solver) –
• config (a configuration object for either snopt or ipopt) –

addLQRObj(*args, **kwargs)

Add a lqr objective.

constructSolver(ip=False)

guessGen(*args, **kwargs)

parseSol(x)

Parse a solution.

preProcess(*args, **kwargs)

setUbound(ulb, uub=None)

Set bounds on control.

If uub is None, we are using -ulb and ulb as bounds

setXbound(xlb, xub)

Set bounds for state variables.

setx0(x0)

Set bounds for initial states.

Parameters:x0 – ndarray, (ndyn*order,), initial state

setxf(xf)

Set bounds for final states.

Parameters:xf – ndarray, (4,) final state

solve(x0=None, *args, **kwargs)

update()

Update bounds on x0 and xf

trajOptLib.trajOptBase module

trajOptBase.py

Classes ready to be used for trajectory optimization.

class trajOptLib.trajOptBase.addX(n, lb=None, ub=None)

Bases: object

A description of additional optimizing parameter.

It is intended to be used if the optimal control has like point constraint. In this class the user has to supply the size and bounds of those variables.

__init__(n, lb=None, ub=None)

Constructor of this class.

Parameters:

• n – int, length of this variable.
• lb – ndarray, (n,) lower bounds for those variables. None means no bound
• ub – ndarray, (n,) uppere bounds for those variables. None means no bound

class trajOptLib.trajOptBase.baseFun(nx, nf, gradmode, ng=None)

Bases: object

Base class for functions, including both objective and constraint.

This function should be inherited to define your own functions. A function with user supplied gradient information, nx, nf, ng should be set.

__callf__(x, F)

Function call with no gradient information.

Parameters:

• x – ndarray, input to the function
• F – ndarray, output of the function which is written inplace

__callg__(x, F, G, row, col, rec, needg)

Function call with no gradient information.

Parameters:

• x – ndarray, input to the function
• F – ndarray, output of the function which is written inplace
• G – ndarray, gradient of the function in sparse form which stores the values
• row – ndarray of int, stores the rows of the sparse gradient matrix
• col – ndarray of int, stores the columns of the sparse gradient matrix
• rec – bool, determine if we write values to row and col
• needg – bool, determine if we need to calculate gradient and write to G

__init__(nx, nf, gradmode, ng=None)

Constructor for base function

Parameters:

• nx – int, number of variables input
• nf – int, number of response output
• gradmode – string, mode of gradient
• ng – int, used only when gradmode == ‘user’, means number of nnz gradients

findTimeGradient(x)

grad = ['user', 'no']

class trajOptLib.trajOptBase.daeSystem(nx, nu, np, nf, nG)

Bases: object

A DAE system.

__init__(nx, nu, np, nf, nG)

Constructor for the problem.

For a dae system described by :math f(t, q, dot{q}, ddot{q}, u, p)=0, nx=3dim(q), nf=dim(q). If it is described by :math f(t, q, dot{q}, u, p)=0, nx=2dim(q), nf=dim(q). We define as order the highest time derivative of state q. Keeping this in mind, nf always equals dim(q), nx = (1+order)dim(q), nDefect = 2*order*nf Compared with system class, system dynamics can be given implicitly. For different order, we are gonna have different number of defect constraints and sizes.

Parameters:

• nx – int, dimension of states, it might also include acceleration
• nu – int, dimension of control
• np – int, dimension of parameter
• nf – int, dimension of dae system
• nG – int, nnz of Jacobian

dyn(t, x, u, p, y, G, row, col, rec, needg)

Implementation of system dynamics expressed in a dae.

It evaluates system dynamics like f(t, q, dq, ddq, p, u) = 0 and calculate gradients if necessary. :param t: float, time of evaluation :param x: ndarray, (nx,) state variable, it might contain q, dq, ddq or in more general case q and dq. :param u: ndarray, (nu,) control variable :param p: ndarray, (np,) parameter used such as reaction force from ground :param y: ndarray, (nq,) this constraint function. It is evaluated here. :param G, row, col: ndarray, (nG,) gradient of this constraints, row and col index :param rec: bool, if we need to write row and col :param needg: bool, if we have to evaluate gradients.

findTimeGradient(catx)

Detect if gradient is time related.

class trajOptLib.trajOptBase.linearConstr(A, lb=None, ub=None)

Bases: object

Class for linear constraints based on the whole x length.

__init__(A, lb=None, ub=None)

class trajOptLib.trajOptBase.linearObj(A)

Bases: object

Class for directly add linear objective function over the entire decision variable.

It serves for objective of form \(y=Ax\) where \(x\) is the collected long vector.

__init__(A)

Constructor for linear objective function using A

Parameters:A – np.ndarray or spmatrix, must of size equal to nsol

class trajOptLib.trajOptBase.linearPointConstr(index, A, lb=None, ub=None)

Bases: trajOptLib.trajOptBase._objectWithMatrix

Class for linear constraint at selected points.

__init__(index, A, lb=None, ub=None)

class trajOptLib.trajOptBase.linearPointObj(index, A, nx, nu, np_)

Bases: trajOptLib.trajOptBase._objectWithMatrix

Class for directly add linear objective function over the entire decision variable.

It serves for objective function of the form \(y=Ax\) where \(x\) is the concatenated vector of state, control and parameter at a selected index.

__init__(index, A, nx, nu, np_)

Constructor for linear objective function using A pointwise

Parameters:

• index – int, at which point is objective function evaluated
• A – np.ndarray or spmatrix, must of size equal to xdim
• nx – int, dimension of state
• nu – int, dimension of control
• np – int, dimension of parameter

class trajOptLib.trajOptBase.lqrObj(F=None, Q=None, R=None, xfbase=None, xbase=None, ubase=None, tfweight=None, P=None, pbase=None)

Bases: object

Class for LQR objective since it is so common. It is treated independently with pathObj.

__init__(F=None, Q=None, R=None, xfbase=None, xbase=None, ubase=None, tfweight=None, P=None, pbase=None)

Constructor for LQR objective function.

\(c=\|x_f-x_{fbase}\|_F + \Sigma (\|x-x_{base}\|_Q + \|u-u_{base}\|_R + \|p-p_{base}\|_P) * h\)

Parameters:

• Q, R (F,) – cost for terminal, path state, path ctrl.
• xbase, ubase (xfbase,) – the basis.
• P – might be None if we do not penalize p
• pbase – might be None if we do not penalize p

class trajOptLib.trajOptBase.nonDiagLQRObj

Bases: object

Class for LQR objective with non-diagonal entries

class trajOptLib.trajOptBase.nonLinearConstr(nsol, nc, lb=None, ub=None, gradmode='user', nG=None)

Bases: trajOptLib.trajOptBase.baseFun

Class for defining constraint function in a general form.

__init__(nsol, nc, lb=None, ub=None, gradmode='user', nG=None)

Constructor for general nonlinear constraint.

Parameters:

• nsol – int, length of the solution vector, used to initialize baseFun
• nc – int, dimension of constraint function
• ub (lb,) – lower and upper bound of the constraint function. None means equal to 0
• gradmode – str, how gradient is provided
• nG – int, number of nnz of Jacobian

class trajOptLib.trajOptBase.nonLinearObj(nsol, gradmode='user', nG=None)

Bases: trajOptLib.trajOptBase.baseFun

Class for general nonlinear objective function over the entire decision variables.

The objective function is basically calculated by calling a nonlinear function.

__init__(nsol, gradmode='user', nG=None)

Constructor for nonlinear objective function.

Parameters:

• nsol – int, length of decision variable
• gradmode – str, how gradient is provided
• nG – int, number of nnz of Jacobian

class trajOptLib.trajOptBase.nonLinearPointConstr(index, nc, nx, nu, np=0, lb=None, ub=None, gradmode='user', nG=None)

Bases: trajOptLib.trajOptBase.baseFun

Class for defining point constraint function.

__init__(index, nc, nx, nu, np=0, lb=None, ub=None, gradmode='user', nG=None)

Constructor for nonlinear point constraint. Also serve as path constraint.

Parameters:

• index – int, at which point is objective calculated
• nc – int, dimension of constraint function
• nu, np (nx,) – int, dimensions
• ub (lb,) – lower and upper bound of the constraint function. None means equal to 0
• gradmode – str, how gradient is provided
• nG – int, number of nnz of Jacobian

class trajOptLib.trajOptBase.nonLinearPointObj(index, nx, nu, np=0, gradmode='user', nG=None)

Bases: trajOptLib.trajOptBase.baseFun

Class for defining point objective function.

Similar to linear case. A function that takes the concatenated vector at a selected index is used.

__init__(index, nx, nu, np=0, gradmode='user', nG=None)

Constructor for nonlinear objective function.

Parameters:

• index – int, at which point is objective calculated
• nu, np (nx,) – int, dimensions
• gradmode – str, how gradient is provided
• nG – int, number of nnz of Jacobian

class trajOptLib.trajOptBase.quadPenalty(indices, weights)

Bases: trajOptLib.trajOptBase.nonLinearObj

In many scenarios, we want to minimize the quadratic of some variables for some variables.

This is generally different from LQR objective by that it is a point constraint and thus not integral one. To make it versatile, the user is allowed to pass indices so we can directly evaluate those variables. User friendly classes are also created so the indices are calculated internally.

__callg__(x, y, G, row, col, rec, needg)

__init__(indices, weights)

Constructor for the class.

Parameters:

• indices – ndarray, indices of variables we aim to penalize.
• weights – float/ndarray, weights for terms

class trajOptLib.trajOptBase.system(nx, nu, np=0, ode='RK4')

Bases: object

Description of the dynamical system.

To define a dynamical system, we need to specify dimension of state, control, and parameter. Optionally, integration approach can be selected. This function should be inherited and users are supposed to override dyn/Jdyn functions.

Jdyn(t, x, u, p=None, h=None)

Dynamics function with Jacobian return.

It has to be overriden.

Parameters:x, u, p, h (t,) – see dyn Returns:y: ndarray, either dotx or x_k+1 Returns:J: ndarray/spmatrix, returning Jacobian of this function evaluation

__init__(nx, nu, np=0, ode='RK4')

Constructor for class.

Parameters:

• nx – int, dimension of state variable
• nu – int, dimension of control variable
• np – int, dimension of additional parameter
• ode – str, integration approach. Default RK4, options: Dis, Euler, BackEuler

dyn(t, x, u, p=None, h=None)

Dynamics function without gradient information.

It has to be overriden.

Parameters:

• t – float, time when evaluating system dynamics
• x – np.ndarray, (nx,) state variable
• u – np.ndarray, (nu,) control variable
• p – np.ndarray, (np,) additional optimizing variable
• h – float, used for discretized system. Integration step size

Returns:

either dotx or x_k+1 depends on system type

odes = ['RK4', 'Dis', 'Euler', 'BackEuler']

setOde(method)

Set ode approach.

Parameters:method – str, name of ode approach.

trajOptLib.trajOptCollocationProblem module

trajOptCollocationProblem.py

This class implements the direct collocation approach for humanoid trajectory optimization

class trajOptLib.trajOptCollocationProblem.trajOptCollocProblem(sys, N, t0, tf, addx=None)

Bases: pyoptsolver.OptProblem

A class for definition of trajectory optimization problem using collocation constraints.

A general framework for using this class is to:

1. Define a class which implements a DAE system, f(t, x, p, u)=0. This is a typical case for humanoid problem, but also flexible enough for simpler first order system.
2. Optionally, write desired cost function by subclass/creating from the list of available cost functions. This can be built incrementally by adding pieces.
3. Optionally, write desired constraint functions by subclass from available constraints. This can be built incrementally by adding pieces. Our approach can correctly detect the Jacobian structure.
4. Create this class with selected system, discretization, t0, tf range
5. Set bounds for state, control, parameters, x0 and xf
6. Add objective functions and constraints to this class
7. Call preProcess method explicitly
8. Create snoptConfig instance and choose desired options
9. Construct the solver
10. Use the solver to solve with either automatic guess or user provided guess

The system dynamics constraints are imposed using direct collocation approach with some additional optimization variables as suggested by others.

__callg__(x, y, G, row, col, rec, needg)

Evaluate those constraints, objective functions, and constraints. It simultaneously allocates sparsity matrix.

Parameters:

• x – ndarray, the solution to the problem
• y – ndarray, return F
• G/row/col – ndarray, information of gradient
• rec/needg – if we record/ if we need gradient

__init__(sys, N, t0, tf, addx=None)

Initialize problem by system, discretization grid size, and allowable time

Change history: now I remove gradmode option since I require the gradient be provided analytically all the time. I remove unnecessary linear objective functions. I reorder variable so q, dq, ddq, u, p are at consecutive place.

Parameters:

• sys – system, describe system dynamics
• N – int, discretization grid size, a uniform grid
• t0 – float/array like, allowable t0
• tf – float/array like, allowable tf
• addX – list of addX / one addX / None, additional optimization variables.

addAddXQuadPenalty(index, weights, mask=None)

Add quadratic penalty to addx variables.

The API is slightly different from previous three since we cannot guarantee weights are of the same lenght.

Parameters:

• index – int, indices of parameter variables to be penalized.
• weights – float/array-like, weights of penalty
• mask – filter

addConstr(constr, path=False, **kwargs)

Add a constraint to the problem.

Parameters:

• constr – a constraint object.
• path – bool, if this constraint is a path constraint. Only applies for point constraint.

addControlQuadPenalty(index, weights, mask=None)

Add a quadratic penalty on selected control variables.

Parameters:

• index – int/array-like, indices of control variables to be penalized.
• weights – float/array-like, weights of penalty
• mask – filter to select subset

addLQRObj(lqrobj)

Add a lqr objective function to the problem. It changes lqrObj into a function being called.

Parameters:lqrobj – a lqrObj class.

addLinearConstr(constr)

Add a linear constraint to the problem.

Parameters:constr – a linearConstr object

addLinearObj(linObj)

Add linear objective function.

Parameters:linObj – linearObj class

addLinearPointConstr(constr, path=False)

Add a linear point constraint to the problem.

Parameters:

• constr – a linearPointConstr object
• path – if this constraint is path constraint

addLinearPointObj(linPointObj, path=False)

Add linear point objective function.

Parameters:

• linPointObj – linearPointObj class
• path – bool, if this is path obj (at every point except for final one)

addNonLinearConstr(constr)

Add a general nonlinear constraint.

Parameters:constr – nonLinConstr class

addNonLinearObj(nonlinObj)

Add nonlinear objective function.

Parameters:nonLinObj – a nonLinObj class

addNonLinearPointConstr(pntConstr, path=False, **kwargs)

Add point constraint.

Parameters:

• pntConstr – pointConstr class
• path – bool, if this obj

Kwargs:

additional parameters, users can specify starting and ending indexes by specifying start and end

addNonLinearPointObj(nonPntObj, path=False)

Add nonlinear point objective.

Parameters:

• nonPntObj – nonLinObj class
• path – bool, if this obj is pointwise

addObj(obj, path=False)

A high level function that add objective function of any kind.

Parameters:

• obj – an objective object.
• path – bool, if the point objective is an integral one.

addParamQuadPenalty(index, weights, mask=None)

Add a quadratic penalty on selected parameter variables.

Parameters:

• index – int/array-like, indices of parameter variables to be penalized.
• weights – float/array-like, weights of penalty
• mask – filter of variables

addStateQuadPenalty(index, weights, mask=None)

Add a quadratic penalty on selected state variables.

Parameters:

• index – int/array-like, indices of state variables to be penalized.
• weights – float/array-like, weights of penalty
• mask – mask-like, filter for selecting subset of variables

addVanillaQuadPenalty(indices, weights)

Add a quadratic penalty term based on indices in the solution vector and weights.

This is dangerous and might cause unexpected trouble unless you are sure indices are correct. You can always use addStateQuadPenalty/addControlQuadPenalty/addParamQuadPenalty/addAddXQuadPenalty to finish these.

Parameters:

• indices – indices of variables to be penalized
• weights – float/ndarray weights associated with those variables.

genGuessFromSol(parsed_sol)

Generate an initial guess from a previous solution. Mainly change grid size or add perturbation. But determining structure is difficult

Parameters:parsed_sol – dictionary, output of calling parseSol

genGuessFromTraj(X=None, U=None, P=None, t0=None, tf=None, addx=None, tstamp=None, obj=None, interp_kind='linear')

Generate an initial guess for the problem with user specified information.

An amazing feature is the user does not have to give a solution of exactly the same time-stamped trajectory used internally. Interpolation approaches are used in such scenarios. The user is not required to provide them all, although we suggest they do so.

Parameters:

• X – ndarray, (x, x) each row corresponds to a state snapshot (if tstamp is None, assume equidistant grid). Column size can be dimx or dimx/sys.order
• U – ndarray, (x, dimu) each row corresponds to a control snapshot. Similar to X but with column size equals dimu
• P – ndarray, (x, dimp) each row corresponds to a parameter snapshot. Similar to X but with column size equals dimp
• t0/tf – float/array-like, initial/final time. If None, we randomly generate one
• addx – list of ndarray, guess of addx, if applicable
• tstamp – ndarray, (x,), None if the X/U/P are provided using equidistant grid.
• obj – ndarray, (x,) the objective part
• interp_kind – str, interpolation type for scipy.interpolate.interp1d, can be (‘linear’, ‘nearest’, ‘zero’, ‘slinear’, ‘quadratic’, ‘cubic’)

getAddXIndexByIndex(i)

With i as index of addx, it returns the starting index in solution vector for this one.

Parameters:i – int, the index of addX we want to query.

getContrlIndexByIndex(i)

With i as index for control variable, return the starting index in solution vector.

Parameters:i – int, the index of control we want to query

getParamIndexByIndex(i)

With i as index for parameter variable, return the starting index in solution vector.

Parameters:i – int, the index of parameter we want to query

getStateIndexByIndex(i)

With i as index for state variable, return the starting index in solution vector.

Parameters:i – int, the index of State we want to query

ipEvalF(x)

The eval_f function required by ipopt.

Parameters:x – a guess/solution of the problem Return f:float, objective function

ipEvalG(x)

Evaluation of the constraint function.

Parameters:x – ndarray, guess/solution to the problem. Return g:constraint function

ipEvalGradF(x)

Evaluation of the gradient of objective function.

Parameters:x – guess/solution to the problem Return grad:gradient of objective function w.r.t x

ipEvalJacG(x, flag)

Evaluate jacobian of constraints. I simply call __callg__

Parameters:

• x – ndarray, guess / solution to the problem
• flag – bool, True return row/col, False return values

parseF(guess, y=None)

Give an guess, evaluate it and parse into parts.

Parameters:

• guess – ndarray, (numSol, ) a guess or a solution to check
• y – ndarray, (numF, ) if None, it stores the solution

Returns:

dict, containing objective and parsed constraints

parse_sol(sol)

Call parseX function from utility and return a dict of solution.

pre_process(colloc_constr_is_on=False, defect_u=True, defect_p=True)

Initialize the instances of probFun now we are ready.

Call this function after the objectives and constraints have been set appropriately. It calculate the space required for SNOPT and allocates sparsity structure if necessary.

Parameters:

• colloc_constr_is_on – bool, if we also impose constraints on those collocation points.
• defect_u – bool, if we want to impose defect constraint on u, i.e. umid=(u0+uf)/2
• defect_p – bool, if we want to impose defect constraint on p, i.e. pmid=(p0+pf)/2

Caveat it might make problem over-constrained, if the path constraints are equality constraints.

randomGenX()

A more reansonable approach to generate random guess for the problem.

It considers bounds on initial and final states so this is satisfied. Then it linearly interpolate between states. Controls are randomly generated within control bound, if it presents. Otherwise [-1, 1]

Return x:ndarray, (numSol, ) an initial guess of the solution

setN(N)

Set N.

Parameters:N – the size of discretization.

setPbound(plb, pub)

Set bounds on parameter variables.

Parameters:

• plb – ndarray, (dimp,) lower bounds on parameter variables.
• pub – ndarray, (dimp,) upper bounds on parameter variables.

setUbound(ulb, uub)

Set bounds on control variables.

Parameters:

• ulb – ndarray, (dimu,) lower bounds on control variables.
• uub – ndarray, (dimu,) upper bounds on control variables.

setX0bound(x0lb, x0ub)

Set bounds on x0. This is optional but useful.

Parameters:

• x0lb – ndarray, (dimx,) lower bounds on x0 variables.
• x0ub – ndarray, (dimx,) upper bounds on x0 variables.

setXbound(xlb, xub)

Set bounds on state variables.

Parameters:

• xlb – ndarray, (dimx,) lower bounds on state variables.
• xub – ndarray, (dimx,) upper bounds on state variables.

setXfbound(xflb, xfub)

Set bounds on xf. This is optional but useful.

Parameters:

• xflb – ndarray, (dimx,) lower bounds on xf variables.
• xfub – ndarray, (dimx,) upper bounds on xf variables.

sett0tf(t0, tf)

Set t0 and tf.

Parameters:

• t0 – float/ndarray (2,) allowable t0
• tf – float/ndarray (2,) allowable tf

trajOptLib.trajOptManifoldCollocationProblem module

trajOptManifoldCollocationProblem.py

Direct collocation approach with manifold constraint. We introduce additional variables to solve this issue. Here the order of a system is really helpful. For a constraint phi(q)=0, by order we know up to which we should differentiate the constraints.

class trajOptLib.trajOptManifoldCollocationProblem.manifoldConstr(nx, nc, order=2, nG=0, nnzJx=0, nnzJg=0)

Bases: object

An object which defines state manifold constraints. We might need other ugly hacks.

Update 1, remove support for constr_order, only allow holonomic constraint, i.e. q However, user is allowed to control to any level, position / velocity / acceleration level. For different daeOrder, the implementation is slightly different

__callg__(x, F, G, row, col, rec, needg)

Constraint evaluation up to order with no correction.

This function serves as a basic path constraint imposed on every knot point It is different from nonLinearPointConstr in the following: 1. It is autonomous and time is not considered 2. Only state variables are passed in, no u or p is used

Parameters:

• x (ndarray, the state variables including q, dq, ddq, etc) –
• gamma (ndarray, the correction term) –
• F (ndarray, it stores the calculation values) –
• row, col (G,) –
• needg (rec,) –

Returns:

Return type:

This subroutine has no return.

__init__(nx, nc, order=2, nG=0, nnzJx=0, nnzJg=0)

Constructor for the class.

Parameters:

• nx (the system state dimension, for daeSystem it is order * nf) –
• nc (the manifold constraint dimension, it is not multiplied by order yet) –
• order (the order of the daeSystem) –
• constr_order (which order of states (number of time derivative) does the constraint first appear) –
• nG (non-zero of the output Jacobian) –
• nnzJx (int, nnz for Jacobian of (J^T gamma) w.r.t. x) –
• nnzJg (int, nnz for Jacobian of (J^T gamma) w.r.t. gamma) –

class trajOptLib.trajOptManifoldCollocationProblem.trajOptManifoldCollocProblem(sys, N, t0, tf, man_constr, addx=None)

Bases: trajOptLib.trajOptCollocationProblem.trajOptCollocProblem

A class for solving state equality constrained problem.

The difference between this class with trajOptCollocProblem is

1. This class directly handles state constrained problem
2. This class introduce additional variables at collocation points and change deflect constraints
3. Additional parameters are linearly interpolated previously, now we let it unconstrained. This is in fact the same with adding
4. How variables are arranged are different
5. The state constraints should be defined using a special class which returns higher order time derivatives

The optimization variables are arranged by 1. Trajectory region, (2*N - 1) * [q, dq, ddq, …, u, p] 2. t0 and tf region, it might be empty 3. addX region, of length lenAddX 4. gamma region, of length (N-1) * (man_constr.nf) 5. Auxiliary objective region

__callg__(x, y, G, row, col, rec, needg)

Evaluate those constraints, objective functions, and constraints. It simultaneously allocates sparsity matrix.

Parameters:

• x – ndarray, the solution to the problem
• y – ndarray, return F
• row, col (G,) – ndarray, information of gradient
• needg (rec,) – if we record/ if we need gradient

__init__(sys, N, t0, tf, man_constr, addx=None)

Constructor for the class, the user is required to provide dynamical system, discretization size, allowable times, state manifold constraint, and possibly auxiliary variables.

Parameters:

• sys (the system instance which describes system dynamics) –
• N (int, discretization grid size) –
• t0 (float/array-like, allowable initial time) –
• tf (float/array-like, allowable final time) –
• man_constr (manifold constraint) –
• addx (list of addX / one addX / None, additional optimization variables.) –

getGammaIndexByIndex(i)

Return the index of gamma by its index.

parseF(guess)

Give an guess, evaluate it and parse into parts.

Parameters:guess – ndarray, (numSol, ) a guess or a solution to check Returns:dict, containing objective and parsed constraints

preProcess(defect_u=True, defect_p=False, gamma_bound=1)

Initialize the problem, allocating spaces.

Compared with trajOptCollocProblem, it has new sets of variables, different constraints. It supports limitation of magnitude of gamma, as gamma_bound means.

trajOptLib.trajOptMultiPhaseCollocationProblem module

trajOptMultiPhaseCollocationProblem.py

This class implements methods for solving problem with multiple phases. I will also change code style to suppress those warnings.

class trajOptLib.trajOptMultiPhaseCollocationProblem.LinearConnectConstr(phase1, phase2, a1, a2, lb=None, ub=None, index1=-1, index2=0, adda=None, addx_index=None)

Bases: object

Class for defining linear constraint functions.

__init__(phase1, phase2, a1, a2, lb=None, ub=None, index1=-1, index2=0, adda=None, addx_index=None)

Constructor for nonlinear point constraint. Also serve as path constraint.

Parameters:

• phase1/phase2 – int, phase number that constraint is imposed. We read data from them
• a1/a2 – ndarray, the constraint, :math: l le A_1 x_1 + A_2 x_2 le u
• lb/ub – lower and upper bound of the constraint function. None means equal to 0
• index1/index2 – int, by default we connect last point of phase 1 with first point of phase 2. However, we allow manual assignment of them.
• adda – ndarray, if additional parameter comes into play, we use this
• addx_index – int, index of the additional parameters

find_time_gradient()

For a matrix, find column 0 indice.

class trajOptLib.trajOptMultiPhaseCollocationProblem.NonLinearConnectConstr(phase1, phase2, nc, lb=None, ub=None, nG=None, index1=-1, index2=0, addx_index=None)

Bases: object

Class for defining point constraint function.

__callg__(x1, x2, F, G, row, col, rec, needg, addx=None)

Call and evaluate the constraint function. x1 and x2 are points in phase 1 and phase 2.

I return them together in G, row, col since it might be derived from sympy so thay are together. But, human derived formulas love the previous one better

__init__(phase1, phase2, nc, lb=None, ub=None, nG=None, index1=-1, index2=0, addx_index=None)

Constructor for nonlinear point constraint. Also serve as path constraint.

Parameters:

• phase1/phase2 – int, phase number that constraint is imposed. We read data from them
• nc – int, dimension of constraint function
• ub (lb,) – lower and upper bound of the constraint function. None means equal to 0
• nG – int, number of nnz of Jacobian
• index1/index2 – int, by default we connect last point of phase 1 with first point of phase 2. However, we allow manual assignment of them.
• addx_index – int, which additional x we use to connect those two phases

find_time_gradient(x1, x2, addx=None)

Find where the gradients are w.r.t time.

class trajOptLib.trajOptMultiPhaseCollocationProblem.TrajOptMultiPhaseCollocProblem(probs, addx=None, process_args={})

Bases: pyoptsolver.OptProblem

A class for definition of trajectory optimization problem using collocation constraints with support for phase transition.

The support of multiple phase is implemented by calling functions defined in trajOptCollocProblem since I do not want to reinvent the wheels. I can conveniently add support for other constraints that connects two phases.

__callg__(x, y, G, row, col, rec, needg)

Evaluate those constraints, objective functions, and constraints. It simultaneously allocates sparsity matrix.

We do this in several parts: - for each phase, we call the private functions explicitly. Ugly hack - call the connection nonlinear constraints - call the nonlinear objective functions associated with addx

Parameters:

• x – ndarray, the solution to the problem
• y – ndarray, return F
• row, col (G,) – ndarray, information of gradient
• needg (rec,) – if we record / if we need gradient

__init__(probs, addx=None, process_args={})

Initialize a multi-phase problem by a list of trajOptColloProblem objects.

Parameters:

• probs – a list of trajOptCollocProblem
• addx – a list of addX
• process_args – arguments for preprocess used in each problem

add_addx_constr(constr)

Add a linear or nonlinear constraint associated with addx.

Parameters:constr – a point constraint.

add_addx_obj(obj)

Add an objective evaluated at addx to the problem.

Parameters:obj – a point objective object

add_connect_constr(constr)

Add a linear constraint that connects two phases.

Parameters:constr – a connect constraint object.

add_connect_linear_constr(constr)

Add a linear connection constraint to the problem.

Parameters:constr – a LinearConnectConstr object.

add_connect_nonlinear_constr(constr)

Add a nonlinear connect constraint to the problem.

Parameters:constr – a NonLinearConnectConstr object.

add_constr(constr)

Add a constraint to the problem.

Parameters:constr – a general constraint function object.

add_nonlinear_constr(constr)

Add a general nonlinear constraint to the problem.

Parameters:constr – a nonLinearConstr object.

add_nonlinear_obj(obj)

Add a nonlinear objective to the problem.

Parameters:obj – a nonLinearObj object.

add_obj(obj)

Add a objective function to the problem.

Parameters:obj – a general objective function object.

change_connect_time_constr_bound(num, xl, xu)

Change the connect time constraint if other specification are required.

guess_gen_from_phase_sol(phase_sols, addX=[])

Generate an initial guess for the multi-phase problem by concatenating solutions from single phases.

Parameters:phase_sols – list of dict, a list of guess of solutions to each phase

guess_gen_from_phase_traj(X=None, U=None, P=None, t0=None, tf=None, addx=None, tstamp=None, obj=None, addX=None, interp_kind='linear')

Generate a guess from the trajectory in other phases.

The parameters are the same with .. py:classmethod::trajOptCollocation.genGuessFromTraj but a list version.

ipEvalF(x)

The eval_f function required by ipopt.

Parameters:x – a guess/solution of the problem Return f:float, objective function

ipEvalG(x)

Evaluation of the constraint function.

Parameters:x – ndarray, guess/solution to the problem. Return g:constraint function

ipEvalGradF(x)

Evaluation of the gradient of objective function.

Parameters:x – guess/solution to the problem Return grad:gradient of objective function w.r.t x

ipEvalJacG(x, flag)

Evaluate jacobian of constraints. I simply call __callg__

Parameters:

• x – ndarray, guess / solution to the problem
• flag – bool, True return row/col, False return values

parse_f(sol)

Use the solution and evaluate on all constraints. Check which are active and analyze why we are not converging.

Parameters:sol – ndarray, the solution we want to analyze

parse_sol(sol)

Given a solution, we parse and return readable data structure.

Parameters:sol – ndarray or result object returned by SNOPT. Return traj:a dict of keys ‘t’, ‘x’, ‘u’, ‘p’, ‘phases’.

• ‘t’ is a concatenated time grid (might not be equidistant, but piecewise equidistant
• ‘x’ ndarray, (*, dimx) is the concatenated state vector of all phases
• ‘u’ ndarray, (*, dimu) is the concatenated control vector of all phases
• ‘p’ ndarray, (*, dimp) is the concatenated parameter vector of all phases
• ‘phases’ list of dictionaries composed of keys ‘t’, ‘x’, ‘u’, ‘p’, ‘addx’ where ‘addx’ is additional optimization variables.

pre_process()

Initialize the instances of probFun now we are ready.

Call this function after the objectives and constraints have been set appropriately. It calculate the space required for SNOPT and allocates sparsity structure if necessary.

random_gen_x()

Generate a random guess to the problem.

trajOptLib.trajOptProblem module

trajOptProblem.py

Class for describing the trajectory optimization problems.

class trajOptLib.trajOptProblem.trajOptProblem(sys, N, t0, tf, gradmode=True, addx=None)

Bases: pyoptsolver.OptProblem

A class for definition of trajectory optimization problem.

A general framework for using this class is to:

1. Define a class inherited from system and write dyn/Jdyn method.
2. Optionally, write desired cost function by inheriting/creating from the list of available cost functions.
3. Optionally, write desired constraint functions by inheriting from available constraints.
4. Create this class with selected system, discretization, t0, tf range, gradient option
5. Set bounds for state, control, parameters, x0 and xf
6. Add objective functions and constraints to this class
7. Call preProcess method explicitly
8. Create snoptConfig instance and choose desired options
9. Construct the solver
10. Use the solver to solve with either automatic guess or user provided guess

__callf__(x, y)

Evaluate those constraints and objective functions.

__callg__(x, y, G, row, col, rec, needg)

Evaluate those constraints, objective functions, and constraints. It simultaneously allocates sparsity matrix.

Parameters:

• x – ndarray, the solution to the problem
• y – ndarray, return F
• row, col (G,) – ndarray, information of gradient
• needg (rec,) – if we record/ if we need gradient

__init__(sys, N, t0, tf, gradmode=True, addx=None)

Initialize problem by system, discretization grid size, and allowable time

Parameters:

• sys – system, describe system dynamics
• N – int, discretization grid size, a uniform grid
• t0 – float/array like, allowable t0
• tf – float/array like, allowable tf
• gradmode – bool, sets if we use gradient mode.
• addX – list of addX / one addX / None, additional optimization variables.

addConstr(constr, path=False)

addLQRObj(lqrobj)

Add a lqr objective function to the problem. It changes lqrObj into a function being called in two modes…

Parameters:lqrobj – a lqrObj class.

addLinearConstr(constr)

addLinearObj(linObj)

Add linear objective function.

Parameters:linObj – linearObj class

addLinearPointConstr(constr, path=False)

addLinearPointObj(linPointObj, path=False)

Add linear point objective function.

Parameters:

• linPointObj – linearPointObj class
• path – bool, if this is path obj (at every point except for final one)

addNonLinearConstr(constr)

Add a general nonlinear constraint.

Parameters:constr – nonLinConstr class

addNonLinearObj(nonlinObj)

Add nonlinear objective function.

Parameters:nonLinObj – a nonLinObj class

addNonLinearPointConstr(pntConstr, path=False)

Add point constraint.

Parameters:

• pntConstr – pointConstr class
• path – bool, if this obj

addNonLinearPointObj(nonPntObj, path=False)

Add nonlinear point objective.

Parameters:

• nonPntObj – nonLinObj class
• path – bool, if this obj is pointwise

addObj(obj, path=False)

A high level function that add objective function of any kind.

genGuessFromTraj(X=None, U=None, P=None, t0=None, tf=None, addx=None, tstamp=None, obj=None, interp_kind='linear')

Generate an initial guess for the problem with user specified information.

An amazing feature is the user does not have to give a solution of exactly the same time-stamped trajectory used internally. Interpolation approaches are used in such scenarios. The user is not required to provide them all, although we suggest they do so.

Parameters:

• X – ndarray, (x, x) each row corresponds to a state snapshot (if tstamp is None, assume equidistant grid). Column size can be dimx or dimx/sys.order
• U – ndarray, (x, dimu) each row corresponds to a control snapshot. Similar to X but with column size equals dimu
• P – ndarray, (x, dimp) each row corresponds to a parameter snapshot. Similar to X but with column size equals dimp
• t0/tf – float/array-like, initial/final time. If None, we randomly generate one
• addx – list of ndarray, guess of addx, if applicable
• tstamp – ndarray, (x,), None if the X/U/P are provided using equidistant grid.
• obj – ndarray, (x,) the objective part
• interp_kind – str, interpolation type for scipy.interpolate.interp1d, can be (‘linear’, ‘nearest’, ‘zero’, ‘slinear’, ‘quadratic’, ‘cubic’)

ipEvalF(x)

The eval_f function required by ipopt.

Parameters:x – a guess/solution of the problem Return f:float, objective function

ipEvalG(x)

Evaluation of the constraint function.

Parameters:x – ndarray, guess/solution to the problem. Return g:constraint function

ipEvalGradF(x)

Evaluation of the gradient of objective function.

Parameters:x – guess/solution to the problem Return grad:gradient of objective function w.r.t x

ipEvalJacG(x, flag)

Evaluate jacobian of constraints. I simply call __callg__

Parameters:

• x – ndarray, guess / solution to the problem
• flag – bool, True return row/col, False return values

parseF(guess)

Give an guess, evaluate it and parse into parts.

Parameters:guess – ndarray, (numSol, ) a guess or a solution to check Returns:dict, containing objective and parsed constraints

parseSol(sol)

Call parseX function from utility and return a dict of solution.

parse_sol(sol)

Call parseX function from utility and return a dict of solution.

preProcess(*args)

Alias for pre_process

pre_process(*args)

Initialize the instances of probFun now we are ready.

Call this function after the objectives and constraints have been set appropriately. It calculate the space required for SNOPT and allocates sparsity structure if necessary.

randomGenX()

A more reansonable approach to generate random guess for the problem.

It considers bounds on initial and final states so this is satisfied. Then it linearly interpolate between states. Controls are randomly generated within control bound, if it presents. Otherwise [-1, 1]

Return x:ndarray, (numSol, ) an initial guess of the solution

setN(N)

Set N.

Parameters:N – the size of discretization.

sett0tf(t0, tf)

Set t0 and tf.

Parameters:

• t0 – float/ndarray (2,) allowable t0
• tf – float/ndarray (2,) allowable tf

trajOptLib.utility module

utility.py

Collection of utility functions such as solution parsing and showing.

class trajOptLib.utility.InfBuilder(n=20)

Bases: object

A class to help us return infinity

__init__(n=20)

class trajOptLib.utility.OneBuilder(n=20)

Bases: object

A class to return array of 1

__init__(n=20)

class trajOptLib.utility.ZeroBuilder(n=20)

Bases: object

A class to return array of 1

__init__(n=20)

trajOptLib.utility.checkInBounds(x, bds)

Check the position of variables in a bound.

Basically, it returns the position of actual value in the bounds [-1, 1]. If both sides are bounded, it simply calculate this value. If one side is bounded, it check if value if within certain threshold with the bound Unbounded variables shall always return 0, the same applies to equality-bounded variables.

trajOptLib.utility.getInf(n=None)

Return an inf array.

Parameters:n – int, size of the array to return, None means scalar Returns y:the scalar inf or array of inf

trajOptLib.utility.interp(t, X, teval, Xeval, kind)

Do interpolation on previous calculated solution.

It handles case when t is None, in which case, teval is not used and we use a uniform grid in [0, 1]

Parameters:

• t – array-like, user-specified time stamps
• X – ndarray, (x, x), variable to be interpolated
• teval – array-like, where to evaluate for the interpolation
• Xeval – ndarray, (x, x), where to store the evaluation. It might has more columns than X, in which case we fill higher-order derivative
• kind – str, interpolation type for scipy.interpolate.interp1d, can be (‘linear’, ‘nearest’, ‘zero’, ‘slinear’, ‘quadratic’, ‘cubic’)

trajOptLib.utility.parseX(x, N, dimx, dimu, dimp, uset0, usetf, setfinal=True)

Parse the solution into a dict of time, state, control, parameter.

Parameters:

• x – ndarray, solution found by SNOPT
• dimx, dimu, dimp (N,) – discretization of the problem
• usetf (uset0,) – float/array like, specification of initial and final time
• setfinal – if we set the final element of u/p the same with second last

Returns:

a dictionary of keys ‘t’, ‘x’, ‘u’, ‘p’

trajOptLib.utility.randomGenInBound(bds, n=None)

Randomly generate a vector within bounds / [-1, 1]

Parameters:

• bds – list/tuple of ndarray, the bounds of variable
• n – if bds are None, this is for determining size of vector

Return x:

ndarray, the random generated variable

trajOptLib.utility.showSol(solDict, xsplit=None, usplit=None, psplit=None, show=True)

Plot the parsed solution.

Parameters:

• solDict – dict, returned from solParse; or a list of such dict. Then they are compared
• xsplit – if state space is large, this splits it into several images it’s a list of int like (3, 6). None means no split
• usplit – similar to xsplit, apply for control
• psplit – similar to xsplit, apply for parameter

Module contents