-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Create CspaceFreeBox class constructor. (#19552)
Factor out CspaceFreePolytopeBase for the common code in both CspaceFreePolytope and CspaceFreeBox.
- Loading branch information
1 parent
51c7d22
commit 8d2fee2
Showing
13 changed files
with
1,069 additions
and
424 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,96 @@ | ||
| #include "drake/geometry/optimization/cspace_free_box.h" | ||
|
|
||
| #include <map> | ||
| #include <memory> | ||
|
|
||
| #include "drake/common/fmt_eigen.h" | ||
| #include "drake/geometry/optimization/cspace_free_internal.h" | ||
|
|
||
| namespace drake { | ||
| namespace geometry { | ||
| namespace optimization { | ||
| CspaceFreeBox::CspaceFreeBox(const multibody::MultibodyPlant<double>* plant, | ||
| const geometry::SceneGraph<double>* scene_graph, | ||
| SeparatingPlaneOrder plane_order, | ||
| const Options& options) | ||
| : CspaceFreePolytopeBase(plant, scene_graph, plane_order, options) {} | ||
|
|
||
| CspaceFreeBox::~CspaceFreeBox() {} | ||
|
|
||
| CspaceFreeBox::SeparatingPlaneLagrangians | ||
| CspaceFreeBox::SeparatingPlaneLagrangians::GetSolution( | ||
| const solvers::MathematicalProgramResult& result) const { | ||
| CspaceFreeBox::SeparatingPlaneLagrangians ret(this->s_box_lower_.rows()); | ||
| for (int i = 0; i < this->s_box_lower_.rows(); ++i) { | ||
| ret.s_box_lower_(i) = result.GetSolution(this->s_box_lower_(i)); | ||
| ret.s_box_upper_(i) = result.GetSolution(this->s_box_upper_(i)); | ||
| } | ||
| return ret; | ||
| } | ||
|
|
||
| void CspaceFreeBox::ComputeSBox( | ||
| const Eigen::Ref<const Eigen::VectorXd>& q_box_lower, | ||
| const Eigen::Ref<const Eigen::VectorXd>& q_box_upper, | ||
| Eigen::VectorXd* s_box_lower, Eigen::VectorXd* s_box_upper, | ||
| Eigen::VectorXd* q_star) const { | ||
| if ((q_box_lower.array() > q_box_upper.array()).any()) { | ||
| throw std::runtime_error( | ||
| fmt::format("CspaceFreeBox: q_box_lower={} has some entries larger " | ||
| "than q_box_upper={}.", | ||
| fmt_eigen(q_box_lower.transpose()), | ||
| fmt_eigen(q_box_upper.transpose()))); | ||
| } | ||
| const auto& plant = this->rational_forward_kin().plant(); | ||
| const Eigen::VectorXd q_position_lower = plant.GetPositionLowerLimits(); | ||
| const Eigen::VectorXd q_position_upper = plant.GetPositionUpperLimits(); | ||
| if ((q_box_lower.array() > q_position_upper.array()).any()) { | ||
| throw std::runtime_error(fmt::format( | ||
| "CspaceFreeBox: q_box_lower={} has some entries larger the the robot " | ||
| "position upper limit={}.", | ||
| fmt_eigen(q_box_lower.transpose()), | ||
| fmt_eigen(q_position_upper.transpose()))); | ||
| } | ||
| if ((q_box_upper.array() < q_position_lower.array()).any()) { | ||
| throw std::runtime_error(fmt::format( | ||
| "CspaceFreeBox: q_box_upper={} has some entries smaller than the robot " | ||
| "position lower limit={}.", | ||
| fmt_eigen(q_box_upper.transpose()), | ||
| fmt_eigen(q_position_lower.transpose()))); | ||
| } | ||
| const Eigen::VectorXd q_lower = | ||
| q_box_lower.array().max(q_position_lower.array()).matrix(); | ||
| const Eigen::VectorXd q_upper = | ||
| q_box_upper.array().min(q_position_upper.array()).matrix(); | ||
| *q_star = 0.5 * (q_lower + q_upper); | ||
| *s_box_lower = this->rational_forward_kin().ComputeSValue(q_lower, *q_star); | ||
| *s_box_upper = this->rational_forward_kin().ComputeSValue(q_upper, *q_star); | ||
| } | ||
|
|
||
| void CspaceFreeBox::GeneratePolynomialsToCertify( | ||
| const Eigen::Ref<const Eigen::VectorXd>& s_box_lower, | ||
| const Eigen::Ref<const Eigen::VectorXd>& s_box_upper, | ||
| const Eigen::Ref<const Eigen::VectorXd>& q_star, | ||
| const IgnoredCollisionPairs& ignored_collision_pairs, | ||
| PolynomialsToCertify* certify_polynomials) const { | ||
| this->CalcSBoundsPolynomial(s_box_lower, s_box_upper, | ||
| &(certify_polynomials->s_minus_s_box_lower), | ||
| &(certify_polynomials->s_box_upper_minus_s)); | ||
|
|
||
| std::map<int, const CSpaceSeparatingPlane<symbolic::Variable>*> | ||
| separating_planes_map; | ||
| for (int i = 0; i < static_cast<int>(separating_planes().size()); ++i) { | ||
| const auto& plane = separating_planes()[i]; | ||
| if (ignored_collision_pairs.count(SortedPair<geometry::GeometryId>( | ||
| plane.positive_side_geometry->id(), | ||
| plane.negative_side_geometry->id())) == 0) { | ||
| separating_planes_map.emplace(i, &plane); | ||
| } | ||
| } | ||
|
|
||
| internal::GenerateRationals(separating_planes_map, y_slack(), q_star, | ||
| rational_forward_kin(), | ||
| &(certify_polynomials->plane_geometries)); | ||
| } | ||
| } // namespace optimization | ||
| } // namespace geometry | ||
| } // namespace drake |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,138 @@ | ||
| #pragma once | ||
|
|
||
| #include <vector> | ||
|
|
||
| #include "drake/geometry/optimization/cspace_free_polytope_base.h" | ||
| #include "drake/geometry/optimization/cspace_free_structs.h" | ||
| #include "drake/solvers/mathematical_program_result.h" | ||
|
|
||
| namespace drake { | ||
| namespace geometry { | ||
| namespace optimization { | ||
| /** | ||
| This class tries to find large axis-aligned bounding boxes in the configuration | ||
| space, such that all configurations in the boxes are collision free. | ||
| Note that we don't guarantee to find the largest box. | ||
| */ | ||
| // CspaceFreeBox "is a" CspaceFreePolytopeBase because it can do anything inside | ||
| // CspaceFreePolytopeBase. We factor out the common code in CspaceFreeBox and | ||
| // CspaceFreePolytope to CspaceFreePolytopeBase, and also rely on the access | ||
| // control (public/protected/private) in CspaceFreePolytopeBase. | ||
| class CspaceFreeBox : public CspaceFreePolytopeBase { | ||
| public: | ||
| DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(CspaceFreeBox) | ||
|
|
||
| using CspaceFreePolytopeBase::IgnoredCollisionPairs; | ||
|
|
||
| ~CspaceFreeBox() override; | ||
|
|
||
| using CspaceFreePolytopeBase::Options; | ||
|
|
||
| /** | ||
| When searching for the separating plane, we want to certify that the | ||
| numerator of a rational is non-negative in the C-space box q_box_lower <= q | ||
| <= q_box_upper (or equivalently s_box_lower <= s <= s_box_upper). Hence for | ||
| each of the rational we will introduce Lagrangian multipliers for the | ||
| polytopic constraint s - s_box_lower >= 0, s_box_upper - s >= 0. | ||
| */ | ||
| class SeparatingPlaneLagrangians { | ||
| public: | ||
| explicit SeparatingPlaneLagrangians(int s_size) | ||
| : s_box_lower_(s_size), s_box_upper_(s_size) {} | ||
|
|
||
| /** Substitutes the decision variables in each Lagrangians with its value in | ||
| * result, returns the substitution result. | ||
| */ | ||
| [[nodiscard]] SeparatingPlaneLagrangians GetSolution( | ||
| const solvers::MathematicalProgramResult& result) const; | ||
|
|
||
| /// The Lagrangians for s - s_box_lower >= 0. | ||
| const VectorX<symbolic::Polynomial>& s_box_lower() const { | ||
| return s_box_lower_; | ||
| } | ||
|
|
||
| /// The Lagrangians for s - s_box_lower >= 0. | ||
| VectorX<symbolic::Polynomial>& mutable_s_box_lower() { | ||
| return s_box_lower_; | ||
| } | ||
|
|
||
| /// The Lagrangians for s_box_upper - s >= 0. | ||
| const VectorX<symbolic::Polynomial>& s_box_upper() const { | ||
| return s_box_upper_; | ||
| } | ||
|
|
||
| /// The Lagrangians for s_box_upper - s >= 0. | ||
| VectorX<symbolic::Polynomial>& mutable_s_box_upper() { | ||
| return s_box_upper_; | ||
| } | ||
|
|
||
| private: | ||
| // The Lagrangians for s - s_box_lower >= 0. | ||
| VectorX<symbolic::Polynomial> s_box_lower_; | ||
| // The Lagrangians for s_box_upper - s >= 0. | ||
| VectorX<symbolic::Polynomial> s_box_upper_; | ||
| }; | ||
|
|
||
| /** | ||
| @param plant The plant for which we compute the C-space free boxes. It | ||
| must outlive this CspaceFreeBox object. | ||
| @param scene_graph The scene graph that has been connected with `plant`. It | ||
| must outlive this CspaceFreeBox object. | ||
| @param plane_order The order of the polynomials in the plane to separate a | ||
| pair of collision geometries. | ||
| @note CspaceFreeBox knows nothing about contexts. The plant and | ||
| scene_graph must be fully configured before instantiating this class. | ||
| */ | ||
| CspaceFreeBox(const multibody::MultibodyPlant<double>* plant, | ||
| const geometry::SceneGraph<double>* scene_graph, | ||
| SeparatingPlaneOrder plane_order, | ||
| const Options& options = Options{}); | ||
|
|
||
| private: | ||
| // Forward declare the tester class that will test the private members. | ||
| friend class CspaceFreeBoxTester; | ||
|
|
||
| /* | ||
| Computes the range of s from the box q_box_lower <= q <= q_box_upper. We also | ||
| set q_star = 0.5(q_box_lower + q_box_upper). | ||
| If q_box_lower is smaller than the robot position lower limit (or q_box_upper | ||
| is larger than the robot position upper limit), then we clamp q_box_lower (or | ||
| q_box_upper) within the robot position limits. | ||
| @throws error if any q_box_lower is larger than q_box_upper or the robot | ||
| position upper limit; similarly throws an error if any q_box_upper is smaller | ||
| than the robot position lower limit. | ||
| */ | ||
| void ComputeSBox(const Eigen::Ref<const Eigen::VectorXd>& q_box_lower, | ||
| const Eigen::Ref<const Eigen::VectorXd>& q_box_upper, | ||
| Eigen::VectorXd* s_box_lower, Eigen::VectorXd* s_box_upper, | ||
| Eigen::VectorXd* q_star) const; | ||
|
|
||
| /* | ||
| This class contains the polynomials that we wish to certify are non-negative. | ||
| */ | ||
| struct PolynomialsToCertify { | ||
| // We have the invariant plane_geometries_[i].plane_index == i. | ||
| std::vector<PlaneSeparatesGeometries> plane_geometries; | ||
| VectorX<symbolic::Polynomial> s_minus_s_box_lower; | ||
| VectorX<symbolic::Polynomial> s_box_upper_minus_s; | ||
| }; | ||
|
|
||
| // Generates the polynomials used for certifying the box s_box_lower <= s <= | ||
| // s_box_upper is collision free. | ||
| // @note The box [s_box_lower, s_box_upper] is already inside the | ||
| // tangent-configuration space box computed from the robot position | ||
| // lower/upper limits. | ||
| // TODO(hongkai.dai): after we finish implementing this class, consider to | ||
| // change the input argument to q_box_lower and q_box_upper, if ComputeSBox is | ||
| // only used with this function. | ||
| void GeneratePolynomialsToCertify( | ||
| const Eigen::Ref<const Eigen::VectorXd>& s_box_lower, | ||
| const Eigen::Ref<const Eigen::VectorXd>& s_box_upper, | ||
| const Eigen::Ref<const Eigen::VectorXd>& q_star, | ||
| const IgnoredCollisionPairs& ignored_collision_pairs, | ||
| PolynomialsToCertify* certify_polynomials) const; | ||
| }; | ||
| } // namespace optimization | ||
| } // namespace geometry | ||
| } // namespace drake |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.