Python API reference

Parametrization algorithms

TOPPRA

class toppra.algorithm.TOPPRA(constraint_list, path, gridpoints=None, solver_wrapper=None, parametrizer=None, **kwargs)

Bases: toppra.algorithm.reachabilitybased.reachability_algorithm.ReachabilityAlgorithm

Time-Optimal Path Parameterization based on Reachability Analysis (TOPPRA).

Examples

>>> instance = algo.TOPPRA([pc_vel, pc_acc], path)
>>> jnt_traj = instance.compute_trajectory()  # rest-to-rest motion
>>> instance.problem_data # intermediate result

Parameters

• constraint_list (List[Constraint]) – List of constraints to which the robotic system is subjected to.

• path (AbstractGeometricPath) – Input geometric path.

• gridpoints (Optional[np.ndarray]) – Gridpoints for discretization of the geometric path. The start and end points must agree with the geometric path’s path_interval. If omited a gridpoint will be automatically selected.

• solver_wrapper (str, optional) –

  Name of the solver wrapper to use. Possible value are:

  • ’seidel’

  • ’hotqpoases’

  For more details see the solverwrappers documentation.

• parametrizer (str, optional) – Name of the output parametrizer to use.

Notes

In addition to the given constraints, there are additional constraints on the solutions enforced by the solver-warpper. Therefore, different parametrizations are returned for different solver wrappers. However, the difference should be very small, especially for well-conditioned problems.

See also

toppra.solverwrapper.seidelWrapper, toppra.solverwrapper.hotqpOASESSolverWrapper

compute_parameterization(sd_start, sd_end, return_data=False)

Compute a path parameterization.

If fail, whether because there is no valid parameterization or because of numerical error, the arrays returns should contain np.nan.

Parameters

• sd_start (float) – Starting path velocity. Must be positive.

• sd_end (float) – Goal path velocity. Must be positive.

• return_data (bool, optional) – If is True, also return matrix K which contains the controllable sets.

Returns

• sdd_vec (array) – Shape (N,). Path accelerations. Double array. Will contain nan(s) if failed.

• sd_vec (array) – Shape (N+1,). Path velocities. Double array. Will contain nan(s) if failed.

• v_vec (array or None) – Shape (N,). Auxiliary variables.

• K (array) – Shape (N+1, 2). Return the controllable set if return_data is True.

compute_trajectory(sd_start=0, sd_end=0)

Compute the resulting joint trajectory and auxilliary trajectory.

This is a convenient method if only the final output is wanted.

Parameters

• sd_start (float) – Starting path velocity.

• sd_end (float) – Goal path velocity.

• return_data – If true, return a dict containing the internal data.

Return type

Optional[AbstractGeometricPath]

Returns

Time-parameterized joint position trajectory or None If unable to parameterize.

TOPPRAsd

class toppra.algorithm.TOPPRAsd(constraint_list, path, gridpoints=None, solver_wrapper=None, parametrizer=None, **kwargs)

Bases: toppra.algorithm.reachabilitybased.reachability_algorithm.ReachabilityAlgorithm

TOPPRA with specified duration.

The key technical idea is to compute the fastest and the slowest time parameterizations. Then use bisection search to find a convex combination of the parametrizations that has the desired duration.

TODO: The current implementation is inefficient.

compute_parameterization(sd_start, sd_end, return_data=False, atol=1e-05)

Compute a path parameterization.

If there is no valid parameterizations, simply return None(s). If the desired duration is not achievable, returns the fastest or the slowest parameterizations.

Parameters

• sd_start (float) – Starting path velocity. Must be positive.

• sd_end (float) – Goal path velocity. Must be positive.

• return_data (bool) – If is True, also return matrix K which contains the controllable sets. Default to False.

• atol (float) – Absolute tolerance of duration. Default to 1e-5.

Returns

• sdd_vec (array or None) – Shape (N,). Path accelerations.

• sd_vec (array None) – Shape (N+1,). Path velocities.

• v_vec (array or None) – Shape (N,). Auxiliary variables.

• K (array) – Shape (N+1, 2). Return if return_data is True. The controllable sets.

compute_trajectory(sd_start=0, sd_end=0)

Compute the resulting joint trajectory and auxilliary trajectory.

This is a convenient method if only the final output is wanted.

Parameters

• sd_start (float) – Starting path velocity.

• sd_end (float) – Goal path velocity.

• return_data – If true, return a dict containing the internal data.

Return type

Optional[AbstractGeometricPath]

Returns

Time-parameterized joint position trajectory or None If unable to parameterize.

set_desired_duration(desired_duration)

Set desired duration for the time-parametrization.

Parameters

desired_duration (float) – The desired duration.

ParameterizationReturnCode

class toppra.algorithm.ParameterizationReturnCode(value)

Return codes from a parametrization attempt.

Ok = 'Ok: Successful parametrization'

ErrUnknown = 'Error: Unknown issue'

ErrShortPath = 'Error: Input path is very short'

FailUncontrollable = 'Error: Instance is not controllable'

ErrForwardPassFail = 'Error: Forward pass fail. Numerical errors occured'

ParameterizationData

class toppra.algorithm.ParameterizationData(*arg, **kwargs)

Internal data and output.

K: Optional[numpy.ndarray]

Shape (N+1, 2). Controllable sets.

Type

np.ndarray

X: Optional[numpy.ndarray]

Shape (N+1, 2). Feasible sets.

Type

np.ndarray

gridpoints: Optional[numpy.ndarray]

Shape (N+1, 1). Gridpoints

Type

np.ndarray

return_code: toppra.algorithm.algorithm.ParameterizationReturnCode

Return code of the last parametrization attempt.

Type

ParameterizationReturnCode

sd_vec: Optional[numpy.ndarray]

Shape (N+1, 1). Path velocities

Type

np.ndarray

sdd_vec: Optional[numpy.ndarray]

Shape (N+1, 1). Path acceleration

Type

np.ndarray

[abstract]ParameterizationAlgorithm

class toppra.algorithm.ParameterizationAlgorithm(constraint_list, path, gridpoints=None, parametrizer=None, gridpt_max_err_threshold=0.001, gridpt_min_nb_points=100)

Base parametrization algorithm class.

This class specifies the generic behavior for parametrization algorithms. For details on how to construct a ParameterizationAlgorithm instance, as well as configure it, refer to the specific class.

Example usage:

# usage
instance.compute_parametrization(0, 0)
output = instance.problem_data

# do this if you only want the final trajectory
traj = instance.compute_trajectory(0, 0)

See also

toppra.algorithm.TOPPRA, toppra.algorithm.TOPPRAsd, ParameterizationReturnCode, ParameterizationData

abstract compute_parameterization(sd_start, sd_end, return_data=False)

Compute the path parameterization subject to starting and ending conditions.

After this method terminates, the attribute problem_data will contain algorithm output, as well as the result. This is the preferred way of retrieving problem output.

Parameters

• sd_start (float) – Starting path velocity. Must be positive.

• sd_end (float) – Goal path velocity. Must be positive.

• return_data (bool) – If true also return the problem data.

compute_trajectory(sd_start=0, sd_end=0)

Compute the resulting joint trajectory and auxilliary trajectory.

This is a convenient method if only the final output is wanted.

Parameters

• sd_start (float) – Starting path velocity.

• sd_end (float) – Goal path velocity.

• return_data – If true, return a dict containing the internal data.

Return type

Optional[AbstractGeometricPath]

Returns

Time-parameterized joint position trajectory or None If unable to parameterize.

property constraints: List[toppra.constraint.constraint.Constraint]

Constraints of interests.

Return type

List[Constraint]

inspect(compute=True)

Inspect the problem internal data.

property problem_data: toppra.algorithm.algorithm.ParameterizationData

Data obtained when solving the path parametrization.

Return type

ParameterizationData

Path Interpolator

This module implements clases to represent geometric paths and trajectories.

SplineInterplator

class toppra.SplineInterpolator(ss_waypoints, waypoints, bc_type='not-a-knot')

Interpolate the given waypoints by cubic spline.

This interpolator is implemented as a simple wrapper over scipy’s CubicSpline class.

Parameters

• ss_waypoints (np.ndarray(m,)) – Path positions of the waypoints.

• waypoints (np.ndarray(m, d)) – Waypoints.

• bc_type (optional) –

  Boundary conditions of the spline. Can be ‘not-a-knot’, ‘clamped’, ‘natural’ or ‘periodic’.

  • ’not-a-knot’: The most default option, return the most naturally looking spline.

  • ’clamped’: First-order derivatives of the spline at the two end are clamped at zero.

  See scipy.CubicSpline documentation for more details.

__call__(path_positions, order=0)

Evaluate the path at given positions.

Parameters

• path_positions (float or np.ndarray) – Path positions to evaluate the interpolator.

• order (int) –

  Order of the evaluation call.

  • 0: position

  • 1: first-order derivative

  • 2: second-order derivative

Returns

The evaluated joint positions, velocity or accelerations. The shape of the result depends on the shape of the input, it is either (N, m) where N is the number of path positions and m is the number of degree-of-freedom, or (m,).

property dof

Return the degrees-of-freedom of the path.

property path_interval

Return the start and end points.

property waypoints

Return the waypoints.

The first element is the positions, the second element is the array of waypoints.

Type

Tuple[np.ndarray, np.ndarray]

RaveTrajectoryWrapper

class toppra.RaveTrajectoryWrapper(traj, robot)

An interpolator that wraps OpenRAVE’s GenericTrajectory.

Only trajectories using quadratic interpolation or cubic interpolation are supported. The trajectory is represented as a piecewise polynomial. The polynomial could be quadratic or cubic depending the interpolation method used by the input trajectory object.

__call__(ss_sam, order=0)

Evaluate the path at given positions.

Parameters

• path_positions (float or np.ndarray) – Path positions to evaluate the interpolator.

• order (int) –

  Order of the evaluation call.

  • 0: position

  • 1: first-order derivative

  • 2: second-order derivative

Returns

The evaluated joint positions, velocity or accelerations. The shape of the result depends on the shape of the input, it is either (N, m) where N is the number of path positions and m is the number of degree-of-freedom, or (m,).

property dof

Return the degrees-of-freedom of the path.

property path_interval

Return the start and end points.

property waypoints

The path’s waypoints if applicable. None otherwise.

Type

Tuple[ndarray, ndarray] or None

simplepath.SimplePath

class toppra.simplepath.SimplePath(x, y, yd=None)

A class for representing continuous multi-dimentional function.

This geometric path is specified by positions, velocities (optional) and acceleration (optional). Internally a scipy.PPoly instance is used to store the path. The polynomial degree depends on the input.

If velocity is not given, they will be computed automatically.

Parameters

• x (ndarray) – “Time instances” of the waypoints.

• y (ndarray) – Function values at waypoints.

• yd (Optional[ndarray]) – First-derivatives. If not given (None) will be computed automatically.

__call__(xi, order=0)

Evaluate the path at given position.

property dof

Return the degrees-of-freedom of the path.

property path_interval

Return the starting and ending path positions.

Returns

The starting and ending path positions.

Return type

np.ndarray(2,)

[abstract]AbstractGeometricPath

class toppra.interpolator.AbstractGeometricPath

Abstract base class that represents geometric paths.

Derive geometric paths classes should implement the below abstract methods. These methods are expected in different steps of the algorithm.

__call__(path_positions, order=0)

Evaluate the path at given positions.

Parameters

• path_positions (float or np.ndarray) – Path positions to evaluate the interpolator.

• order (int) –

  Order of the evaluation call.

  • 0: position

  • 1: first-order derivative

  • 2: second-order derivative

Return type

ndarray

Returns

The evaluated joint positions, velocity or accelerations. The shape of the result depends on the shape of the input, it is either (N, m) where N is the number of path positions and m is the number of degree-of-freedom, or (m,).

property dof: int

Return the degrees-of-freedom of the path.

Return type

int

property path_interval

Return the starting and ending path positions.

Returns

The starting and ending path positions.

Return type

np.ndarray(2,)

property waypoints

The path’s waypoints if applicable. None otherwise.

Type

Tuple[ndarray, ndarray] or None

[internal]Methods

toppra.interpolator.propose_gridpoints(path, max_err_threshold=0.0001, max_iteration=100, max_seg_length=0.05, min_nb_points=100)

Generate gridpoints that sufficiently cover the given path.

This function operates in multiple passes through the geometric path from the start to the end point. In each pass, for each segment, the maximum interpolation error is estimated using the following equation:

\[err_{est} = 0.5 * \mathrm{max}(\mathrm{abs}(p'' * d_{segment} ^ 2))\]

Here \(p''\) is the second derivative of the path and d_segment is the length of the segment. If the estimated error \(err_{test}\) is greater than the given threshold max_err_threshold then the segment is divided in two half.

Intuitively, at positions with higher curvature, there must be more points in order to improve approximation quality. Theoretically toppra performs the best when the proposed gridpoint is optimally distributed.

Parameters

• path (AbstractGeometricPath) – Input geometric path.

• max_err_threshold (float) – Maximum worstcase error thrshold allowable.

• max_iteration (int) – Maximum number of iterations.

• max_seg_length (float) – All segments length should be smaller than this value.

Returns

gridpoints_ept – The proposed gridpoints.

Return type

np.ndarray(N,)

Constraints

Modules implementing different dynamics constraints.

ConstraintType

class toppra.constraint.ConstraintType(value)

Type of path parametrization constraint.

CanonicalConic = 1

Linear constraints with linear conic inequalities.

CanonicalLinear = 0

Simple linear constraints with only linear inequalities

Unknown = -1

Unknown

DiscretizationType

class toppra.constraint.DiscretizationType(value)

Enum to mark different Discretization Scheme for constraint.

In general, the difference in speed is not too large. Should use Interpolation if possible.

Collocation = 0

Smaller problem size, but lower accuracy.

Interpolation = 1

Larger problem size, but higher accuracy.

LinearConstraint

class toppra.constraint.LinearConstraint

Bases: toppra.constraint.constraint.Constraint

A core type of constraints.

Also known as Second-order Constraint.

A Canonical Linear Constraint has the following form:

\[\begin{split}\mathbf a_i u + \mathbf b_i x + \mathbf c_i &= v, \\ \mathbf F_i v &\leq \mathbf g_i, \\ x^{bound}_{i, 0} \leq x &\leq x^{bound}_{i, 1}, \\ u^{bound}_{i, 0} \leq u &\leq u^{bound}_{i, 1}.\end{split}\]

Alternatively, if \(\mathbf F_i\) is constant for all values of \(i\), then we can consider the simpler constraint:

\[\begin{split}\mathbf F v &\leq \mathbf w, \\\end{split}\]

In this case, the returned value of \(F\) by compute_constraint_params has shape (k, m) instead of (N, k, m), \(w\) shape (k) instead of (N, k) and the class attribute identical will be True.

Note

Derived classes of LinearConstraint should at least implement the method compute_constraint_params().

See also

JointAccelerationConstraint JointVelocityConstraint CanonicalLinearSecondOrderConstraint

__init__()

compute_constraint_params(path, gridpoints, *args, **kwargs)

Compute numerical coefficients of the given constraint.

Parameters

• path (Interpolator) – The geometric path.

• gridpoints (np.ndarray) – Shape (N+1,). Gridpoint use for discretizing path.

Returns

• a (np.ndarray or None) – Shape (N + 1, m). See notes.

• b (np.ndarray, or None) – Shape (N + 1, m). See notes.

• c (np.ndarray or None) – Shape (N + 1, m). See notes.

• F (np.ndarray or None) – Shape (N + 1, k, m). See notes.

• g (np.ndarray or None) – Shape (N + 1, k,). See notes

• ubound (np.ndarray, or None) – Shape (N + 1, 2). See notes.

• xbound (np.ndarray or None) – Shape (N + 1, 2). See notes.

JointVelocityConstraint

class toppra.constraint.JointVelocityConstraint(vlim)

Bases: toppra.constraint.linear_constraint.LinearConstraint

A Joint Velocity Constraint class.

Parameters

vlim (np.ndarray) – Shape (dof, 2). The lower and upper velocity bounds of the j-th joint are given by vlim[j, 0] and vlim[j, 1] respectively.

__init__(vlim)

compute_constraint_params(path, gridpoints)

Compute numerical coefficients of the given constraint.

Parameters

• path (Interpolator) – The geometric path.

• gridpoints (np.ndarray) – Shape (N+1,). Gridpoint use for discretizing path.

Returns

• a (np.ndarray or None) – Shape (N + 1, m). See notes.

• b (np.ndarray, or None) – Shape (N + 1, m). See notes.

• c (np.ndarray or None) – Shape (N + 1, m). See notes.

• F (np.ndarray or None) – Shape (N + 1, k, m). See notes.

• g (np.ndarray or None) – Shape (N + 1, k,). See notes

• ubound (np.ndarray, or None) – Shape (N + 1, 2). See notes.

• xbound (np.ndarray or None) – Shape (N + 1, 2). See notes.

JointVelocityConstraintVarying

class toppra.constraint.JointVelocityConstraintVarying(vlim_func)

Bases: toppra.constraint.linear_constraint.LinearConstraint

A Joint Velocity Constraint class.

This class handle velocity constraints that vary along the path.

Parameters

vlim_func ((float) -> np.ndarray) – A function that receives a scalar (float) and produce an array with shape (dof, 2). The lower and upper velocity bounds of the j-th joint are given by out[j, 0] and out[j, 1] respectively.

__init__(vlim_func)

compute_constraint_params(path, gridpoints)

Compute numerical coefficients of the given constraint.

Parameters

• path (Interpolator) – The geometric path.

• gridpoints (np.ndarray) – Shape (N+1,). Gridpoint use for discretizing path.

Returns

• a (np.ndarray or None) – Shape (N + 1, m). See notes.

• b (np.ndarray, or None) – Shape (N + 1, m). See notes.

• c (np.ndarray or None) – Shape (N + 1, m). See notes.

• F (np.ndarray or None) – Shape (N + 1, k, m). See notes.

• g (np.ndarray or None) – Shape (N + 1, k,). See notes

• ubound (np.ndarray, or None) – Shape (N + 1, 2). See notes.

• xbound (np.ndarray or None) – Shape (N + 1, 2). See notes.

SecondOrderConstraints

class toppra.constraint.SecondOrderConstraint(inv_dyn, constraint_F, constraint_g, dof, custom_term=None, discretization_scheme=1)

Bases: toppra.constraint.linear_constraint.LinearConstraint

This class implements the linear Second-Order constraint.

Conventionally, a SecondOrderConstraint is given by the following formula:

\[A(\mathbf{q}) \ddot {\mathbf{q}} + \dot {\mathbf{q}}^\top B(\mathbf{q}) \dot {\mathbf{q}} + C(\mathbf{q}) = w,\]

where w is a vector that satisfies the polyhedral constraint:

\[F(\mathbf{q}) w \leq g(\mathbf{q}).\]

Take the example of a robot torque bound, the functions \(A, B, C\) represent respectively the inertial, Corriolis and gravitational terms of the robot’s rigid body dynamics.

We can evaluate a constraint on a given geometric path \(\mathbf{p}(s)\) using the following equations, which are obtained by direct substitution:

\[\begin{split}A(\mathbf{q}) \mathbf{p}'(s) \ddot s + [A(\mathbf{q}) \mathbf{p}''(s) + \mathbf{p}'(s)^\top B(\mathbf{q}) \mathbf{p}'(s)] \dot s^2 + C(\mathbf{q}) = w, \\ \mathbf{a}(s) \ddot s + \mathbf{b}(s) \dot s ^2 + \mathbf{c}(s) = w.\end{split}\]

where \(\mathbf{p}', \mathbf{p}''\) denote respectively the first and second derivatives of the path. It is important to understand that the vector functions \(\mathbf a, \mathbf b, \mathbf c\) are what toppra needs to solve for path parametrizations.

To evaluate these coefficients \(\mathbf a(s), \mathbf b(s), \mathbf c(s)\), fortunately, it is not necessary to have the functions \(A, B, C\) explicitly. Rather, it is only required to have the sum of the these 3 functions–the so-called inverse dynamic function:

\[\mathrm{inverse\_dyn}(\mathbf q, \dot{\mathbf q}, \ddot{\mathbf q}) := A(\mathbf{q}) \ddot {\mathbf{q}} + \dot {\mathbf{q}}^\top B(\mathbf{q}) \dot {\mathbf{q}} + C(\mathbf{q})\]

In some cases, one might have terms that depends purely on the path:

\[\mathbf{a}(s) \ddot s + \mathbf{b}(s) \dot s ^2 + \mathbf{c}(s) + \mathcal{C}(\mathbf p, s)= w.\]

an example is the joint friction. This term is referred to as custom_term in the initializing arguments of SecondOrderConstraint.

It is interesting to note that we can actually use a more general form of the above equations, hence covering a wider class of constraints. In particular, one can replace \(A(\mathbf{q}), B(\mathbf{q}), C(\mathbf{q}), F(\mathbf{q}), g(\mathbf{q})\) with \(A(\mathbf{q}, s), B(\mathbf{q}, s), C(\mathbf{q}, s), F(\mathbf{q}, s), g(\mathbf{q}, s)\). This form, however, is not implemented in toppra.

__init__(inv_dyn, constraint_F, constraint_g, dof, custom_term=None, discretization_scheme=1)

Initialize the constraint.

Parameters

• inv_dyn ((np.ndarray, np.ndarray, np.ndarray) -> np.ndarray) – The “inverse dynamics” function that receives joint position, velocity, acceleration and path position as inputs and ouputs the constrained vector \(\mathbf w\). See above for more details.

• constraint_F ((np.ndarray) -> np.ndarray) – The constraint coefficient function \(\mathbf F(\mathbf q, s)\). See above for more details.

• constraint_g ((np.ndarray) -> np.ndarray) – The constraint coefficient function \(\mathbf g(\mathbf q, s)\). See above for more details.

• dof (int) – The dimension of the joint position.

• custom_term ((Interpolator, float) -> np.ndarray) – This function receives as input a geometric path and a float path position, then returns an additive term. See the above note for more details.

• discretization_scheme (DiscretizationType) – Type of discretization.

compute_constraint_params(path, gridpoints)

Compute numerical coefficients of the given constraint.

Parameters

• path (Interpolator) – The geometric path.

• gridpoints (np.ndarray) – Shape (N+1,). Gridpoint use for discretizing path.

Returns

• a (np.ndarray or None) – Shape (N + 1, m). See notes.

• b (np.ndarray, or None) – Shape (N + 1, m). See notes.

• c (np.ndarray or None) – Shape (N + 1, m). See notes.

• F (np.ndarray or None) – Shape (N + 1, k, m). See notes.

• g (np.ndarray or None) – Shape (N + 1, k,). See notes

• ubound (np.ndarray, or None) – Shape (N + 1, 2). See notes.

• xbound (np.ndarray or None) – Shape (N + 1, 2). See notes.

classmethod joint_torque_constraint(inv_dyn, taulim, joint_friction, **kwargs)

Initialize a Joint Torque constraint.

Parameters

• inv_dyn ((np.ndarray, np.ndarray, np.ndarray) -> np.ndarray) – Inverse dynamic function of the robot.

• taulim (np.ndarray) – Shape (N, 2). The i-th element contains the minimum and maximum joint torque limits respectively.

• joint_friction (np.ndarray) – Shape (N,). The i-th element contains the dry friction (torque) in the i-th joint.

JointTorqueConstraint

class toppra.constraint.JointTorqueConstraint(inv_dyn, tau_lim, fs_coef, discretization_scheme=DiscretizationType.Collocation)

Bases: toppra.constraint.linear_constraint.LinearConstraint

Joint Torque Constraint.

A joint torque constraint is given by

\[A(q) \ddot q + \dot q^\top B(q) \dot q + C(q) + D( \dot q )= w,\]

where w is a vector that satisfies the polyhedral constraint:

\[F(q) w \leq g(q).\]

Notice that inv_dyn(q, qd, qdd) = w and that cnsf_coeffs(q) = F(q), g(q).

To evaluate the constraint on a geometric path p(s), multiple calls to inv_dyn and const_coeff are made. Specifically one can derive the second-order equation as follows

\[A(q) p'(s) \ddot s + [A(q) p''(s) + p'(s)^\top B(q) p'(s)] \dot s^2 + C(q) + D( \dot q ) = w, a(s) \ddot s + b(s) \dot s ^2 + c(s) = w\]

To evaluate the coefficients a(s), b(s), c(s), inv_dyn is called repeatedly with appropriate arguments.

Parameters

• inv_dyn ((array, array, array) -> array) – The “inverse dynamics” function that receives joint position, velocity and acceleration as inputs and ouputs the “joint torque”. See notes for more details.

• tau_lim (array) – Shape (dof, 2). The lower and upper torque bounds of the j-th joint are tau_lim[j, 0] and tau_lim[j, 1] respectively.

• fs_coef (array) – Shape (dof). The coefficients of dry friction of the joints.

• discretization_scheme (DiscretizationType) – Can be either Collocation (0) or Interpolation (1). Interpolation gives more accurate results with slightly higher computational cost.

__init__(inv_dyn, tau_lim, fs_coef, discretization_scheme=DiscretizationType.Collocation)

compute_constraint_params(path, gridpoints)

Compute numerical coefficients of the given constraint.

Parameters

• path (Interpolator) – The geometric path.

• gridpoints (np.ndarray) – Shape (N+1,). Gridpoint use for discretizing path.

Returns

• a (np.ndarray or None) – Shape (N + 1, m). See notes.

• b (np.ndarray, or None) – Shape (N + 1, m). See notes.

• c (np.ndarray or None) – Shape (N + 1, m). See notes.

• F (np.ndarray or None) – Shape (N + 1, k, m). See notes.

• g (np.ndarray or None) – Shape (N + 1, k,). See notes

• ubound (np.ndarray, or None) – Shape (N + 1, 2). See notes.

• xbound (np.ndarray or None) – Shape (N + 1, 2). See notes.

JointAccelerationConstraint

class toppra.constraint.JointAccelerationConstraint(alim, discretization_scheme=DiscretizationType.Interpolation)

Bases: toppra.constraint.linear_constraint.LinearConstraint

The Joint Acceleration Constraint class.

A joint acceleration constraint is given by

\[\begin{split}\ddot{\mathbf{q}}_{min} & \leq \ddot{\mathbf q} &\leq \ddot{\mathbf{q}}_{max} \\ \ddot{\mathbf{q}}_{min} & \leq \mathbf{q}'(s_i) u_i + \mathbf{q}''(s_i) x_i &\leq \ddot{\mathbf{q}}_{max}\end{split}\]

where \(u_i, x_i\) are respectively the path acceleration and path velocity square at \(s_i\). For more detail see Derivation of kinematical quantities.

Rearranging the above pair of vector inequalities into the form required by LinearConstraint, we have:

• a[i] := \(\mathbf q'(s_i)\)

• b[i] := \(\mathbf q''(s_i)\)

• F := \([\mathbf{I}, -\mathbf I]^T\)

• h := \([\ddot{\mathbf{q}}_{max}^T, -\ddot{\mathbf{q}}_{min}^T]^T\)

__init__(alim, discretization_scheme=DiscretizationType.Interpolation)

Initialize the joint acceleration class.

Parameters

• alim (array) – Shape (dof, 2). The lower and upper acceleration bounds of the j-th joint are alim[j, 0] and alim[j, 1] respectively.

• discretization_scheme (DiscretizationType) – Can be either Collocation (0) or Interpolation (1). Interpolation gives more accurate results with slightly higher computational cost.

compute_constraint_params(path, gridpoints, *args, **kwargs)

Compute numerical coefficients of the given constraint.

Parameters

• path (Interpolator) – The geometric path.

• gridpoints (np.ndarray) – Shape (N+1,). Gridpoint use for discretizing path.

Returns

• a (np.ndarray or None) – Shape (N + 1, m). See notes.

• b (np.ndarray, or None) – Shape (N + 1, m). See notes.

• c (np.ndarray or None) – Shape (N + 1, m). See notes.

• F (np.ndarray or None) – Shape (N + 1, k, m). See notes.

• g (np.ndarray or None) – Shape (N + 1, k,). See notes

• ubound (np.ndarray, or None) – Shape (N + 1, 2). See notes.

• xbound (np.ndarray or None) – Shape (N + 1, 2). See notes.

RobustLinearConstraint

class toppra.constraint.RobustLinearConstraint(cnst, ellipsoid_axes_lengths, discretization_scheme=DiscretizationType.Collocation)

Bases: toppra.constraint.conic_constraint.ConicConstraint

The simple canonical conic constraint.

This constraint can be seen as a robustified version of a CanonicalLinear constraint. In particular, the perturbations term, [Delta a[i, j], Delta b[i, j], Delta c[i, j]] is assumed to lie in a centered ellipsoid:

\[[\Delta a[i, j], \Delta b[i, j], \Delta c[i, j]]^\top = diag(ru, rx, rc) \mathbf e,\]

where |mathbf e|_2 leq 1.

Parameters

• cnst (LinearConstraint) – The base constraint to robustify.

• ellipsoid_axes_lengths ((3,)array) – Lengths of the axes of the perturbation ellipsoid. Must all be non-negative.

• discretization_scheme (DiscretizationType) – Constraint discretization scheme to use.

compute_constraint_params(path, gridpoints)

Evaluate parameters of the constraint.

[abstract]Constraint

class toppra.constraint.Constraint

The base constraint class.

__repr__()

Return repr(self).

compute_constraint_params(path, gridpoints, *args, **kwargs)

Evaluate parameters of the constraint.

set_discretization_type(discretization_type)

Discretization type: Collocation or Interpolation.

Parameters

discretization_type (int or DiscretizationType) – Method to discretize this constraint.

[internal]Methods

toppra.constraint.canlinear_colloc_to_interpolate(a, b, c, F, g, xbound, ubound, gridpoints, identical=False)

Convert a set of parameters to the interpolation discretization scheme.

If a set of parameters is None, the resulting set is also None.

Parameters

• a (np.ndarray or None) – Shape (N + 1, m). See notes.

• b (np.ndarray or None) – Shape (N + 1, m). See notes.

• c (np.ndarray or None) – Shape (N + 1, m). See notes.

• F (np.ndarray or None) – Shape (N + 1, k, m). See notes.

• g (np.ndarray or None) – Shape (N + 1, k,). See notes

• ubound (np.ndarray, or None) – Shape (N + 1, 2). See notes.

• xbound (np.ndarray or None) – Shape (N + 1, 2). See notes.

• gridpoints (np.ndarray) – Shape (N+1,). The path discretization.

• identical (bool, optional) – If True, matrices F and g are identical at all gridpoint.

Returns

• a_intp (np.ndarray, or None) – Shape (N + 1, m). See notes.

• b_intp (np.ndarray, or None) – Shape (N + 1, m). See notes.

• c_intp (np.ndarray, or None) – Shape (N + 1, m). See notes.

• F_intp (np.ndarray, or None) – Shape (N + 1, k, m). See notes.

• g_intp (np.ndarray, or None) – Shape (N + 1, k,). See notes

• ubound (np.ndarray, or None) – Shape (N + 1, 2). See notes.

• xbound (np.ndarray, or None) – Shape (N + 1, 2). See notes.

toppra.solverwrapper

All computations in TOPP-RA algorithms are done by the linear and quadratic solvers, wrapped in solver wrappers.

SolverWrapper

class toppra.solverwrapper.SolverWrapper(constraint_list, path, path_discretization)

The base class for all solver wrappers.

All SolverWrapper have to implement a core method needed by all Reachability Analysis-based algorithms: solve_stagewise_optim. This methods solves a Linear/Quadratic Program subject to linear constraints at the given stage, and possibly with additional auxiliary constraints.

Note that some SolverWrappers only handle Linear Program while some handle both.

Certain solver wrappers need to be setup and close down before and after usage. For instance, the wrappers for mosek and qpOASES with warmstart capability. To provide this functionality, this class contains two abstract methods setup_solver and close_solver, which should be called before and after any call to solve_stagewise_optim, so that necessary setups can be made.

Each solver wrapper should provide solver-specific constraint, such as ultimate bound the variable u, x. For some solvers such as ECOS, this is very important.

Variables

• constraints (toppra.Constraint []) – Constraints on the robot system.

• path (Interpolator) – The geometric path to be time-parametrized.

• path_discretization (array) – The discretization grid use to discretize the geometric path.

get_no_stages()

Return the number of stages.

The number of gridpoints equals N + 1, where N is the number of stages.

get_no_vars()

Return total number of variables, including u, x.

solve_stagewise_optim(i, H, g, x_min, x_max, x_next_min, x_next_max)

Solve a stage-wise quadratic (or linear) optimization problem.

The quadratic optimization problem is described below:

\[\begin{split}\text{min } & 0.5 [u, x, v] H [u, x, v]^\top + [u, x, v] g \\ \text{s.t. } & [u, x] \text{ is feasible at stage } i \\ & x_{min} \leq x \leq x_{max} \\ & x_{next, min} \leq x + 2 \Delta_i u \leq x_{next, max},\end{split}\]

where v is an auxiliary variable, only exist if there are non-canonical constraints. The linear program is the quadratic problem without the quadratic term.

Parameters

• i (int) – The stage index.

• H (ndarray or None) – Shape (d, d). The coefficient of the quadratic objective function. If is None, set the quadratic term to zero.

• g (ndarray) – Shape (d,). The linear term.

• x_min (float) – If not specified, set to NaN.

• x_max (float) – If not specified, set to NaN.

• x_next_min (float) – If not specified, set to NaN.

• x_next_max (float) – If not specified, set to NaN.

Returns

If successes, return a double array containing the optimal variable. Otherwise return a array that contains numpy.nan.

Return type

double ndarray

hotqpOASESSolverWrapper

class toppra.solverwrapper.hotqpOASESSolverWrapper(constraint_list, path, path_discretization, disable_check=False, scaling_solverwrapper=True)

qpOASES solver wrapper with hot-start.

This wrapper takes advantage of the warm-start capability of the qpOASES quadratic programming solver. It uses two different qp solvers. One to solve for maximized controllable sets and one to solve for minimized controllable sets. The wrapper selects which solver to use by looking at the optimization direction.

This solver wrapper also scale data before invoking qpOASES.

If the logger “toppra” is set to debug level, qpoases solvers are initialized with PrintLevel.HIGH. Otherwise, these are initialized with PrintLevel.NONE

Currently only support Canonical Linear Constraints.

Parameters

• constraint_list (Constraint []) – The constraints the robot is subjected to.

• path (Interpolator) – The geometric path.

• path_discretization (array) – The discretized path positions.

• disable_check (bool, optional) – Disable check for solution validity. Improve speed by about 20% but entails the possibility that failure is not reported correctly.

• scaling_solverwrapper (bool, optional) – If is True, try to scale the data of each optimization before running. Important: Currently scaling is always done regardless of the value of this variable. To be fixed.

setup_solver()

Initiate two internal solvers for warm-start.

solve_stagewise_optim(i, H, g, x_min, x_max, x_next_min, x_next_max)

Solve a stage-wise quadratic (or linear) optimization problem.

The quadratic optimization problem is described below:

\[\begin{split}\text{min } & 0.5 [u, x, v] H [u, x, v]^\top + [u, x, v] g \\ \text{s.t. } & [u, x] \text{ is feasible at stage } i \\ & x_{min} \leq x \leq x_{max} \\ & x_{next, min} \leq x + 2 \Delta_i u \leq x_{next, max},\end{split}\]

where v is an auxiliary variable, only exist if there are non-canonical constraints. The linear program is the quadratic problem without the quadratic term.

Parameters

• i (int) – The stage index.

• H (ndarray or None) – Shape (d, d). The coefficient of the quadratic objective function. If is None, set the quadratic term to zero.

• g (ndarray) – Shape (d,). The linear term.

• x_min (float) – If not specified, set to NaN.

• x_max (float) – If not specified, set to NaN.

• x_next_min (float) – If not specified, set to NaN.

• x_next_max (float) – If not specified, set to NaN.

Returns

If successes, return a double array containing the optimal variable. Otherwise return a array that contains numpy.nan.

Return type

double ndarray

qpOASESSolverWrapper

class toppra.solverwrapper.qpOASESSolverWrapper(constraint_list, path, path_discretization)

A solver wrapper using qpOASES.

Parameters

• constraint_list (list of Constraint) – The constraints the robot is subjected to.

• path (Interpolator) – The geometric path.

• path_discretization (array) – The discretized path positions.

solve_stagewise_optim(i, H, g, x_min, x_max, x_next_min, x_next_max)

Solve a stage-wise quadratic (or linear) optimization problem.

The quadratic optimization problem is described below:

\[\begin{split}\text{min } & 0.5 [u, x, v] H [u, x, v]^\top + [u, x, v] g \\ \text{s.t. } & [u, x] \text{ is feasible at stage } i \\ & x_{min} \leq x \leq x_{max} \\ & x_{next, min} \leq x + 2 \Delta_i u \leq x_{next, max},\end{split}\]

where v is an auxiliary variable, only exist if there are non-canonical constraints. The linear program is the quadratic problem without the quadratic term.

Parameters

• i (int) – The stage index.

• H (ndarray or None) – Shape (d, d). The coefficient of the quadratic objective function. If is None, set the quadratic term to zero.

• g (ndarray) – Shape (d,). The linear term.

• x_min (float) – If not specified, set to NaN.

• x_max (float) – If not specified, set to NaN.

• x_next_min (float) – If not specified, set to NaN.

• x_next_max (float) – If not specified, set to NaN.

Returns

If successes, return a double array containing the optimal variable. Otherwise return a array that contains numpy.nan.

Return type

double ndarray

seidelWrapper

class toppra.solverwrapper.seidelWrapper(list constraint_list, path, path_discretization)

A solver wrapper that implements Seidel’s LP algorithm.

This wrapper can only be used if there is only Canonical Linear Constraints.

Parameters

• constraint_list (list of Constraint) – The constraints the robot is subjected to.

• path (Interpolator) – The geometric path.

• path_discretization (array) – The discretized path positions.

solve_stagewise_optim(self, unsigned int i, H, ndarray g, double x_min, double x_max, double x_next_min, double x_next_max)

Solve a stage-wise linear optimization problem.

The linear optimization problem is described below.

\[\begin{split}\text{min } & [u, x] g \\ \text{s.t. } & [u, x] \text{ is feasible at stage } i \\ & x_{min} \leq x \leq x_{max} \\ & x_{next, min} \leq x + 2 \Delta_i u \leq x_{next, max},\end{split}\]

TODO if x_min == x_max, one can solve an LP instead of a 2D LP. This optimization is currently not implemented.

Parameters

• i (int) – The stage index. See notes for details on each variable.

• H (array or None) – This term is not used and is neglected.

• g ((d,)array) – The linear term.

• x_min (float) – If not specified, set to NaN.

• x_max (float) – If not specified, set to NaN.

• x_next_min (float) – If not specified, set to NaN.

• x_next_max (float) – If not specified, set to NaN.

Returns

If successes, return an array containing the optimal variable. Since NaN is also a valid double, this list contains NaN if the optimization problem is infeasible.

Return type

double C array or list

ecosWrapper

class toppra.solverwrapper.ecosWrapper(constraint_list, path, path_discretization)

A solver wrapper that handles linear and conic-quadratic constraints using ECOS.

ecosWrapper and cvxpyWrapper are the only wrappers that can handle conic-quadratic constraints, which are necessary to compute robust path parameterization.

Notes

To reduce numerical-related issues, ECOS_MAXX is used to regulate the magnitude of the solution.

ECOS is not very well implemented. There are many cases in which the solver fails simply because there is a very large bound (>1e6). Because of this, the test suites included in toppra do not include many tests for ecos.

Variables

• constraints (list of Constraint) – Constraints on the robot system.

• path (Interpolator) – The geometric path to be time-parametrized.

• path_discretization (array) – The discretization grid use to discretize the geometric path.

solve_stagewise_optim(i, H, g, x_min, x_max, x_next_min, x_next_max)

Solve a stage-wise quadratic (or linear) optimization problem.

The quadratic optimization problem is described below:

\[\begin{split}\text{min } & 0.5 [u, x, v] H [u, x, v]^\top + [u, x, v] g \\ \text{s.t. } & [u, x] \text{ is feasible at stage } i \\ & x_{min} \leq x \leq x_{max} \\ & x_{next, min} \leq x + 2 \Delta_i u \leq x_{next, max},\end{split}\]

where v is an auxiliary variable, only exist if there are non-canonical constraints. The linear program is the quadratic problem without the quadratic term.

Parameters

• i (int) – The stage index.

• H (ndarray or None) – Shape (d, d). The coefficient of the quadratic objective function. If is None, set the quadratic term to zero.

• g (ndarray) – Shape (d,). The linear term.

• x_min (float) – If not specified, set to NaN.

• x_max (float) – If not specified, set to NaN.

• x_next_min (float) – If not specified, set to NaN.

• x_next_max (float) – If not specified, set to NaN.

Returns

If successes, return a double array containing the optimal variable. Otherwise return a array that contains numpy.nan.

Return type

double ndarray