|
Drake
|
|
Drake
|
This virtual class is the base of CspaceFreePolytope and CspaceFreeBox.
We take the common functionality between these concrete derived class to this shared parent class.
#include <drake/geometry/optimization/cspace_free_polytope_base.h>
Classes | |
| struct | Options |
| Optional argument for constructing CspaceFreePolytopeBase. More... | |
Public Types | |
| using | IgnoredCollisionPairs = std::unordered_set< SortedPair< geometry::GeometryId > > |
Public Member Functions | |
| virtual | ~CspaceFreePolytopeBase () |
| const multibody::RationalForwardKinematics & | rational_forward_kin () const |
| Getter for the rational forward kinematics object that computes the forward kinematics as rational functions. More... | |
| const std::unordered_map< SortedPair< geometry::GeometryId >, int > & | map_geometries_to_separating_planes () const |
| separating_planes()[map_geometries_to_separating_planes.at(geometry1_id, geometry2_id)] is the separating plane that separates geometry 1 and geometry 2. More... | |
| const std::vector< CSpaceSeparatingPlane< symbolic::Variable > > & | separating_planes () const |
| All the separating planes between each pair of geometries. More... | |
| const Vector3< symbolic::Variable > & | y_slack () const |
| Get the slack variable used for non-polytopic collision geometries. More... | |
Does not allow copy, move, or assignment | |
| CspaceFreePolytopeBase (const CspaceFreePolytopeBase &)=delete | |
| CspaceFreePolytopeBase & | operator= (const CspaceFreePolytopeBase &)=delete |
| CspaceFreePolytopeBase (CspaceFreePolytopeBase &&)=delete | |
| CspaceFreePolytopeBase & | operator= (CspaceFreePolytopeBase &&)=delete |
Protected Types | |
| enum | SForPlane { kAll, kOnChain } |
| When we set up the separating plane {x | a(s)ᵀx + b(s) = 0} between a pair of geometries, we need to determine which s are used in a(s) and b(s). More... | |
Protected Member Functions | |
| CspaceFreePolytopeBase (const multibody::MultibodyPlant< double > *plant, const geometry::SceneGraph< double > *scene_graph, SeparatingPlaneOrder plane_order, SForPlane s_for_plane_enum, const Options &options=Options{}) | |
| Constructor. More... | |
| template<typename T > | |
| void | CalcSBoundsPolynomial (const VectorX< T > &s_lower, const VectorX< T > &s_upper, VectorX< symbolic::Polynomial > *s_minus_s_lower, VectorX< symbolic::Polynomial > *s_upper_minus_s) const |
| Computes s-s_lower and s_upper - s as polynomials of s. More... | |
| int | GetSeparatingPlaneIndex (const SortedPair< geometry::GeometryId > &pair) const |
| Returns the index of the plane which will separate the geometry pair. More... | |
| const symbolic::Variables & | get_s_set () const |
| const geometry::SceneGraph< double > & | scene_graph () const |
| const std::map< multibody::BodyIndex, std::vector< std::unique_ptr< CIrisCollisionGeometry > > > & | link_geometries () const |
| SeparatingPlaneOrder | plane_order () const |
| const std::unordered_map< SortedPair< multibody::BodyIndex >, std::array< VectorX< symbolic::Monomial >, 4 > > & | map_body_to_monomial_basis_array () const |
Maps a pair of body (body1, body2) to an array of monomial basis monomial_basis_array. More... | |
| bool | with_cross_y () const |
| Check Options::with_cross_y for more details. More... | |
| const std::unordered_map< SortedPair< multibody::BodyIndex >, std::vector< int > > & | map_body_pair_to_s_on_chain () const |
| For a pair of bodies body_pair, returns the indices of all s on the kinematics chain from body_pair.first() to body_pair.second(). More... | |
| VectorX< symbolic::Variable > | GetSForPlane (const SortedPair< multibody::BodyIndex > &body_pair, SForPlane s_for_plane_enum) const |
| Returns a vector of s variable used in a(s), b(s), which parameterize the separating plane {x | a(s)ᵀx+b(s) = 0}. More... | |
| void | SolveCertificationForEachPlaneInParallel (const std::vector< int > &active_plane_indices, const std::function< std::pair< bool, int >(int)> &solve_plane_sos, Parallelism parallelism, bool verbose, bool terminate_at_failure) const |
| For each pair of geometries, solve the certification problem to find their separation plane in parallel. More... | |
| int | GetGramVarSizeForPolytopeSearchProgram (const std::vector< PlaneSeparatesGeometries > &plane_geometries_vec, const IgnoredCollisionPairs &ignored_collision_pairs, const std::function< int(const symbolic::RationalFunction &rational, const std::array< VectorX< symbolic::Monomial >, 4 > &monomial_basis_array)> &count_gram_per_rational) const |
| Get the total size of all the decision variables for the Gram matrices, so as to search for the polytope with given Lagrangian multipliers. More... | |
Friends | |
| class | CspaceFreePolytopeBaseTester |
| using IgnoredCollisionPairs = std::unordered_set<SortedPair<geometry::GeometryId> > |
|
strongprotected |
|
delete |
|
delete |
|
virtual |
|
protected |
Constructor.
We put the constructor in protected method to make sure that the user cannot instantiate a CspaceFreePolytopeBase instance.
|
protected |
Computes s-s_lower and s_upper - s as polynomials of s.
|
protected |
|
protected |
Get the total size of all the decision variables for the Gram matrices, so as to search for the polytope with given Lagrangian multipliers.
| plane_geometries_vec | This struct contains the information on the rationals that we need to certify, so as to prove the existence of separating planes. |
| ignored_collision_pairs | The collision pairs that we ignore. |
| count_gram_per_rational | We will impose the sos condition that certain rational is always non-negative within a semialgebraic set. This function returns the number of variables in the Gram matrices for this rational. |
|
protected |
Returns the index of the plane which will separate the geometry pair.
Returns -1 if the pair is not in map_geometries_to_separating_planes_.
|
protected |
Returns a vector of s variable used in a(s), b(s), which parameterize the separating plane {x | a(s)ᵀx+b(s) = 0}.
|
protected |
|
protected |
For a pair of bodies body_pair, returns the indices of all s on the kinematics chain from body_pair.first() to body_pair.second().
For each pair of collidable collision geometry (A, B), we denote their body as (bodyA, bodyB). This keys in this map include all these (bodyA, bodyB), together with (body_middle, bodyA) and (body_middle, bodyB), where body_middle is the body in the middle of the kinematics chain between bodyA and bodyB.
|
protected |
Maps a pair of body (body1, body2) to an array of monomial basis monomial_basis_array.
monomial_basis_array[0] contains all the monomials of form ∏ᵢ pow(sᵢ, dᵢ), dᵢ=0 or 1, sᵢ correspond to the revolute/prismatic joint on the kinematic chain between body1 and body2. monomial_basis_array[i+1] = y_slack_[i] * monomial_basis_array[0]
| const std::unordered_map<SortedPair<geometry::GeometryId>, int>& map_geometries_to_separating_planes | ( | ) | const |
separating_planes()[map_geometries_to_separating_planes.at(geometry1_id, geometry2_id)] is the separating plane that separates geometry 1 and geometry 2.
|
delete |
|
delete |
|
protected |
| const multibody::RationalForwardKinematics& rational_forward_kin | ( | ) | const |
Getter for the rational forward kinematics object that computes the forward kinematics as rational functions.
|
protected |
| const std::vector<CSpaceSeparatingPlane<symbolic::Variable> >& separating_planes | ( | ) | const |
All the separating planes between each pair of geometries.
|
protected |
For each pair of geometries, solve the certification problem to find their separation plane in parallel.
| active_plane_indices | We will search for the plane in this->separating_planes()[active_plane_indices[i]]. |
| solve_plane_sos | The solve_plane_sos(plane_count) returns the pair (is_success, plane_count), where is_success indicates whether the solve for this->separating_planes()[active_plane_indices[plane_count]] is successful or not. This function returns the input plane_count as one of the output. This is because when we access the return value of solve_small_sos, we need to know the plane_count, and the return value and the input plane_count live in different part of the code due to multi-threading. |
| parallelism | The number of threads in the parallel solve. |
| verbose | Whether to print out some messages during the parallel solve. |
| terminate_at_failure | If set to true, then terminate this function when we failed to find a separating plane. |
|
protected |
Check Options::with_cross_y for more details.
| const Vector3<symbolic::Variable>& y_slack | ( | ) | const |
Get the slack variable used for non-polytopic collision geometries.
Check Options class for more details.
|
friend |