Detailed Description

template<typename T>
class drake::trajectories::PiecewiseConstantCurvatureTrajectory< T >

A piecewise constant curvature trajectory in a plane, where the plane is posed arbitrarily in three dimensions.

The trajectory is defined by the position vector r(s): ℝ → ℝ³, parameterized by arclength s, and lies in a plane with normal vector p̂. The parameterization is C¹, i.e. position r(s) and tangent vector t̂(s) = dr(s)/ds are continuous functions of arclength s. The trajectory's length is divided into segments s₀ < s₁ < ... < sₙ, where each interval [sᵢ, sᵢ₊₁) has constant curvature determined by the turning rate ρᵢ (with units of 1/m). The turning rate is defined such that curvature is κᵢ = |ρᵢ|, with its sign indicating the curve's direction around p̂ according ot the right-hand rule (counterclockwise if positive and clockwise if negative) . For ρᵢ = 0, the segment is a straight line.

Given the tangent vector t̂(s) = dr(s)/ds and the plane's normal p̂, we define the normal vector along the curve as n̂(s) = p̂ × t̂(s). These three vectors are used to define a moving frame M along the curve with basis vectors Mx, My, Mz coincident with vectors t̂, n̂, p̂, respectively.

A trajectory is said to be periodic if the pose X_AM of frame M is a periodic function of arclength, i.e. X_AM(s) = X_AM(s + k⋅L) ∀ k ∈ ℤ, where L is the length of a single cycle along the trajectory. Periodicity is determined at construction.

Note: Though similar, frame M is distinct from the Frenet–Serret frame defined by the tangent-normal-binormal vectors T̂, N̂, B̂ See Frenet–Serret formulas for further reading.

For constant curvature paths on a plane, the Frenet–Serret formulas simplify and we can write:

    dMx/ds(s) =  ρ(s)⋅My(s)
    dMy/ds(s) = -ρ(s)⋅Mx(s)
    dMz/ds(s) =  0

for the entire trajectory.

The spatial orientation of the curve is defined by the plane's normal p̂ and the initial tangent vector t̂₀ at s = 0. At construction, these are provided in a reference frame A, defining the pose X_AM₀ at s = 0. This class provides functions to compute the curve's kinematics given by its pose X_AM (comprising position vector r(s) = p_AoMo_A(s) and orientation R_AM(s)), spatial velocity V_AM_A(s) and spatial acceleration A_AM_A(s). We also provide functions to return these quantities in the M frame (where they are much simpler), i.e. V_AM_M(s) and A_AM_M(s).

Warning: Note that this class models a curve parameterized by arclength, rather than time as the Trajectory class and many of its inheriting types assume. Time derivatives, i.e. spatial velocity and acceleration, are computed for externally provided values of velocity ṡ and acceleration s̈ along the curve.

See also: multibody::CurvilinearJoint

Template Parameters

T	The scalar type, which must be one of the default scalars.

#include <drake/common/trajectories/piecewise_constant_curvature_trajectory.h>

Public Types
template<typename U >
using	ScalarValueConverter = typename systems::scalar_conversion::template ValueConverter< T, U >

Public Member Functions
	PiecewiseConstantCurvatureTrajectory ()=default
	An empty piecewise constant curvature trajectory. More...

	PiecewiseConstantCurvatureTrajectory (const std::vector< T > &breaks, const std::vector< T > &turning_rates, const Vector3< T > &initial_curve_tangent, const Vector3< T > &plane_normal, const Vector3< T > &initial_position, double periodicity_tolerance=1e-8)
	Constructs a piecewise constant curvature trajectory. More...

template<typename U >
	PiecewiseConstantCurvatureTrajectory (const PiecewiseConstantCurvatureTrajectory< U > other)
	Scalar conversion constructor. More...

T	length () const

boolean< T >	is_periodic () const
	Returns `true` if `this` trajectory is periodic. More...

math::RigidTransform< T >	CalcPose (const T &s) const
	Calculates the trajectory's pose X_AM(s) at the given arclength s. More...

MatrixX< T >	value (const T &s) const final
	Computes r(s), the trajectory's position p_AoMo_A(s) expressed in reference frame A, at the given arclength s. More...

const T &	curvature (const T &s) const
	Returns the signed curvature ρ(s). More...

multibody::SpatialVelocity< T >	CalcSpatialVelocity (const T &s, const T &s_dot) const
	Computes the spatial velocity V_AM_A(s,ṡ) of frame M measured and expressed in the reference frame A. More...

multibody::SpatialVelocity< T >	CalcSpatialVelocityInM (const T &s, const T &s_dot) const
	Computes the spatial velocity V_AM_M(s,ṡ) of frame M measured in frame A but expressed in frame M. More...

multibody::SpatialAcceleration< T >	CalcSpatialAcceleration (const T &s, const T &s_dot, const T &s_ddot) const
	Computes the spatial acceleration A_AM_A(s,ṡ,s̈) of frame M measured and expressed in the reference frame A. More...

multibody::SpatialAcceleration< T >	CalcSpatialAccelerationInM (const T &s, const T &s_dot, const T &s_ddot) const
	Computes the spatial acceleration A_AM_M(s,ṡ,s̈) of frame M measured in frame A but expressed in frame M. More...

boolean< T >	IsNearlyPeriodic (double tolerance) const

Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
	PiecewiseConstantCurvatureTrajectory (const PiecewiseConstantCurvatureTrajectory &)=default

PiecewiseConstantCurvatureTrajectory &	operator= (const PiecewiseConstantCurvatureTrajectory &)=default

	PiecewiseConstantCurvatureTrajectory (PiecewiseConstantCurvatureTrajectory &&)=default

PiecewiseConstantCurvatureTrajectory &	operator= (PiecewiseConstantCurvatureTrajectory &&)=default

Public Member Functions inherited from PiecewiseTrajectory< T >
	~PiecewiseTrajectory () override

int	get_number_of_segments () const

T	start_time (int segment_number) const

T	end_time (int segment_number) const

T	duration (int segment_number) const

boolean< T >	is_time_in_range (const T &t) const
	Returns true iff `t >= getStartTime() && t <= getEndTime()`. More...

int	get_segment_index (const T &t) const

const std::vector< T > &	get_segment_times () const

void	segment_number_range_check (int segment_number) const

Public Member Functions inherited from Trajectory< T >
virtual	~Trajectory ()

virtual std::unique_ptr< Trajectory< T > >	Clone () const

MatrixX< T >	vector_values (const std::vector< T > &t) const
	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. More...

MatrixX< T >	vector_values (const Eigen::Ref< const VectorX< T >> &t) const
	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. More...

bool	has_derivative () const
	Returns true iff the Trajectory provides and implementation for EvalDerivative() and MakeDerivative(). More...

MatrixX< T >	EvalDerivative (const T &t, int derivative_order=1) const
	Evaluates the derivative of `this` at the given time `t`. More...

std::unique_ptr< Trajectory< T > >	MakeDerivative (int derivative_order=1) const
	Takes the derivative of this Trajectory. More...

virtual Eigen::Index	rows () const

virtual Eigen::Index	cols () const

virtual T	start_time () const

virtual T	end_time () const

Friends
template<typename U >
class	PiecewiseConstantCurvatureTrajectory

Additional Inherited Members
Static Public Member Functions inherited from PiecewiseTrajectory< T >
static std::vector< T >	RandomSegmentTimes (int num_segments, std::default_random_engine &generator)

Static Public Attributes inherited from PiecewiseTrajectory< T >
static constexpr double	kEpsilonTime = std::numeric_limits<double>::epsilon()
	Minimum delta quantity used for comparing time. More...

Protected Member Functions inherited from PiecewiseTrajectory< T >
	PiecewiseTrajectory ()=default

	PiecewiseTrajectory (const std::vector< T > &breaks)
	`breaks` increments must be greater or equal to kEpsilonTime. More...

T	do_start_time () const override

T	do_end_time () const override

bool	SegmentTimesEqual (const PiecewiseTrajectory &b, double tol=kEpsilonTime) const

const std::vector< T > &	breaks () const

std::vector< T > &	get_mutable_breaks ()

	PiecewiseTrajectory (const PiecewiseTrajectory &)=default

PiecewiseTrajectory &	operator= (const PiecewiseTrajectory &)=default

	PiecewiseTrajectory (PiecewiseTrajectory &&)=default

PiecewiseTrajectory &	operator= (PiecewiseTrajectory &&)=default

Protected Member Functions inherited from Trajectory< T >
	Trajectory ()=default

virtual bool	do_has_derivative () const

virtual MatrixX< T >	DoEvalDerivative (const T &t, int derivative_order) const

virtual std::unique_ptr< Trajectory< T > >	DoMakeDerivative (int derivative_order) const

	Trajectory (const Trajectory &)=default

Trajectory &	operator= (const Trajectory &)=default

	Trajectory (Trajectory &&)=default

Trajectory &	operator= (Trajectory &&)=default

Member Typedef Documentation

◆ ScalarValueConverter

using ScalarValueConverter = typename systems::scalar_conversion::template ValueConverter<T, U>

Constructor & Destructor Documentation

◆ PiecewiseConstantCurvatureTrajectory() [1/5]

PiecewiseConstantCurvatureTrajectory ( const PiecewiseConstantCurvatureTrajectory< T > & )

default

◆ PiecewiseConstantCurvatureTrajectory() [2/5]

PiecewiseConstantCurvatureTrajectory ( PiecewiseConstantCurvatureTrajectory< T > && )

default

◆ PiecewiseConstantCurvatureTrajectory() [3/5]

PiecewiseConstantCurvatureTrajectory ( )

default

An empty piecewise constant curvature trajectory.

◆ PiecewiseConstantCurvatureTrajectory() [4/5]

PiecewiseConstantCurvatureTrajectory	(	const std::vector< T > &	breaks,
		const std::vector< T > &	turning_rates,
		const Vector3< T > &	initial_curve_tangent,
		const Vector3< T > &	plane_normal,
		const Vector3< T > &	initial_position,
		double	periodicity_tolerance = `1e-8`
	)

Constructs a piecewise constant curvature trajectory.

Endpoints of each constant-curvature segments are defined by n breaks s₀ = 0 < s₁ < ... < sₙ (in meters). The turning rates ρ₀, ..., ρₙ₋₁ (in 1/m) are passed through turning_rates. There must be exactly one turning rate per segment, i.e. turning_rates.size() == breaks.size() - 1.

Warning: Users of this class are responsible for providing initial_curve_tangent, plane_normal and initial_position expressed in the same reference frame A.

The initial_curve_tangent t̂₀ and the plane_normal p̂, along with the initial curve's normal n̂₀ = p̂ × t̂₀, define the initial orientation R_AM₀ of frame M at s = 0.

Parameters

breaks	A vector of n break values sᵢ between segments. The parent class, PiecewiseTrajectory, enforces that the breaks increase by at least PiecewiseTrajectory::kEpsilonTime.
turning_rates	A vector of n-1 turning rates ρᵢ for each segment.
initial_curve_tangent	The initial tangent of the curve expressed in the parent frame, t̂_A(s₀) = Mx_A(s₀).
plane_normal	The normal axis of the 2D plane in which the curve lies, expressed in the parent frame, p̂_A = Mz_A (constant).
initial_position	The initial position of the curve expressed in the parent frame, p_AoMo_A(s₀).
periodicity_tolerance	Tolerance used to determine if the resulting trajectory is periodic, according to the metric defined by IsNearlyPeriodic(). If IsNearlyPeriodic(periodicity_tolerance) is true, then the newly constructed trajectory will be periodic. That is, X_AM(s) = X_AM(s + k⋅L) ∀ k ∈ ℤ, where L equals length(). Subsequent calls to is_periodic() will return `true`.

Exceptions

std::exception	if the number of turning rates does not match the number of segments
std::exception	if or s₀ is not 0.
std::exception	if initial_curve_tangent or plane_normal have zero norm.
std::exception	if initial_curve_tangent is not perpendicular to plane_normal.

◆ PiecewiseConstantCurvatureTrajectory() [5/5]

PiecewiseConstantCurvatureTrajectory ( const PiecewiseConstantCurvatureTrajectory< U > other )

explicit

Scalar conversion constructor.

See System Scalar Conversion.

Member Function Documentation

◆ CalcPose()

math::RigidTransform<T> CalcPose ( const T & s ) const

Calculates the trajectory's pose X_AM(s) at the given arclength s.

Note: For s < 0 and s > length() the pose is extrapolated as if the curve continued with the curvature of the corresponding end segment.

Parameters

s	The query arclength in meters.

Returns: the pose X_AM(s).

◆ CalcSpatialAcceleration()

multibody::SpatialAcceleration<T> CalcSpatialAcceleration	(	const T &	s,
		const T &	s_dot,
		const T &	s_ddot
	)		const

Computes the spatial acceleration A_AM_A(s,ṡ,s̈) of frame M measured and expressed in the reference frame A.

See the class's documentation for frame definitions.

In frame invariant notation, the angular acceleration α(s) and translational acceleration a(s) are:

  α(s) = s̈⋅ρ(s)⋅p̂
  a(s) = ṡ²⋅ρ(s)⋅n̂(s) + s̈⋅t̂(s)

where ρ(s), t̂(s) and n̂(s) are extrapolated for s < 0 and s > length() keeping the constant curvature of the corresponding end segment.

As the curve does not have continuous acceleration at the breaks, by convention we set the acceleration at the break sᵢ to be the limit as approached from the right – i.e. the acceleration is continuous on each segment domain [sᵢ, sᵢ₊₁).

Parameters

s	The query arclength, in meters.
s_dot	The magnitude ṡ of the tangential velocity along the curve, in meters per second.
s_ddot	The magnitude s̈ of the tangential acceleration along the curve, in meters per second squared.

Return values

A_AM_A The spatial acceleration of M in A, expressed in A.

◆ CalcSpatialAccelerationInM()

multibody::SpatialAcceleration<T> CalcSpatialAccelerationInM	(	const T &	s,
		const T &	s_dot,
		const T &	s_ddot
	)		const

Computes the spatial acceleration A_AM_M(s,ṡ,s̈) of frame M measured in frame A but expressed in frame M.

See CalcSpatialAcceleration() for details. Note that when expressed in frame M, the vectors p̂, t̂, and n̂ are constant, with p̂_M = [0 0 1]ᵀ, t̂_M = [1 0 0]ᵀ, and n̂_M = [0 1 0]ᵀ so the returned spatial acceleration is just A_AM_M = [0 0 s̈⋅ρ(s) s̈ ṡ²⋅ρ(s) 0]ᵀ.

Parameters

s	The query arclength, in meters.
s_dot	The magnitude ṡ of the tangential velocity along the curve, in meters per second.
s_ddot	The magnitude s̈ of the tangential acceleration along the curve, in meters per second squared.

Return values

A_AM_M The spatial acceleration of M in A, expressed in M.

◆ CalcSpatialVelocity()

multibody::SpatialVelocity<T> CalcSpatialVelocity	(	const T &	s,
		const T &	s_dot
	)		const

Computes the spatial velocity V_AM_A(s,ṡ) of frame M measured and expressed in the reference frame A.

See the class's documentation for frame definitions.

In frame invariant notation, the angular velocity ω(s) and translational velocity v(s) are:

  ω(s) = ṡ⋅ρ(s)⋅p̂
  v(s) = ṡ⋅t̂(s)

where ρ(s) and t̂(s) are extrapolated for s < 0 and s > length() keeping the constant curvature of the corresponding end segment.

Parameters

s	The query arclength, in meters.
s_dot	The magnitude ṡ of the tangential velocity along the curve, in meters per second.

Return values

V_AM_A The spatial velocity of M in A, expressed in A.

◆ CalcSpatialVelocityInM()

multibody::SpatialVelocity<T> CalcSpatialVelocityInM	(	const T &	s,
		const T &	s_dot
	)		const

Computes the spatial velocity V_AM_M(s,ṡ) of frame M measured in frame A but expressed in frame M.

See CalcSpatialVelocity() for details. Note that when expressed in frame M, both p̂ and t̂ are constant, with p̂_M = [0 0 1]ᵀ and t̂_M = [1 0 0]ᵀ so the returned spatial velocity is just V_AM_M = [0 0 ṡ⋅ρ(s) ṡ 0 0]ᵀ.

Parameters

s	The query arclength, in meters.
s_dot	The magnitude ṡ of the tangential velocity along the curve, in meters per second.

Return values

V_AM_M The spatial velocity of M in A, expressed in M.

◆ curvature()

const T& curvature ( const T & s ) const

Returns the signed curvature ρ(s).

Parameters

s	The query arclength in meters.

Returns: curvature ρ(s)

◆ is_periodic()

boolean<T> is_periodic ( ) const

Returns true if this trajectory is periodic.

That is, X_AM(s) = X_AM(s + k⋅L) ∀ k ∈ ℤ, where L equals length().

◆ IsNearlyPeriodic()

boolean<T> IsNearlyPeriodic ( double tolerance ) const

Returns: true if the trajectory is periodic within a given tolerance.

Periodicity is defined as the beginning and end poses X_AM(s₀) and X_AM(sₙ) being equal up to the same tolerance, checked via RigidTransform::IsNearlyEqualTo() using tolerance.

Parameters

tolerance The tolerance for periodicity check.

◆ length()

T length ( ) const

Returns: the total arclength of the curve in meters.

◆ operator=() [1/2]

PiecewiseConstantCurvatureTrajectory& operator= ( PiecewiseConstantCurvatureTrajectory< T > && )

default

◆ operator=() [2/2]

PiecewiseConstantCurvatureTrajectory& operator= ( const PiecewiseConstantCurvatureTrajectory< T > & )

default

◆ value()

MatrixX<T> value ( const T & s ) const

finalvirtual

Computes r(s), the trajectory's position p_AoMo_A(s) expressed in reference frame A, at the given arclength s.

Parameters

s	The query arclength in meters.

Returns: position vector r(s).

Reimplemented from Trajectory< T >.

Friends And Related Function Documentation

◆ PiecewiseConstantCurvatureTrajectory

friend class PiecewiseConstantCurvatureTrajectory

friend

The documentation for this class was generated from the following file:

drake/common/trajectories/piecewise_constant_curvature_trajectory.h

Detailed Description

template<typename T> class drake::trajectories::PiecewiseConstantCurvatureTrajectory< T >

Public Types

Public Member Functions

Friends

Additional Inherited Members

Member Typedef Documentation

◆ ScalarValueConverter

Constructor & Destructor Documentation

◆ PiecewiseConstantCurvatureTrajectory() [1/5]

◆ PiecewiseConstantCurvatureTrajectory() [2/5]

◆ PiecewiseConstantCurvatureTrajectory() [3/5]

◆ PiecewiseConstantCurvatureTrajectory() [4/5]

◆ PiecewiseConstantCurvatureTrajectory() [5/5]

Member Function Documentation

◆ CalcPose()

◆ CalcSpatialAcceleration()

◆ CalcSpatialAccelerationInM()

◆ CalcSpatialVelocity()

◆ CalcSpatialVelocityInM()

◆ curvature()

◆ is_periodic()

◆ IsNearlyPeriodic()

◆ length()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ value()

Friends And Related Function Documentation

◆ PiecewiseConstantCurvatureTrajectory

template<typename T>
class drake::trajectories::PiecewiseConstantCurvatureTrajectory< T >