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.legacy package
  • 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

class trajoptlib.classBuilder.DaeSystemWrapper(fun, nx, nu, np, nf, *args)

Bases: trajoptlib.trajOptBase.DaeSystem

This class takes a function and returns a system.

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.

class trajoptlib.classBuilder.SystemWrapper(fun, nx, nu, np, *args)

Bases: trajoptlib.trajOptBase.System

This class takes a function and returns a system.

dyn(t, x, u, p)

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

jac_dyn(t, x, u, p)

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

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]))

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

trajoptlib.systemBuilder module

Code to build dynamical systems

class trajoptlib.systemBuilder.GeometricRobot(dim, dimp=0)

Bases: trajoptlib.trajOptBase.DaeSystem

A class that defines a geometric robot, no dynamics involved

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

Write dynamics function

class trajoptlib.systemBuilder.GeometricRobotSystem(dim, dimp=0, ode='Euler')

Bases: trajoptlib.trajOptBase.System

A class that implements a geometric robot, second order.

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

jac_dyn(t, x, u, p, 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

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.

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.

findTimeGradient(x)

grad = ['user', 'no']

class trajoptlib.trajOptBase.DaeSystem(nx, nu, np, nf, nG)

Bases: object

A DAE system.

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, offset=0)

Bases: object

Class for linear constraints based on the whole x length.

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.

class trajoptlib.trajOptBase.LinearPointConstr(index, A, lb=None, ub=None, offset=0)

Bases: trajoptlib.trajOptBase._objectWithMatrix

Class for linear constraint at selected points.

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.

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.

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.

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.

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.

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.

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.

class trajoptlib.trajOptBase.System(nx, nu, np=0, ode='Euler')

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/jac_dyn functions.

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

jac_dyn(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

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

set_ode(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.pyoptsolvercpp.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.

Currentmodule:

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)

Alias for add_constr()

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)

Alias for trajOptLib.TrajOptCollocProblem.add_lqr_obj()

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)

Alias for addObj()

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.

add_constr(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.

add_lqr_obj(lqrobj)

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

Parameters:lqrobj – a lqrObj class.

add_obj(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.

genGuessFromSol(parsed_sol)

Alias for gen_guess_from_traj()

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

Alias for gen_guess_from_traj()

gen_guess_from_sol(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 parse_sol

gen_guess_from_traj(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

parseF(guess, y=None)

Alias for parse_f()

parse_f(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.

plot_jacobian(savefnm=None)

Plot the jacobian pattern of this problem.

Parameters:savefnm – str, the filename to save pattern into. If none, no figure is saved.

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()

Alias for trajOptLib.TrajOptCollocProblem.random_gen_guess()

random_gen_guess()

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

set_N(N)

Set N.

Parameters:N – the size of discretization.

set_p_bound(plb, pub)

Set bounds on parameter variables.

Parameters:

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

set_t0_tf(t0, tf)

Set t0 and tf.

Parameters:

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

set_u_bound(ulb, uub)

Set bounds on control variables.

Parameters:

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

set_x0_bound(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.

set_x_bound(xlb, xub)

Set bounds on state variables.

Parameters:

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

set_xf_bound(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.

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

Currentmodule:

__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

x : ndarray, the state variables including q, dq, ddq, etc gamma : ndarray, the correction term F : ndarray, it stores the calculation values G, row, col : ndarray, the triplet for Jacobian return rec, needg : control if G is calculated and row, col are modified

This subroutine has no return.

__calc_correction__(x, gamma, y, Gq, rowq, colq, Gg, rowg, colg, rec, needg)

Calculate the correction term

It calculate J(qc)^T gamma, and the sparse Jacobians associated with it

x : array-like, q, dq, ddq, etc gamma : array-like, the correction vector gamma y : array-like, the function calculation value, it is essentially J(qc)^T gamma Gq, rowq, colq : array-like, store the Jacobian of correction term w.r.t. x Gg, rowg, colg : array-like, store the Jacobian of correction term w.r.t. gamma rec : if we record row and column information needg : if we calculate G

Calculations are done in-place so no return is required.

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

getGammaIndexByIndex(i)

Return the index of gamma by its index.

parseF(guess)

Alias for trajOptLib.TrajOptManifoldCollocProblem.parse_f()

parse_f(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

pre_process(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.

Currentmodule:

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.

Currentmodule:

__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

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.pyoptsolvercpp.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.

Currentmodule:

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.

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.pyoptsolvercpp.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/jac_dyn 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

Currentmodule:

addConstr(constr, path)

Alias for add_constr()

addLQRObj(lqrobj)

Alias for add_lqr_obj()

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)

Alias for add_obj()

add_constr(constr, path=False)

Add a constraint to the problem.

Parameters:constr – a constraint object, can be LinearConstr/LinearPointConstr/NonLinearConstr/NonLinearPointConstr

add_lqr_obj(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.

add_obj(obj, path=False)

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

Parameters:

• obj – an objective instance, can be LinearObj/LinearPointObj/NonLinearObj/NonLinearPointObj, LqrObj
• path – bool, indicating if the constraint is path constraint.

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

Alias for gen_guess_from_traj

gen_guess_from_traj(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’)

parseF(guess)

Alias for parse_f()

parseSol(sol)

Alias for parse_sol()

parse_f(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

parse_sol(sol)

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

Parameters:sol – ndarray, the solution.

plot_jacobian(savefnm=None)

Plot the jacobian pattern of this problem.

Parameters:savefnm – str, the filename to save pattern into. If none, no figure is saved.

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()

Alias for random_gen_guess()

random_gen_guess()

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

set_N(N)

Set N.

Parameters:N – the size of discretization.

set_t0_tf(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

class trajoptlib.utility.OneBuilder(n=20)

Bases: object

A class to return array of 1

class trajoptlib.utility.ZeroBuilder(n=20)

Bases: object

A class to return array of 1

trajoptlib.utility.check_in_bounds(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.get_inf(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.parse_X(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.random_gen_in_bound(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.show_sol(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

class trajoptlib.System(nx, nu, np=0, ode='Euler')

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/jac_dyn functions.

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

jac_dyn(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

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

set_ode(method)

Set ode approach.

Parameters:method – str, name of ode approach.

class trajoptlib.DaeSystem(nx, nu, np, nf, nG)

Bases: object

A DAE system.

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.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.

findTimeGradient(x)

grad = ['user', 'no']

class trajoptlib.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.

class trajoptlib.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.

class trajoptlib.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.

class trajoptlib.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.

class trajoptlib.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.

class trajoptlib.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.

class trajoptlib.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.

class trajoptlib.NonLinearConstr(nsol, nc, lb=None, ub=None, gradmode='user', nG=None)

Bases: trajoptlib.trajOptBase.BaseFun

Class for defining constraint function in a general form.

class trajoptlib.LinearPointConstr(index, A, lb=None, ub=None, offset=0)

Bases: trajoptlib.trajOptBase._objectWithMatrix

Class for linear constraint at selected points.

class trajoptlib.LinearConstr(A, lb=None, ub=None, offset=0)

Bases: object

Class for linear constraints based on the whole x length.

class trajoptlib.TrajOptProblem(sys, N, t0, tf, gradmode=True, addx=None)

Bases: pyoptsolver.pyoptsolvercpp.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/jac_dyn 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

Currentmodule:

addConstr(constr, path)

Alias for add_constr()

addLQRObj(lqrobj)

Alias for add_lqr_obj()

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)

Alias for add_obj()

add_constr(constr, path=False)

Add a constraint to the problem.

Parameters:constr – a constraint object, can be LinearConstr/LinearPointConstr/NonLinearConstr/NonLinearPointConstr

add_lqr_obj(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.

add_obj(obj, path=False)

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

Parameters:

• obj – an objective instance, can be LinearObj/LinearPointObj/NonLinearObj/NonLinearPointObj, LqrObj
• path – bool, indicating if the constraint is path constraint.

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

Alias for gen_guess_from_traj

gen_guess_from_traj(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’)

parseF(guess)

Alias for parse_f()

parseSol(sol)

Alias for parse_sol()

parse_f(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

parse_sol(sol)

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

Parameters:sol – ndarray, the solution.

plot_jacobian(savefnm=None)

Plot the jacobian pattern of this problem.

Parameters:savefnm – str, the filename to save pattern into. If none, no figure is saved.

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()

Alias for random_gen_guess()

random_gen_guess()

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

set_N(N)

Set N.

Parameters:N – the size of discretization.

set_t0_tf(t0, tf)

Set t0 and tf.

Parameters:

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

class trajoptlib.TrajOptCollocProblem(sys, N, t0, tf, addx=None)

Bases: pyoptsolver.pyoptsolvercpp.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.

Currentmodule:

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)

Alias for add_constr()

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)

Alias for trajOptLib.TrajOptCollocProblem.add_lqr_obj()

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)

Alias for addObj()

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.

add_constr(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.

add_lqr_obj(lqrobj)

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

Parameters:lqrobj – a lqrObj class.

add_obj(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.

genGuessFromSol(parsed_sol)

Alias for gen_guess_from_traj()

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

Alias for gen_guess_from_traj()

gen_guess_from_sol(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 parse_sol

gen_guess_from_traj(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

parseF(guess, y=None)

Alias for parse_f()

parse_f(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.

plot_jacobian(savefnm=None)

Plot the jacobian pattern of this problem.

Parameters:savefnm – str, the filename to save pattern into. If none, no figure is saved.

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()

Alias for trajOptLib.TrajOptCollocProblem.random_gen_guess()

random_gen_guess()

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

set_N(N)

Set N.

Parameters:N – the size of discretization.

set_p_bound(plb, pub)

Set bounds on parameter variables.

Parameters:

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

set_t0_tf(t0, tf)

Set t0 and tf.

Parameters:

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

set_u_bound(ulb, uub)

Set bounds on control variables.

Parameters:

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

set_x0_bound(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.

set_x_bound(xlb, xub)

Set bounds on state variables.

Parameters:

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

set_xf_bound(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.

class trajoptlib.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.

Currentmodule:

__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

find_time_gradient(x1, x2, addx=None)

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

class trajoptlib.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.

Currentmodule:

find_time_gradient()

For a matrix, find column 0 indice.

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

Bases: pyoptsolver.pyoptsolvercpp.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.

Currentmodule:

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.

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.

class trajoptlib.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

Currentmodule:

__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

x : ndarray, the state variables including q, dq, ddq, etc gamma : ndarray, the correction term F : ndarray, it stores the calculation values G, row, col : ndarray, the triplet for Jacobian return rec, needg : control if G is calculated and row, col are modified

This subroutine has no return.

__calc_correction__(x, gamma, y, Gq, rowq, colq, Gg, rowg, colg, rec, needg)

Calculate the correction term

It calculate J(qc)^T gamma, and the sparse Jacobians associated with it

x : array-like, q, dq, ddq, etc gamma : array-like, the correction vector gamma y : array-like, the function calculation value, it is essentially J(qc)^T gamma Gq, rowq, colq : array-like, store the Jacobian of correction term w.r.t. x Gg, rowg, colg : array-like, store the Jacobian of correction term w.r.t. gamma rec : if we record row and column information needg : if we calculate G

Calculations are done in-place so no return is required.

class trajoptlib.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

getGammaIndexByIndex(i)

Return the index of gamma by its index.

parseF(guess)

Alias for trajOptLib.TrajOptManifoldCollocProblem.parse_f()

parse_f(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

pre_process(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.

class trajoptlib.SystemWrapper(fun, nx, nu, np, *args)

Bases: trajoptlib.trajOptBase.System

This class takes a function and returns a system.

dyn(t, x, u, p)

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

jac_dyn(t, x, u, p)

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

class trajoptlib.DaeSystemWrapper(fun, nx, nu, np, nf, *args)

Bases: trajoptlib.trajOptBase.DaeSystem

This class takes a function and returns a system.

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

Override class method

class trajoptlib.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.

trajoptlib.block_index(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]))

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.GeometricRobot(dim, dimp=0)

Bases: trajoptlib.trajOptBase.DaeSystem

A class that defines a geometric robot, no dynamics involved

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

Write dynamics function

class trajoptlib.GeometricRobotSystem(dim, dimp=0, ode='Euler')

Bases: trajoptlib.trajOptBase.System

A class that implements a geometric robot, second order.

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

jac_dyn(t, x, u, p, 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

trajoptlib.parse_X(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.show_sol(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

trajoptlib.get_inf(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

class trajoptlib.OptSolver(problem, config)

Bases: object

solve_guess(guess)

solve_rand()

class trajoptlib.OptConfig(backend='ipopt', **kw)

Bases: object

This class is used to keep track of various optimization configurations.

It provides a unified interface for interacting with those solvers. Some custom options are also supported, but might not work depending on the backend.

ipopt_options = set(['exact_hessian', 'fd_jacobian', 'linear_solver', 'print_freq'])

scipy_kws = set(['tol'])

scipy_option_kws = set(['barrier_tol', 'disp', 'factorization_method', 'grad', 'gtol', 'initial_barrier_parameter', 'initial_barrier_tolerance', 'initial_constr_penalty', 'initial_tr_radius', 'xtol'])

shared_options = set(['deriv_check', 'fea_tol', 'major_iter', 'opt_tol', 'print_level'])

snopt_options = set(['iter_limit', 'minor_iter', 'print_file'])