pydrake.trajectories

class pydrake.trajectories.BsplineTrajectory

Bases: pydrake.trajectories.Trajectory

Represents a B-spline curve using a given basis with ordered control_points such that each control point is a matrix in ℝʳᵒʷˢ ˣ ᶜᵒˡˢ.

See also

math::BsplineBasis

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.trajectories.BsplineTrajectory) -> None
2. __init__(self: pydrake.trajectories.BsplineTrajectory, basis: pydrake.math.BsplineBasis_[float], control_points: List[numpy.ndarray[numpy.float64[m, n]]]) -> None

Constructs a B-spline trajectory with the given basis and control_points.

Precondition:

control_points.size() == basis.num_basis_functions()

basis(self: pydrake.trajectories.BsplineTrajectory) → pydrake.math.BsplineBasis_[float]

Returns the basis of this curve.

Clone(self: pydrake.trajectories.BsplineTrajectory) → pydrake.trajectories.Trajectory

control_points(self: pydrake.trajectories.BsplineTrajectory) → List[numpy.ndarray[numpy.float64[m, n]]]

Returns the control points of this curve.

CopyBlock(self: pydrake.trajectories.BsplineTrajectory, start_row: int, start_col: int, block_rows: int, block_cols: int) → pydrake.trajectories.BsplineTrajectory

Returns a new BsplineTrajectory that uses the same basis as this, and whose control points are the result of calling point.block(start_row, start_col, block_rows, block_cols) on each point in this->control_points().

CopyHead(self: pydrake.trajectories.BsplineTrajectory, n: int) → pydrake.trajectories.BsplineTrajectory

Returns a new BsplineTrajectory that uses the same basis as this, and whose control points are the result of calling point.head(n) on each point in this->control_points().

Precondition:

this->cols() == 1

Precondition:

control_points()[0].head(n) must be a valid operation.

FinalValue(self: pydrake.trajectories.BsplineTrajectory) → numpy.ndarray[numpy.float64[m, n]]

Returns this->value(this->end_time())

InitialValue(self: pydrake.trajectories.BsplineTrajectory) → numpy.ndarray[numpy.float64[m, n]]

Returns this->value(this->start_time())

InsertKnots(self: pydrake.trajectories.BsplineTrajectory, additional_knots: List[float]) → None

Adds new knots at the specified additional_knots without changing the behavior of the trajectory. The basis and control points of the trajectory are adjusted such that it produces the same value for any valid time before and after this method is called. The resulting trajectory is guaranteed to have the same level of continuity as the original, even if knot values are duplicated. Note that additional_knots need not be sorted.

Precondition:

start_time() <= t <= end_time() for all t in additional_knots

num_control_points(self: pydrake.trajectories.BsplineTrajectory) → int

Returns the number of control points in this curve.

class pydrake.trajectories.PiecewisePolynomial

Bases: pydrake.trajectories.PiecewiseTrajectory

A scalar multi-variate piecewise polynomial.

PiecewisePolynomial represents a list of contiguous segments in a scalar independent variable (typically corresponding to time) with Polynomials defined at each segment. We call the output from evaluating the PiecewisePolynomial at the scalar independent variable “the output”, and that output can be either a Eigen MatrixX<T> (if evaluated using value()) or a scalar (if evaluated using scalar_value()).

An example of a piecewise polynomial is a function of m segments in time, where a different polynomial is defined for each segment. For a specific example, consider the absolute value function over the interval [-1, 1]. We can define a PiecewisePolynomial over this interval using breaks at t = { -1.0, 0.0, 1.0 }, and “samples” of abs(t).

// Construct the PiecewisePolynomial.
const std::vector<double> breaks = { -1.0, 0.0, 1.0 };
std::vector<Eigen::MatrixXd> samples(3);
for (int i = 0; i < static_cast<int>(breaks.size()); ++i) {
  samples[i].resize(1, 1);
  samples[i](0, 0) = std::abs(breaks[i]);
}
const auto pp =
     PiecewisePolynomial<double>::FirstOrderHold(breaks, samples);
const int row = 0, col = 0;

// Evaluate the PiecewisePolynomial at some values.
std::cout << pp.value(-.5)(row, col) << std::endl;    // Outputs 0.5.
std::cout << pp.value(0.0)(row, col) << std::endl;    // Outputs 0.0;

// Show how we can evaluate the first derivative (outputs -1.0).
std::cout << pp.derivative(1).value(-.5)(row, col) << std::endl;

A note on terminology. For piecewise-polynomial interpolation, we use breaks to indicate the scalar (e.g. times) which form the boundary of each segment. We use samples to indicate the function value at the breaks, e.g. p(breaks[i]) = samples[i]. The term knot should be reserved for the “(x,y)” coordinate, here knot[i] = (breaks[i], samples[i]), though it is used inconsistently in the interpolation literature (sometimes for breaks, sometimes for samples), so we try to mostly avoid it here.

PiecewisePolynomial objects can be added, subtracted, and multiplied. They cannot be divided because Polynomials are not closed under division.

Warning

PiecewisePolynomial silently clips input evaluations outside of the range defined by the breaks. So pp.value(-2.0, row, col) in the example above would evaluate to -1.0. See value().

@tparam_default_scalars

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.trajectories.PiecewisePolynomial) -> None

Constructs an empty piecewise polynomial.

2. __init__(self: pydrake.trajectories.PiecewisePolynomial, arg0: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) -> None

Single segment, constant value constructor over the interval [-∞, ∞]. The constructed PiecewisePolynomial will return constant_value at every evaluated point (i.e., value(t) = constant_value ∀t ∈ [-∞, ∞]).

3. __init__(self: pydrake.trajectories.PiecewisePolynomial, arg0: List[numpy.ndarray[object[m, n]]], arg1: List[float]) -> None

Constructs a PiecewisePolynomial using matrix-output Polynomials defined over each segment.

Precondition:

polynomials.size() == breaks.size() - 1

4. __init__(self: pydrake.trajectories.PiecewisePolynomial, arg0: List[pydrake.polynomial.Polynomial], arg1: List[float]) -> None

Constructs a PiecewisePolynomial using scalar-output Polynomials defined over each segment.

Precondition:

polynomials.size() == breaks.size() - 1

AppendCubicHermiteSegment(self: pydrake.trajectories.PiecewisePolynomial, time: float, sample: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], sample_dot: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) → None

The CubicHermite spline construction has a nice property of being incremental (each segment can be solved independently). Given a new sample and it’s derivative, this method adds one segment to the end of this where the start sample and derivative are taken as the value and derivative at the final break of this.

Precondition:

this is not empty()

Precondition:

time > end_time()

Precondition:

sample and sample_dot must have size rows() x cols().

AppendFirstOrderSegment(self: pydrake.trajectories.PiecewisePolynomial, time: float, sample: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) → None

Given a new sample, this method adds one segment to the end of this using a first-order hold, where the start sample is taken as the value at the final break of this.

Block(self: pydrake.trajectories.PiecewisePolynomial, start_row: int, start_col: int, block_rows: int, block_cols: int) → pydrake.trajectories.PiecewisePolynomial

Extracts a trajectory representing a block of size (block_rows, block_cols) starting at (start_row, start_col) from the PiecewisePolynomial.

Returns:a PiecewisePolynomial such that ret.value(t) = this.value(t).block(i,j,p,q);

ConcatenateInTime(self: pydrake.trajectories.PiecewisePolynomial, other: pydrake.trajectories.PiecewisePolynomial) → None

Concatenates other to the end of this.

Warning

The resulting PiecewisePolynomial will only be continuous to the degree that the first Polynomial of other is continuous with the last Polynomial of this. See warning about evaluating discontinuous derivatives at breaks in derivative().

Parameter other:

PiecewisePolynomial instance to concatenate.

Raises:

• RuntimeError if trajectories’ dimensions do not match each other
• (either rows() or cols() does not match between this and
• other).

Raises:

• RuntimeError if this->end_time() and other->start_time()
• are not within PiecewiseTrajectory<T>::kEpsilonTime from each
• other.

static CubicHermite(breaks: numpy.ndarray[numpy.float64[m, 1]], samples: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], samples_dot: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) → pydrake.trajectories.PiecewisePolynomial

Version of CubicHermite(breaks, samples, samples_dot) that uses vector samples and Eigen VectorXd / MatrixX<T> arguments. Corresponding columns of samples and samples_dot are used as the sample point and independent variable derivative, respectively.

Precondition:

samples.cols() == samples_dot.cols() == breaks.size().

static CubicShapePreserving(breaks: numpy.ndarray[numpy.float64[m, 1]], samples: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], zero_end_point_derivatives: bool = False) → pydrake.trajectories.PiecewisePolynomial

Version of CubicShapePreserving(breaks, samples, zero_end_point_derivatives) that uses vector samples and Eigen VectorXd and MatrixX<T> arguments. Each column of samples represents a sample point.

Precondition:

samples.cols() == breaks.size().

Raises:

• RuntimeError under the conditions specified under
• coefficient_construction_methods.

static CubicWithContinuousSecondDerivatives(*args, **kwargs)

Overloaded function.

1. CubicWithContinuousSecondDerivatives(breaks: numpy.ndarray[numpy.float64[m, 1]], samples: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], sample_dot_at_start: numpy.ndarray[numpy.float64[m, 1]], sample_dot_at_end: numpy.ndarray[numpy.float64[m, 1]]) -> pydrake.trajectories.PiecewisePolynomial

Version of CubicWithContinuousSecondDerivatives() that uses vector samples and Eigen VectorXd / MatrixX<T> arguments. Each column of samples represents a sample point.

Precondition:

samples.cols() == breaks.size().

Raises:

• RuntimeError under the conditions specified under
• coefficient_construction_methods.

2. CubicWithContinuousSecondDerivatives(breaks: numpy.ndarray[numpy.float64[m, 1]], samples: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], periodic_end: bool) -> pydrake.trajectories.PiecewisePolynomial

Version of CubicWithContinuousSecondDerivatives(breaks, samples) that uses vector samples and Eigen VectorXd / MatrixX<T> arguments. Each column of samples represents a sample point.

Precondition:

samples.cols() == breaks.size().

derivative(self: pydrake.trajectories.PiecewisePolynomial, derivative_order: int = 1) → pydrake.trajectories.PiecewisePolynomial

Returns a PiecewisePolynomial where each segment is the specified derivative of the corresponding segment in this. Any rules or limitations of Polynomial::derivative() also apply to this function.

Derivatives evaluated at non-differentiable points return the value at the left hand side of the interval.

Parameter derivative_order:

The order of the derivative, namely, if derivative_order = n, the n’th derivative of the polynomial will be returned.

Warning

In the event of discontinuous derivatives evaluated at breaks, it is not defined which polynomial (i.e., to the left or right of the break) will be the one that is evaluated at the break.

static FirstOrderHold(arg0: numpy.ndarray[numpy.float64[m, 1]], arg1: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) → pydrake.trajectories.PiecewisePolynomial

Version of FirstOrderHold(breaks, samples) that uses vector samples and Eigen VectorXd / MatrixX<T> arguments. Each column of samples represents a sample point.

Precondition:

samples.cols() == breaks.size()

Raises:

• RuntimeError under the conditions specified under
• coefficient_construction_methods.

getPolynomial(self: pydrake.trajectories.PiecewisePolynomial, segment_index: int, row: int = 0, col: int = 0) → pydrake.polynomial.Polynomial

Gets the Polynomial with the given matrix row and column index that corresponds to the given segment index. Equivalent to getPolynomialMatrix(segment_index)(row, col).

Note

Calls PiecewiseTrajectory<T>::segment_number_range_check() to validate segment_index.

getPolynomialMatrix(self: pydrake.trajectories.PiecewisePolynomial, segment_index: int) → numpy.ndarray[object[m, n]]

Gets the matrix of Polynomials corresponding to the given segment index.

Warning

segment_index is not checked for validity.

getSegmentPolynomialDegree(self: pydrake.trajectories.PiecewisePolynomial, segment_index: int, row: int = 0, col: int = 0) → int

Gets the degree of the Polynomial with the given matrix row and column index that corresponds to the given segment index. Equivalent to getPolynomial(segment_index, row, col).GetDegree().

isApprox(self: pydrake.trajectories.PiecewisePolynomial, other: pydrake.trajectories.PiecewisePolynomial, tol: float, tol_type: pydrake.common._module_py.ToleranceType = ToleranceType.kRelative) → bool

Checks whether a PiecewisePolynomial is approximately equal to this one by calling Polynomial<T>::CoefficientsAlmostEqual() on every element of every segment.

See also

Polynomial<T>::CoefficientsAlmostEqual().

static LagrangeInterpolatingPolynomial(times: numpy.ndarray[numpy.float64[m, 1]], samples: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) → pydrake.trajectories.PiecewisePolynomial

Version of LagrangeInterpolatingPolynomial(times, samples) that uses vector samples and Eigen VectorXd / MatrixX<T> arguments. Each column of samples represents a sample point.

Precondition:

samples.cols() == times.size().

RemoveFinalSegment(self: pydrake.trajectories.PiecewisePolynomial) → None

Removes the final segment from the trajectory, reducing the number of segments by 1.

Precondition:

this is not empty()

Reshape(self: pydrake.trajectories.PiecewisePolynomial, rows: int, cols: int) → None

Reshapes the dimensions of the Eigen::MatrixX<T> returned by value(), EvalDerivative(), etc.

Precondition:

rows x cols must equal this.rows() * this.cols().

See also

Eigen::PlainObjectBase::resize().

ReverseTime(self: pydrake.trajectories.PiecewisePolynomial) → None

Modifies the trajectory so that pp_after(t) = pp_before(-t).

Note

The new trajectory will evaluate differently at precisely the break points if the original trajectory was discontinuous at the break points. This is because the segments are defined on the half-open intervals [breaks(i), breaks(i+1)), and the order of the breaks have been reversed.

ScaleTime(self: pydrake.trajectories.PiecewisePolynomial, scale: float) → None

Scales the time of the trajectory by non-negative scale (use ReverseTime() if you want to also negate time). The resulting polynomial evaluates to pp_after(t) = pp_before(t/scale).

As an example, `scale`=2 will result in a trajectory that is twice as long (start_time() and end_time() have both doubled).

setPolynomialMatrixBlock(self: pydrake.trajectories.PiecewisePolynomial, replacement: numpy.ndarray[object[m, n]], segment_index: int, row_start: int = 0, col_start: int = 0) → None

Replaces the specified block of the PolynomialMatrix at the given segment index.

Note

Calls PiecewiseTrajectory<T>::segment_number_range_check() to validate segment_index.

Warning

This code relies upon Eigen to verify that the replacement block is not too large.

shiftRight(self: pydrake.trajectories.PiecewisePolynomial, offset: float) → None

Adds offset to all of the breaks. offset need not be a non-negative number. The resulting polynomial will evaluate to pp_after(t) = pp_before(t-offset).

As an example, `offset`=2 will result in the start_time() and end_time() being 2 seconds later.

slice(self: pydrake.trajectories.PiecewisePolynomial, start_segment_index: int, num_segments: int) → pydrake.trajectories.PiecewisePolynomial

Returns the PiecewisePolynomial comprising the num_segments segments starting at the specified start_segment_index.

Note

Calls PiecewiseTrajectory<T>::segment_number_range_check() to validate segment_index.

static ZeroOrderHold(arg0: numpy.ndarray[numpy.float64[m, 1]], arg1: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) → pydrake.trajectories.PiecewisePolynomial

Version of ZeroOrderHold(breaks, samples) that uses vector samples and Eigen VectorXd/MatrixX<T> arguments. Each column of samples represents a sample point.

Precondition:

samples.cols() == breaks.size()

Raises:

• RuntimeError under the conditions specified under
• coefficient_construction_methods.

class pydrake.trajectories.PiecewiseQuaternionSlerp

Bases: pydrake.trajectories.PiecewiseTrajectory

A class representing a trajectory for quaternions that are interpolated using piecewise slerp (spherical linear interpolation). All the orientation samples are expected to be with respect to the same parent reference frame, i.e. q_i represents the rotation R_PBi for the orientation of frame B at the ith sample in a fixed parent frame P. The world frame is a common choice for the parent frame. The angular velocity and acceleration are also relative to the parent frame and expressed in the parent frame. Since there is a sign ambiguity when using quaternions to represent orientation, namely q and -q represent the same orientation, the internal quaternion representations ensure that q_n.dot(q_{n+1}) >= 0. Another intuitive way to think about this is that consecutive quaternions have the shortest geodesic distance on the unit sphere.

@tparam_default_scalars

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.trajectories.PiecewiseQuaternionSlerp) -> None

Builds an empty PiecewiseQuaternionSlerp.

2. __init__(self: pydrake.trajectories.PiecewiseQuaternionSlerp, breaks: List[float], quaternions: List[pydrake.common.eigen_geometry.Quaternion_[float]]) -> None

Builds a PiecewiseQuaternionSlerp.

Raises:

• RuntimeError if breaks and quaternions have different length, or
• breaks have length < 2.

3. __init__(self: pydrake.trajectories.PiecewiseQuaternionSlerp, breaks: List[float], rotation_matrices: List[numpy.ndarray[numpy.float64[3, 3]]]) -> None

Builds a PiecewiseQuaternionSlerp.

Raises:

• RuntimeError if breaks and rot_matrices have different length, or
• breaks have length < 2.

4. __init__(self: pydrake.trajectories.PiecewiseQuaternionSlerp, breaks: List[float], rotation_matrices: List[pydrake.math.RotationMatrix_[float]]) -> None

Builds a PiecewiseQuaternionSlerp.

Raises:

• RuntimeError if breaks and rot_matrices have different length, or
• breaks have length < 2.

5. __init__(self: pydrake.trajectories.PiecewiseQuaternionSlerp, breaks: List[float], angle_axes: List[pydrake.common.eigen_geometry.AngleAxis_[float]]) -> None

Builds a PiecewiseQuaternionSlerp.

Raises:

• RuntimeError if breaks and ang_axes have different length, or
• breaks have length < 2.

angular_acceleration(self: pydrake.trajectories.PiecewiseQuaternionSlerp, time: float) → numpy.ndarray[numpy.float64[3, 1]]

Interpolates angular acceleration.

Parameter time:

Time for interpolation.

Returns:The interpolated angular acceleration at time, which is always zero for slerp.

angular_velocity(self: pydrake.trajectories.PiecewiseQuaternionSlerp, time: float) → numpy.ndarray[numpy.float64[3, 1]]

Interpolates angular velocity.

Parameter time:

Time for interpolation.

Returns:The interpolated angular velocity at time, which is constant per segment.

Append(*args, **kwargs)

Overloaded function.

1. Append(self: pydrake.trajectories.PiecewiseQuaternionSlerp, time: float, quaternion: pydrake.common.eigen_geometry.Quaternion_[float]) -> None

Given a new Quaternion, this method adds one segment to the end of this.

2. Append(self: pydrake.trajectories.PiecewiseQuaternionSlerp, time: float, rotation_matrix: pydrake.math.RotationMatrix_[float]) -> None

Given a new RotationMatrix, this method adds one segment to the end of this.

3. Append(self: pydrake.trajectories.PiecewiseQuaternionSlerp, time: float, angle_axis: pydrake.common.eigen_geometry.AngleAxis_[float]) -> None

Given a new AngleAxis, this method adds one segment to the end of this.

orientation(self: pydrake.trajectories.PiecewiseQuaternionSlerp, time: float) → pydrake.common.eigen_geometry.Quaternion_[float]

Interpolates orientation.

Parameter time:

Time for interpolation.

Returns:The interpolated quaternion at time.

class pydrake.trajectories.PiecewiseTrajectory

Bases: pydrake.trajectories.Trajectory

Abstract class that implements the basic logic of maintaining consequent segments of time (delimited by breaks) to implement a trajectory that is represented by simpler logic in each segment or “piece”.

@tparam_default_scalars

__init__

Initialize self. See help(type(self)) for accurate signature.

duration(self: pydrake.trajectories.PiecewiseTrajectory, segment_index: int) → float

end_time(*args, **kwargs)

Overloaded function.

1. end_time(self: pydrake.trajectories.PiecewiseTrajectory, segment_index: int) -> float
2. end_time(self: pydrake.trajectories.PiecewiseTrajectory) -> float

get_number_of_segments(self: pydrake.trajectories.PiecewiseTrajectory) → int

get_segment_index(self: pydrake.trajectories.PiecewiseTrajectory, t: float) → int

get_segment_times(self: pydrake.trajectories.PiecewiseTrajectory) → List[float]

is_time_in_range(self: pydrake.trajectories.PiecewiseTrajectory, t: float) → bool

Returns true iff t >= getStartTime() && t <= getEndTime().

start_time(*args, **kwargs)

Overloaded function.

1. start_time(self: pydrake.trajectories.PiecewiseTrajectory, segment_index: int) -> float
2. start_time(self: pydrake.trajectories.PiecewiseTrajectory) -> float

class pydrake.trajectories.Trajectory

A Trajectory represents a time-varying matrix, indexed by a single scalar time.

@tparam_default_scalars

__init__

Initialize self. See help(type(self)) for accurate signature.

cols(self: pydrake.trajectories.Trajectory) → int

Returns:The number of columns in the matrix returned by value().

end_time(self: pydrake.trajectories.Trajectory) → float

EvalDerivative(self: pydrake.trajectories.Trajectory, t: float, derivative_order: int = 1) → numpy.ndarray[numpy.float64[m, n]]

Evaluates the derivative of this at the given time t. Returns the nth derivative, where n is the value of derivative_order.

Precondition:

derivative_order must be non-negative.

MakeDerivative(self: pydrake.trajectories.Trajectory, derivative_order: int = 1) → pydrake.trajectories.Trajectory

Takes the derivative of this Trajectory.

Parameter derivative_order:

The number of times to take the derivative before returning.

Returns:The nth derivative of this object.

rows(self: pydrake.trajectories.Trajectory) → int

Returns:The number of rows in the matrix returned by value().

start_time(self: pydrake.trajectories.Trajectory) → float

value(self: pydrake.trajectories.Trajectory, t: float) → numpy.ndarray[numpy.float64[m, n]]

Evaluates the trajectory at the given time t.

Parameter t:

The time at which to evaluate the trajectory.

Returns:The matrix of evaluated values.

vector_values(self: pydrake.trajectories.Trajectory, arg0: List[float]) → numpy.ndarray[numpy.float64[m, n]]

If cols()==1, then evaluates the trajectory at each time t, and returns the results as a Matrix with the ith column corresponding to the ith time. Otherwise, if rows()==1, then evaluates the trajectory at each time t, and returns the results as a Matrix with the ith row corresponding to the ith time.

Raises:RuntimeError if both cols and rows are not equal to 1.