GcsTrajectoryOptimization::EdgesBetweenSubgraphs Class Reference

Detailed Description

EdgesBetweenSubgraphs are defined as the connecting edges between two given subgraphs.

These edges are a subset of the many other edges in the larger graph. From an API standpoint, EdgesBetweenSubgraphs enable transitions between Subgraphs, which can enable transitions between modes. Further, it allows different constraints to be added in the transition between subgraphs. Note that the EdgesBetweenSubgraphs can't be separated from the actual edges in the GraphOfConvexSets framework, thus mixing it with other instances of GCSTrajetoryOptimization is not supported.

#include <drake/planning/trajectory_optimization/gcs_trajectory_optimization.h>

Public Member Functions
	~EdgesBetweenSubgraphs ()
void	AddVelocityBounds (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub)
	Adds a linear velocity constraint to the control point connecting the subgraphs lb ≤ q̇(t) ≤ ub.
void	AddNonlinearDerivativeBounds (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, int derivative_order)
	Adds a nonlinear derivative constraints to the control point connecting the subgraphs lb ≤ dᴺq(t) / dtᴺ ≤ ub.
void	AddZeroDerivativeConstraints (int derivative_order)
	Enforces zero derivatives on the control point connecting the subgraphs.
void	AddPathContinuityConstraints (int continuity_order)
	Enforces that for any two subsequent path segments that are joined by an edge in this EdgesBetweenSubgraphs, the continuity_orderth path derivative at the end of the first segment equals that of the start of the second segment.
void	AddContinuityConstraints (int continuity_order)
	Enforces that for any two subsequent path segments that are joined by an edge in this EdgesBetweenSubgraphs, the continuity_orderth time derivative at the end of the first segment equals that of the start of the second segment.
const std::vector< geometry::optimization::GraphOfConvexSets::Edge * > &	Edges ()
	Returns constant reference to a vector of mutable pointers to the edges.
std::vector< const geometry::optimization::GraphOfConvexSets::Edge * >	Edges () const
	Returns pointers to the edges.
const std::pair< symbolic::Variable, symbolic::Variable > &	edge_constituent_vertex_durations () const
	Returns a pair of placeholder decision variables (not actually declared as decision variables in the MathematicalProgram) associated with the time scaling of the trajectory in two sets that are connected by an edge from this EdgesBetweenSubgraphs.
const std::pair< solvers::MatrixXDecisionVariable, solvers::MatrixXDecisionVariable > &	edge_constituent_vertex_control_points () const
	Returns a pair of placeholder decision variables (not actually declared as decision variables in the MathematicalProgram) associated with the control points of the trajectory in two sets that are connected by an edge from this EdgesBetweenSubgraphs.
void	AddEdgeCost (const symbolic::Expression &e, const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &use_in_transcription={ geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction})
	Adds an arbitrary user-defined cost to every edge within the EdgesBetweenSubgraphs.
void	AddEdgeCost (const solvers::Binding< solvers::Cost > &binding, const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &use_in_transcription={ geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction})
	Convenience overload of AddEdgeCost to take in a Binding<Cost>.
void	AddEdgeConstraint (const symbolic::Formula &e, const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &use_in_transcription={ geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction})
	Adds an arbitrary user-defined constraint to every edge within the EdgesBetweenSubgraphs.
void	AddEdgeConstraint (const solvers::Binding< solvers::Constraint > &binding, const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &use_in_transcription={ geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction})
	Convenience overload of AddEdgeConstraint to take in a Binding<Constraint>.
Does not allow copy, move, or assignment
	EdgesBetweenSubgraphs (const EdgesBetweenSubgraphs &)=delete
EdgesBetweenSubgraphs &	operator= (const EdgesBetweenSubgraphs &)=delete
	EdgesBetweenSubgraphs (EdgesBetweenSubgraphs &&)=delete
EdgesBetweenSubgraphs &	operator= (EdgesBetweenSubgraphs &&)=delete

Friends
class	GcsTrajectoryOptimization

Constructor & Destructor Documentation

EdgesBetweenSubgraphs() [1/2]

EdgesBetweenSubgraphs ( const EdgesBetweenSubgraphs & )

delete

EdgesBetweenSubgraphs() [2/2]

EdgesBetweenSubgraphs ( EdgesBetweenSubgraphs && )

delete

~EdgesBetweenSubgraphs()

~EdgesBetweenSubgraphs

(

)

Member Function Documentation

AddContinuityConstraints()

void AddContinuityConstraints

(

int

continuity_order

)

Enforces that for any two subsequent path segments that are joined by an edge in this EdgesBetweenSubgraphs, the continuity_orderth time derivative at the end of the first segment equals that of the start of the second segment.

This adds a nonlinear constraint to the restriction and MIP GraphOfConvexSets::Transcription, while adding a convex surrogate to the relaxation. For more details, see Guiding Non-convex Optimization with the GraphOfConvexSets.

The continuity is enforced on the control points of q(t), which appear as nonlinear constraints.

 (dᴺrᵤ(s=1) / dsᴺ) / hᵤᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥᴺ 

The convex surrogate is simply the path continuity, where hᵤᴺ and hᵥᴺ are replaced by the characteristic times of the respective sets:

 (dᴺrᵤ(s=1) / dsᴺ) / hᵤ₀ᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥ₀ᴺ 

. For now, these are set to one, but future work may involve scaling them by the size of the sets.

Parameters

continuity_order

is the order of the continuity constraint.

Exceptions

std::exception	if the continuity order is not equal or less than the order of both subgraphs.
std::exception	if the continuity order is less than one since path continuity is enforced by default.

Note

To enforce that the trajectory is of class C^k, you must call AddContinuityConstraint for each continuity_order 1 through k.

AddEdgeConstraint() [1/2]

void AddEdgeConstraint	(	const solvers::Binding< solvers::Constraint > &	binding,
		const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &	use_in_transcription = { geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction} )

Convenience overload of AddEdgeConstraint to take in a Binding<Constraint>.

Exceptions

std::exception

if any variables besides those from edge_constituent_vertex_durations and edge_constituent_vertex_control_points are used.

AddEdgeConstraint() [2/2]

void AddEdgeConstraint	(	const symbolic::Formula &	e,
		const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &	use_in_transcription = { geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction} )

Adds an arbitrary user-defined constraint to every edge within the EdgesBetweenSubgraphs.

The constraint should be defined using the placeholder control point variables (obtained from edge_constituent_vertex_control_points()) and the placeholder time scaling variables (obtained from edge_constituent_vertex_durations()). This enables greater modeling freedom, but we cannot guarantee a feasible solution for all possible constraints.

Exceptions

std::exception

if any variables besides those from edge_constituent_vertex_durations and edge_constituent_vertex_control_points are used.

Constraints which do not support the perspective operation cannot be used with Transcription::kMIP or Transcription::kRelaxation. Consider providing an appropriate "convex surrogate" that is supported within GraphOfConvexSets, or exclusively using the SolveConvexRestriction method.

AddEdgeCost() [1/2]

void AddEdgeCost	(	const solvers::Binding< solvers::Cost > &	binding,
		const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &	use_in_transcription = { geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction} )

Convenience overload of AddEdgeCost to take in a Binding<Cost>.

Exceptions

std::exception

if any variables besides those from edge_constituent_vertex_durations and edge_constituent_vertex_control_points are used.

AddEdgeCost() [2/2]

void AddEdgeCost	(	const symbolic::Expression &	e,
		const std::unordered_set< geometry::optimization::GraphOfConvexSets::Transcription > &	use_in_transcription = { geometry::optimization::GraphOfConvexSets::Transcription::kMIP, geometry::optimization::GraphOfConvexSets::Transcription::kRelaxation, geometry::optimization::GraphOfConvexSets::Transcription::kRestriction} )

Adds an arbitrary user-defined cost to every edge within the EdgesBetweenSubgraphs.

The cost should be defined using the placeholder control point variables (obtained from edge_constituent_vertex_control_points()) and the placeholder time scaling variables (obtained from edge_constituent_vertex_durations()). This enables greater modeling freedom, but we cannot guarantee a feasible solution for all possible costs.

Exceptions

std::exception

if any variables besides those from edge_constituent_vertex_durations and edge_constituent_vertex_control_points are used.

Costs which do not support the perspective operation cannot be used with Transcription::kMIP or Transcription::kRelaxation. Consider providing an appropriate "convex surrogate" that is supported within GraphOfConvexSets, or exclusively using the SolveConvexRestriction method.

AddNonlinearDerivativeBounds()

void AddNonlinearDerivativeBounds	(	const Eigen::Ref< const Eigen::VectorXd > &	lb,
		const Eigen::Ref< const Eigen::VectorXd > &	ub,
		int	derivative_order )

Adds a nonlinear derivative constraints to the control point connecting the subgraphs lb ≤ dᴺq(t) / dtᴺ ≤ ub.

This adds a nonlinear constraint to the restriction and MIP GraphOfConvexSets::Transcription, while adding a convex surrogate to the relaxation. For more details, see Guiding Non-convex Optimization with the GraphOfConvexSets.

The nonlinear constraint involves the derivative dᴺq(t) / dtᴺ which is decomposed as dᴺr(s) / dsᴺ / hᴺ. The convex surrogate replaces the nonlinear component hᴺ with h₀ᴺ⁻¹h, where h₀ is the characteristic time of the set. For now, h₀ is set to 1.0 for all sets.

Parameters

lb	is the lower bound of the derivative.
ub	is the upper bound of the derivative.
derivative_order	is the order of the derivative to be constrained.

Exceptions

std::exception	if both subgraphs order is less than the desired derivative order.
std::exception	if the derivative order <= 1, since the linear velocity bounds are preferred.
std::exception	if lb or ub are not of size num_positions().

AddPathContinuityConstraints()

void AddPathContinuityConstraints

(

int

continuity_order

)

Enforces that for any two subsequent path segments that are joined by an edge in this EdgesBetweenSubgraphs, the continuity_orderth path derivative at the end of the first segment equals that of the start of the second segment.

Note that the constraints are on the control points of the derivatives of r(s) and not q(t). This may result in discontinuities of the trajectory return by SolvePath() since the r(s) will get rescaled by the duration h to yield q(t). NormalizeSegmentTimes() will return r(s) with valid continuity.

Parameters

continuity_order

is the order of the continuity constraint.

Exceptions

std::exception	if the continuity order is not equal or less than the order of both subgraphs.
std::exception	if the continuity order is less than one since path continuity is enforced by default.

Note

To enforce that the trajectory is of class C^k, you must call AddPathContinuityConstraint for each continuity_order 1 through k.

AddVelocityBounds()

void AddVelocityBounds	(	const Eigen::Ref< const Eigen::VectorXd > &	lb,
		const Eigen::Ref< const Eigen::VectorXd > &	ub )

Adds a linear velocity constraint to the control point connecting the subgraphs lb ≤ q̇(t) ≤ ub.

Parameters

lb	is the lower bound of the velocity.
ub	is the upper bound of the velocity.

Exceptions

std::exception	if both subgraphs order is zero, since the velocity is defined as the derivative of the Bézier curve. At least one of the subgraphs must have an order of at least 1.
std::exception	if lb or ub are not of size num_positions().

AddZeroDerivativeConstraints()

void AddZeroDerivativeConstraints

(

int

derivative_order

)

Enforces zero derivatives on the control point connecting the subgraphs.

For velocity, acceleration, jerk, etc. enforcing zero-derivative on the trajectory q(t) is equivalent to enforcing zero-derivative on the trajectory r(s). Hence this constraint is convex.

Parameters

derivative_order

is the order of the derivative to be constrained.

Exceptions

std::exception	if the derivative order < 1.
std::exception	if both subgraphs order is less than the desired derivative order.

edge_constituent_vertex_control_points()

const std::pair< solvers::MatrixXDecisionVariable, solvers::MatrixXDecisionVariable > & edge_constituent_vertex_control_points

(

)

const

Returns a pair of placeholder decision variables (not actually declared as decision variables in the MathematicalProgram) associated with the control points of the trajectory in two sets that are connected by an edge from this EdgesBetweenSubgraphs.

Each variable will be of shape (num_positions(), order+1), where the ith column is the ith control point. (Note that the first and second variable will have different shapes if the order of the two subgraphs is different.) These variables will be substituted for real decision variables in methods like AddEdgeCost and AddEdgeConstraint. Passing this variable directly into objectives/constraints will result in an error.

edge_constituent_vertex_durations()

const std::pair< symbolic::Variable, symbolic::Variable > & edge_constituent_vertex_durations

(

)

const

Returns a pair of placeholder decision variables (not actually declared as decision variables in the MathematicalProgram) associated with the time scaling of the trajectory in two sets that are connected by an edge from this EdgesBetweenSubgraphs.

These variables will be substituted for real decision variables in methods like AddEdgeCost and AddEdgeConstraint. Passing this variable directly into objectives/constraints will result in an error.

Edges() [1/2]

const std::vector< geometry::optimization::GraphOfConvexSets::Edge * > & Edges

(

)

Returns constant reference to a vector of mutable pointers to the edges.

Edges() [2/2]

std::vector< const geometry::optimization::GraphOfConvexSets::Edge * > Edges

(

)

const

Returns pointers to the edges.

operator=() [1/2]

EdgesBetweenSubgraphs & operator= ( const EdgesBetweenSubgraphs & )

delete

operator=() [2/2]

EdgesBetweenSubgraphs & operator= ( EdgesBetweenSubgraphs && )

delete

Friends And Related Symbol Documentation

GcsTrajectoryOptimization

friend class GcsTrajectoryOptimization

friend

---

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

• drake/planning/trajectory_optimization/gcs_trajectory_optimization.h