diff --git a/geometry/optimization/dev/BUILD.bazel b/geometry/optimization/dev/BUILD.bazel index 59b07441d429..f0d108e3e346 100644 --- a/geometry/optimization/dev/BUILD.bazel +++ b/geometry/optimization/dev/BUILD.bazel @@ -76,13 +76,14 @@ drake_cc_googletest( drake_cc_googletest( name = "cspace_free_polytope_test", - timeout = "moderate", + timeout = "long", deps = [ ":c_iris_test_utilities", ":cspace_free_polytope", "//common/test_utilities:eigen_matrix_compare", "//common/test_utilities:symbolic_test_util", "//solvers:mosek_solver", + "//solvers:sos_basis_generator", ], ) diff --git a/geometry/optimization/dev/collision_geometry.cc b/geometry/optimization/dev/collision_geometry.cc index a83baaa0971f..3392d6a0867f 100644 --- a/geometry/optimization/dev/collision_geometry.cc +++ b/geometry/optimization/dev/collision_geometry.cc @@ -28,22 +28,19 @@ struct ReifyData { const multibody::RationalForwardKinematics::Pose& m_X_AB_multilinear, const multibody::RationalForwardKinematics& m_rational_forward_kin, - const std::optional& m_separating_margin, PlaneSide m_plane_side, GeometryId m_geometry_id, - double m_polytope_chebyshev_radius_multiplier, - std::vector* m_rationals, - std::optional>* m_unit_length_vector) + const VectorX& m_y_slack, + std::vector* m_rationals_for_points, + std::vector* m_rationals_for_matrix_sos) : a{&m_a}, b{&m_b}, X_AB_multilinear{&m_X_AB_multilinear}, rational_forward_kin{&m_rational_forward_kin}, - separating_margin{&m_separating_margin}, plane_side{m_plane_side}, geometry_id{m_geometry_id}, - polytope_chebyshev_radius_multiplier{ - m_polytope_chebyshev_radius_multiplier}, - rationals{m_rationals}, - unit_length_vector{m_unit_length_vector} {} + y_slack{&m_y_slack}, + rationals_for_points{m_rationals_for_points}, + rationals_for_matrix_sos{m_rationals_for_matrix_sos} {} // To avoid copying objects (which might be expensive), I store the // non-primitive-type objects with pointers. @@ -52,24 +49,21 @@ struct ReifyData { const multibody::RationalForwardKinematics::Pose* X_AB_multilinear; const multibody::RationalForwardKinematics* rational_forward_kin; - const std::optional* separating_margin; const PlaneSide plane_side; const GeometryId geometry_id; - const double polytope_chebyshev_radius_multiplier; - std::vector* rationals; - std::optional>* unit_length_vector; + const VectorX* y_slack; + std::vector* rationals_for_points; + std::vector* rationals_for_matrix_sos; }; // Compute the rational function -// (aᵀ*p_AQ + b) - (offset + separating_margin) if plane_side = kPositive -// -(aᵀ*p_AQ + b) - (offset + separating_margin) if plane_side = kNegative +// (aᵀ*p_AQ + b) - offset if plane_side = kPositive +// -(aᵀ*p_AQ + b) - offset if plane_side = kNegative // @param a_A The vector a measured in the frame A. // @param b The constant term in the separating plane. // @param p_GQ The position of the point Q in the geometry frame G. // @param X_BG The pose of the geometry frame G in the body frame B. // @param X_AB_multilinear The pose of body frame B expressed in frame A. -// @param separating_margin If set to std::nullopt, then we ignore the -// separating margin. // @param plane_side The side of the plane where Q lives. [[nodiscard]] symbolic::RationalFunction ComputePointOnPlaneSideRational( const Vector3& a_A, const symbolic::Polynomial& b, @@ -77,8 +71,7 @@ struct ReifyData { const math::RigidTransformd& X_BG, const multibody::RationalForwardKinematics::Pose& X_AB_multilinear, - double offset, const std::optional& separating_margin, - PlaneSide plane_side, + double offset, PlaneSide plane_side, const multibody::RationalForwardKinematics& rational_forward_kin) { // First compute p_AQ. const Eigen::Vector3d p_BQ = X_BG * p_GQ; @@ -86,95 +79,105 @@ struct ReifyData { X_AB_multilinear.position + X_AB_multilinear.rotation * p_BQ; // Compute lhs=aᵀ*p_AQ + b const symbolic::Polynomial lhs = a_A.dot(p_AQ) + b; - // Compute rhs=offset + separating_margin - // Note that separating_margin is NOT an indeterminates. - const symbolic::Polynomial rhs = - offset + (separating_margin.has_value() - ? symbolic::Polynomial( - {{symbolic::Monomial(), separating_margin.value()}}) - : symbolic::Polynomial(nullptr)); if (plane_side == PlaneSide::kPositive) { return rational_forward_kin.ConvertMultilinearPolynomialToRationalFunction( - lhs - rhs); + lhs - offset); } else { return rational_forward_kin.ConvertMultilinearPolynomialToRationalFunction( - -rhs - lhs); + -offset - lhs); } } -void ImplementPolytopeGeometry(const Eigen::Ref& p_GV, - const math::RigidTransformd& X_BG, void* data) { - // For a polytope (including box) to be on one side of the separating plane - // (and the other side geometry is also a polytope), we impose the following - // constraint: +void ImplementPointRationals(const Eigen::Ref& p_GV, + const math::RigidTransformd& X_BG, void* data) { + // As part of the conditions that a geometry is on one side of the separating + // plane, we impose the following constraint: // if plane_side = kPositive - // aᵀ*p_AV+b ≥ δ - // aᵀ*p_AC+b ≥ k*r + // aᵀ*p_AV+b ≥ 1 // if plane_side = kNegative - // aᵀ*p_AV+b ≤ -δ - // aᵀ*p_AC+b ≤ -k*r - // where C is the Chebyshev center of the polytope, and r is the distance - // from C to the polytope boundary. k is a positive scalar between (0, 1), - // defined in polytope_chebyshev_radius_multiplier_ - // - // We impose the condition aᵀ*p_AC+b ≥ k*r (or ≤ -k*r) to rule out the - // trivial solution a = 0, b=0, since the right hand side k*r (or -k*r) is - // strictly non-zero. - // - // If separating_margin has value, then we also impose the constraint |a|≤1. + // aᵀ*p_AV+b ≤ -1 auto* reify_data = static_cast(data); - const double offset{0}; - const int num_rationals = p_GV.cols() + 1; - reify_data->rationals->reserve(num_rationals); + const double offset{1}; + const int num_rationals_for_points = p_GV.cols(); + reify_data->rationals_for_points->reserve(num_rationals_for_points); // The position of the vertices V in the geometry frame G. for (int i = 0; i < p_GV.cols(); ++i) { - reify_data->rationals->push_back(ComputePointOnPlaneSideRational( + reify_data->rationals_for_points->push_back(ComputePointOnPlaneSideRational( *(reify_data->a), *(reify_data->b), p_GV.col(i), X_BG, - *(reify_data->X_AB_multilinear), offset, - *(reify_data->separating_margin), reify_data->plane_side, + *(reify_data->X_AB_multilinear), offset, reify_data->plane_side, *(reify_data->rational_forward_kin))); } - const HPolyhedron h_polyhedron{VPolytope(p_GV)}; - const Eigen::Vector3d p_GC = h_polyhedron.ChebyshevCenter(); - const double radius = ((h_polyhedron.b() - h_polyhedron.A() * p_GC).array() / - h_polyhedron.A().rowwise().norm().array()) - .minCoeff(); - reify_data->rationals->push_back(ComputePointOnPlaneSideRational( - *(reify_data->a), *(reify_data->b), p_GC, X_BG, - *(reify_data->X_AB_multilinear), - reify_data->polytope_chebyshev_radius_multiplier * radius, std::nullopt, - reify_data->plane_side, *(reify_data->rational_forward_kin))); - if (reify_data->separating_margin->has_value()) { - reify_data->unit_length_vector->emplace(*(reify_data->a)); - } else { - reify_data->unit_length_vector->reset(); - } +} + +void ImplementSpherePsdMatRational(const Eigen::Vector3d& p_GS, + const math::RigidTransformd& X_BG, + double radius, void* data) { + // As part of the condition that a sphere-based geometry (e.g., sphere, + // capsule) is on one side of the separating plane, we impose the constraint + // aᵀ*p_AS + b ≥ r|a| if plane_side = kPositive (1) + // aᵀ*p_AS + b ≤ -r|a| if plane_side = kNegative (2) + // (1) means that if plane_side = kPositive, the matrix + // ⌈aᵀp_AS + b aᵀ⌉ is psd. (3) + // ⌊ a (aᵀp_AS + b)/r²*I₃⌋ + // (3) is equivalent to the rational + // ⌈1⌉ᵀ⌈aᵀp_AS + b aᵀ⌉⌈1⌉ + // ⌊y⌋ ⌊ a (aᵀp_AS+ b)/r²*I₃⌋⌊y⌋ + // being non-negative. + // Likewise if plane_side = kNegative, the matrix + // ⌈-aᵀp_AS - b aᵀ⌉ is psd. (4) + // ⌊ a -(aᵀp_AS + b)/r²*I₃⌋ + // (4) is equivalent to the rational + // ⌈1⌉ᵀ⌈-aᵀp_AS - b aᵀ⌉⌈1⌉ + // ⌊y⌋ ⌊ a -(aᵀp_AS+ b)/r²*I₃⌋⌊y⌋ + // being non-negative. + auto* reify_data = static_cast(data); + + const Eigen::Vector3d p_BS = X_BG * p_GS; + const Vector3 p_AS = + reify_data->X_AB_multilinear->position + + reify_data->X_AB_multilinear->rotation * p_BS; + // Compute aᵀp_AS + b + const symbolic::RationalFunction a_dot_x_plus_b = + reify_data->rational_forward_kin + ->ConvertMultilinearPolynomialToRationalFunction( + reify_data->a->dot(p_AS) + *(reify_data->b)); + const Vector3 y_poly( + symbolic::Polynomial((*(reify_data->y_slack))(0)), + symbolic::Polynomial((*(reify_data->y_slack))(1)), + symbolic::Polynomial((*(reify_data->y_slack))(2))); + // Compute yᵀy + const symbolic::Polynomial y_squared{ + {{symbolic::Monomial((*(reify_data->y_slack))(0), 2), 1}, + {symbolic::Monomial((*(reify_data->y_slack))(1), 2), 1}, + {symbolic::Monomial((*(reify_data->y_slack))(2), 2), 1}}}; + const int sign = reify_data->plane_side == PlaneSide::kPositive ? 1 : -1; + reify_data->rationals_for_matrix_sos->emplace_back( + sign * a_dot_x_plus_b.numerator() + + 2 * reify_data->a->dot(y_poly) * a_dot_x_plus_b.denominator() + + y_squared / (radius * radius) * sign * a_dot_x_plus_b.numerator(), + a_dot_x_plus_b.denominator()); } class OnPlaneSideReifier : public ShapeReifier { public: OnPlaneSideReifier(const Shape* geometry, math::RigidTransformd X_BG, - GeometryId geometry_id, - double polytope_chebyshev_radius_multiplier) + GeometryId geometry_id) : geometry_{geometry}, X_BG_{std::move(X_BG)}, - geometry_id_{geometry_id}, - polytope_chebyshev_radius_multiplier_{ - polytope_chebyshev_radius_multiplier} {} + geometry_id_{geometry_id} {} void ProcessData( const Vector3& a, const symbolic::Polynomial& b, const multibody::RationalForwardKinematics::Pose& X_AB_multilinear, const multibody::RationalForwardKinematics& rational_forward_kin, - const std::optional& separating_margin, - PlaneSide plane_side, std::vector* rationals, - std::optional>* unit_length_vector) { - ReifyData data(a, b, X_AB_multilinear, rational_forward_kin, - separating_margin, plane_side, geometry_id_, - polytope_chebyshev_radius_multiplier_, rationals, - unit_length_vector); + PlaneSide plane_side, const VectorX& y_slack, + std::vector* rationals_for_points, + std::vector* rationals_for_matrix_sos) { + ReifyData data(a, b, X_AB_multilinear, rational_forward_kin, plane_side, + geometry_id_, y_slack, rationals_for_points, + rationals_for_matrix_sos); geometry_->Reify(this, &data); } @@ -193,60 +196,64 @@ class OnPlaneSideReifier : public ShapeReifier { p_GV.row(0) *= box.width() / 2; p_GV.row(1) *= box.depth() / 2; p_GV.row(2) *= box.height() / 2; - ImplementPolytopeGeometry(p_GV, X_BG_, data); + ImplementPointRationals(p_GV, X_BG_, data); } void ImplementGeometry(const Convex& convex, void* data) { const Eigen::Matrix3Xd p_GV = GetVertices(convex); - ImplementPolytopeGeometry(p_GV, X_BG_, data); + ImplementPointRationals(p_GV, X_BG_, data); } void ImplementGeometry(const Sphere& sphere, void* data) { - // If the sphere with radius r is on one side of the plane with a margin δ, + // If the sphere with radius r is on one side of the plane // it is equivalent to the following condition - // aᵀ*p_AS + b ≥ r + δ if plane_side = kPositive (1a) - // aᵀ*p_AS + b ≤ -(r + δ) if plane_side = kNegative (1b) - // |a| ≤ 1 (2) - // where S is the center of the sphere. - auto* reify_data = static_cast(data); - - reify_data->rationals->push_back(ComputePointOnPlaneSideRational( - *(reify_data->a), *(reify_data->b), Eigen::Vector3d::Zero(), X_BG_, - *(reify_data->X_AB_multilinear), sphere.radius(), - *(reify_data->separating_margin), reify_data->plane_side, - *(reify_data->rational_forward_kin))); - reify_data->unit_length_vector->emplace(*(reify_data->a)); + // aᵀ*p_AS + b ≥ r|a| if plane_side = kPositive (1a) + // aᵀ*p_AS + b ≥ 1 if plane_side = kPositive (1b) + // + // aᵀ*p_AS + b ≤ -r|a| if plane_side = kNegative (2a) + // aᵀ*p_AS + b ≤ -1 if plane_side = kNegative (2b) + // where S is the center of the sphere. p_AS is the position of S expressed + // in the frame A where the separating plane is also expressed. + + // First add the psd matrix constraint. + ImplementSpherePsdMatRational(Eigen::Vector3d::Zero(), X_BG_, + sphere.radius(), data); + // Now add the rational constraint + // aᵀ*p_AS + b ≥ 1 if plane_side = kPositive (1b) + // aᵀ*p_AS + b ≤ -1 if plane_side = kNegative (2b) + ImplementPointRationals(Eigen::Vector3d::Zero(), X_BG_, data); } void ImplementGeometry(const Capsule& capsule, void* data) { - // If the capsule with radius r is on one side of the plane with a margin δ, + // If the capsule with radius r is on one side of the plane // it is equivalent to the following condition - // aᵀ*p_AS1 + b ≥ r + δ if plane_side = kPositive (1a) - // aᵀ*p_AS2 + b ≥ r + δ if plane_side = kPositive (2a) - // aᵀ*p_AS1 + b ≤ -(r + δ) if plane_side = kNegative (1b) - // aᵀ*p_AS2 + b ≤ -(r + δ) if plane_side = kNegative (2b) - // |a| ≤ 1 (3) - // where S1 and S2 are the center of the two spheres on the two ends of the - // capsule. - auto* reify_data = static_cast(data); - Eigen::Matrix p_GS; - p_GS.col(0) = Eigen::Vector3d(0, 0, capsule.length() / 2); - p_GS.col(1) = Eigen::Vector3d(0, 0, -capsule.length() / 2); - reify_data->rationals->reserve(reify_data->rationals->size() + 2); - for (int i = 0; i < 2; ++i) { - reify_data->rationals->push_back(ComputePointOnPlaneSideRational( - *(reify_data->a), *(reify_data->b), p_GS.col(i), X_BG_, - *(reify_data->X_AB_multilinear), capsule.radius(), - *(reify_data->separating_margin), reify_data->plane_side, - *(reify_data->rational_forward_kin))); - } - reify_data->unit_length_vector->emplace(*(reify_data->a)); + // If plane_side = kPositive + // aᵀ*p_AS1 + b ≥ r|a| (1a) + // aᵀ*p_AS2 + b ≥ r|a| (1b) + // aᵀ*p_AO + b ≥ 1 (1c) + // + // If plane_side = kNegative + // aᵀ*p_AS1 + b ≤ -r|a| (2a) + // aᵀ*p_AS2 + b ≤ -r|a| (2b) + // aᵀ*p_AO + b ≤ -1 (2c) + // where S1 and S2 are the center of the two spheres, O is the center of + // the capsule. + // Please refer to our implementation for sphere to understand how we + // impose conditions (1) and (2). + // + // Add the psd-mat constraints. + ImplementSpherePsdMatRational(Eigen::Vector3d(0, 0, capsule.length() / 2), + X_BG_, capsule.radius(), data); + ImplementSpherePsdMatRational(Eigen::Vector3d(0, 0, -capsule.length() / 2), + X_BG_, capsule.radius(), data); + // aᵀ*p_AO + b ≥ 1 if plane_side = kPositive + // aᵀ*p_AO + b ≤ -1 if plane_side = kNegative + ImplementPointRationals(Eigen::Vector3d::Zero(), X_BG_, data); } const Shape* geometry_; math::RigidTransformd X_BG_; GeometryId geometry_id_; - double polytope_chebyshev_radius_multiplier_; }; class GeometryTypeReifier : public ShapeReifier { @@ -289,10 +296,9 @@ class GeometryTypeReifier : public ShapeReifier { const Shape* shape_; }; -class NumRationalsPerPlaneSideReifier : public ShapeReifier { +class NumRationalsForPointsReifier : public ShapeReifier { public: - explicit NumRationalsPerPlaneSideReifier(const Shape* shape) - : shape_{shape} {} + explicit NumRationalsForPointsReifier(const Shape* shape) : shape_{shape} {} int ProcessData() { int ret; @@ -305,13 +311,50 @@ class NumRationalsPerPlaneSideReifier : public ShapeReifier { void ImplementGeometry(const Box&, void* data) { auto* num = static_cast(data); - *num = 9; + *num = 8; } void ImplementGeometry(const Convex& convex, void* data) { auto* num = static_cast(data); const Eigen::Matrix3Xd p_GV = GetVertices(convex); - *num = p_GV.cols() + 1; + *num = p_GV.cols(); + } + + void ImplementGeometry(const Sphere&, void* data) { + auto* num = static_cast(data); + *num = 1; + } + + void ImplementGeometry(const Capsule&, void* data) { + auto* num = static_cast(data); + *num = 1; + } + + const Shape* shape_; +}; + +class NumRationalsForMatrixSosReifier : public ShapeReifier { + public: + explicit NumRationalsForMatrixSosReifier(const Shape* shape) + : shape_{shape} {} + + int ProcessData() { + int ret; + shape_->Reify(this, &ret); + return ret; + } + + private: + using ShapeReifier::ImplementGeometry; + + void ImplementGeometry(const Box&, void* data) { + auto* num = static_cast(data); + *num = 0; + } + + void ImplementGeometry(const Convex&, void* data) { + auto* num = static_cast(data); + *num = 0; } void ImplementGeometry(const Sphere&, void* data) { @@ -381,10 +424,61 @@ class DistanceToHalfspaceReifier : public ShapeReifier { capsule.radius(); } + void ImplementGeometry(const Cylinder& cylinder, void* data) { + // The distance from a point on the top rim [r*cosθ, r*sinθ, h/2] to the + // face {x | a.dot(x)+b=0} is (a(0)*r*cosθ + a(1)*r*sinθ + a(2)*h/2 + b) / + // |a|. Taking the minimum over θ, we get the distance from the top rim to + // the face as (-r * |[a(0), a(1)]| + a(2)*h/2 + b) / |a|. Similarly, the + // distance from the bottom rim [r*cosθ, r*sinθ, -h/2] to the face {x | + // a.dot(x)+b=0} is + // (-r * |[a(0), a(1)]| - a(2)*h/2 + b) / |a|. + double* distance = static_cast(data); + *distance = + (-cylinder.radius() * a_G_.head<2>().norm() + + (a_G_(2) >= 0 ? -a_G_(2) : a_G_(2)) * cylinder.length() / 2 + b_G_) / + a_G_.norm(); + } + const Shape* shape_; Eigen::Vector3d a_G_; double b_G_; }; + +class YSlackSizeReifier : public ShapeReifier { + public: + explicit YSlackSizeReifier(const Shape* shape) : shape_{shape} {} + + double ProcessData() { + int ret; + shape_->Reify(this, &ret); + return ret; + } + + private: + using ShapeReifier::ImplementGeometry; + + void ImplementGeometry(const Box&, void* data) { + int* y_slack_size = static_cast(data); + *y_slack_size = 0; + } + + void ImplementGeometry(const Convex&, void* data) { + int* y_slack_size = static_cast(data); + *y_slack_size = 0; + } + + void ImplementGeometry(const Sphere&, void* data) { + int* y_slack_size = static_cast(data); + *y_slack_size = 3; + } + + void ImplementGeometry(const Capsule&, void* data) { + int* y_slack_size = static_cast(data); + *y_slack_size = 3; + } + + const Shape* shape_; +}; } // namespace void CollisionGeometry::OnPlaneSide( @@ -392,14 +486,12 @@ void CollisionGeometry::OnPlaneSide( const multibody::RationalForwardKinematics::Pose& X_AB_multilinear, const multibody::RationalForwardKinematics& rational_forward_kin, - const std::optional& separating_margin, - PlaneSide plane_side, std::vector* rationals, - std::optional>* unit_length_vector) const { - OnPlaneSideReifier reifier(geometry_, X_BG_, id_, - polytope_chebyshev_radius_multiplier_); - reifier.ProcessData(a, b, X_AB_multilinear, rational_forward_kin, - separating_margin, plane_side, rationals, - unit_length_vector); + PlaneSide plane_side, const VectorX& y_slack, + std::vector* rationals_for_points, + std::vector* rationals_for_matrix_sos) const { + OnPlaneSideReifier reifier(geometry_, X_BG_, id_); + reifier.ProcessData(a, b, X_AB_multilinear, rational_forward_kin, plane_side, + y_slack, rationals_for_points, rationals_for_matrix_sos); } GeometryType CollisionGeometry::type() const { @@ -407,8 +499,18 @@ GeometryType CollisionGeometry::type() const { return reifier.ProcessData(); } -int CollisionGeometry::num_rationals_per_side() const { - NumRationalsPerPlaneSideReifier reifier(geometry_); +int CollisionGeometry::num_rationals_for_points() const { + NumRationalsForPointsReifier reifier(geometry_); + return reifier.ProcessData(); +} + +int CollisionGeometry::num_rationals_for_matrix_sos() const { + NumRationalsForMatrixSosReifier reifier(geometry_); + return reifier.ProcessData(); +} + +int CollisionGeometry::y_slack_size() const { + YSlackSizeReifier reifier(geometry_); return reifier.ProcessData(); } diff --git a/geometry/optimization/dev/collision_geometry.h b/geometry/optimization/dev/collision_geometry.h index bff162500ea9..d8066eb4b80c 100644 --- a/geometry/optimization/dev/collision_geometry.h +++ b/geometry/optimization/dev/collision_geometry.h @@ -50,52 +50,28 @@ class CollisionGeometry { const math::RigidTransformd& X_BG() const { return X_BG_; } - /** - When we constrain a polytope to be on one side of the plane (for example the - positive side), we impose the constraint - aᵀ*p_AVᵢ + b ≥ δ - aᵀ*p_AC + b ≥ k * r - where Vᵢ being the i'th vertex of the polytope. C is the Chebyshev center of - the polytope, r is the radius (namely the distance from the Chebyshev center - to the boundary of the polytope), and k is a positive scalar. This function - returns the value of k. - */ - double polytope_chebyshev_radius_multiplier() const { - return polytope_chebyshev_radius_multiplier_; - } - - /** Setter for polytope_chebyshev_radius_multiplier. */ - void set_polytope_chebyshev_radius_multiplier(double value) { - DRAKE_DEMAND(value > 0 && value <= 1); - polytope_chebyshev_radius_multiplier_ = value; - } - - /** - To impose the geometric constraint that this collision geometry is on one - side of a plane {x|aᵀx+b=0} with a certain margin, we can equivalently write - this constraint with the following conditions - 1. Some rational functions are always non-negative. - 2. A certain vector has length <= 1. - - For example, if this geometry is a sphere with radius r, and we constrain - that the sphere is on the positive side of the plane with a margin δ, we can - impose the following constraint - aᵀ*p_AS(s) + b ≥ r + δ (1) - |a| ≤ 1 (2) - where p_AS(s) is the position of the sphere center S expressed in a frame A. - The left hand side of equation (1) is a rational function of indeterminate - s. Constraint (2) says the vector a has length <= 1. - - Similarly we can write down the conditions for other geometry types, - including polytopes and capsules. - - Note that when we don't require a separating margin δ, and both this - geometry and the geometry on the other side of the plane are polytopes, then - we consider the constraint - aᵀp_AVᵢ(s) + b ≥ 1 if the polytope is on the positive side of the plane, and - aᵀp_AVᵢ(s) + b ≤ -1 if the polytope is on the negative side of the plane. - Note that in this case we don't have the "vector with length <= 1 " - constraint. + /** Impose the constraint that the geometry is on a given side of the plane {x + | aᵀx+b=0}. + + For example, to impose the constraint that a polytope is on the positive + side of the plane, we consider the following constraints + aᵀp_AVᵢ + b ≥ 1 (1) + where Vᵢ is the i'th vertex of the polytope. + (1) says rational functions are non-negative. + + To impose the constraint that a sphere is on positive side of the + plane, we consider the following constraints + aᵀp_AS + b ≥ r*|a| (2a) + aᵀp_AS + b ≥ 1 (2b) + where S is the sphere center, r is the sphere radius. + + We can reformulate (2a) as the following constraint + ⌈aᵀp_AS + b aᵀ⌉ is psd. (3) + ⌊ a (aᵀp_AS + b)/r²*I₃⌋ + (3) is equivalent to the rational + ⌈1⌉ᵀ*⌈aᵀp_AS + b aᵀ⌉*⌈1⌉ + ⌊y⌋ ⌊ a (aᵀp_AS+ b)/r²*I₃⌋ ⌊y⌋ + is positive. @param a The normal vector in the separating plane. a is expressed in frame A. @@ -106,40 +82,49 @@ class CollisionGeometry { RationalForwardKinematics::CalcBodyPoseAsMultilinearPolynomial. @param rational_forward_kin This object is constructed with the MultibodyPlant containing this collision geometry. - @param separating_margin δ in the documentation above. @param plane_side Whether the geometry is on the positive or negative side of the plane. - @param other_side_geometry_type The type of the geometry on the other side of - the plane. - @param[out] rationals The rationals that should be positive when the geometry - is on the designated side of the plane. - @param[out] unit_length_vector The vector that should have length <= 1 when - the geometry is on the designated side of the plane. + @param y_slack The slack variable y in the documentation above, used for + non-polytopic geometries. + @param[out] rationals_for_points The rational functions that need to be + positive to represent a point is on a given side of the plane. + @param[out] rationals_for_matrix_sos The rationals (whose indeterminates are + y and s) that need to be positive. This is used for non-polytopic geometries. */ void OnPlaneSide( const Vector3& a, const symbolic::Polynomial& b, const multibody::RationalForwardKinematics::Pose& X_AB_multilinear, const multibody::RationalForwardKinematics& rational_forward_kin, - const std::optional& separating_margin, - PlaneSide plane_side, std::vector* rationals, - std::optional>* unit_length_vector) const; + PlaneSide plane_side, const VectorX& y_slack, + std::vector* rationals_for_points, + std::vector* rationals_for_matrix_sos) const; [[nodiscard]] GeometryType type() const; /** - Returns the number of rationals in the condition "this geometry is on one - side of the plane." + Returns the number of rationals for points in the condition "this geometry is + on one side of the plane." */ - [[nodiscard]] int num_rationals_per_side() const; + [[nodiscard]] int num_rationals_for_points() const; + + /** + Returns the number of rationals_for_matrix_sos in the condition "this + geometry is on one side of the plane." + */ + [[nodiscard]] int num_rationals_for_matrix_sos() const; + + /** + Returns the size of y used in the matrix-sos constraint to impose the + geometry on one side of a plane. + */ + [[nodiscard]] int y_slack_size() const; private: const Shape* geometry_; multibody::BodyIndex body_index_; geometry::GeometryId id_; math::RigidTransformd X_BG_; - - double polytope_chebyshev_radius_multiplier_{1E-3}; }; /** Computes the signed distance from `collision_geometry` to the halfspace ℋ, diff --git a/geometry/optimization/dev/cspace_free_polytope.cc b/geometry/optimization/dev/cspace_free_polytope.cc index 88a4d691be3f..d26c8f9579dc 100644 --- a/geometry/optimization/dev/cspace_free_polytope.cc +++ b/geometry/optimization/dev/cspace_free_polytope.cc @@ -96,23 +96,32 @@ struct BodyPairHash { /** The monomials in the numerator of body point position has the form ∏ᵢ pow(tᵢ, dᵢ) * ∏ⱼ pow(xⱼ, cⱼ), where tᵢ is the configuration variable tᵢ = tan(Δθᵢ/2) - for the revolute joint, dᵢ = 0, 1, 2, on the kinematic chain between the pair - of bodies; xⱼ is the prismatic joint displacement on the same kinematic chain, - cⱼ = 0 or 1. When we use SeparatingPlaneOrder = kAffine, the monomial basis - would include all the monomials in the form ∏ᵢ pow(sᵢ, mᵢ) with mᵢ = 0 or 1. - - @param[in/out] map_body_to_monomial_basis stores all the monomials basis + for the revolute joint, dᵢ = 0, 1, or 2, on the kinematic chain between the + pair of bodies; xⱼ is the prismatic joint displacement on the same kinematic + chain, cⱼ = 0 or 1. When we use SeparatingPlaneOrder = kAffine, the monomial + basis would include all the monomials in the form ∏ᵢ pow(sᵢ, nᵢ) with nᵢ = 0 + or 1, s ∈ {tᵢ, xⱼ}. Let's denote this monomial basis as m(s). + + When we have non-polytopic collision geometries, we will also impose a matrix + of rational functions being positive semidefinite. This requires a matrix-sos + constraint with slack variable y. We will need the monomial basis in the form + yᵢ*m(s) with i=0,1,2. + + @param[in/out] map_body_to_monomial_basis_array stores all the monomials basis already computed. - @param[out] monomial_basis Te monomial basis for this pair of body. + @param[out] monomial_basis_array The monomial basis array for this pair of + body, monomial_basis_array = [m(s), y₀*m(s), y₁*m(s), y₂*m(s)] */ -void FindMonomialBasis( +void FindMonomialBasisArray( const multibody::RationalForwardKinematics& rational_forward_kin, + const Vector3& y_slack, const SortedPair& body_pair, std::unordered_map, - VectorX>* map_body_to_monomial_basis, - VectorX* monomial_basis) { - auto it = map_body_to_monomial_basis->find(body_pair); - if (it == map_body_to_monomial_basis->end()) { + std::array, 4>>* + map_body_to_monomial_basis_array, + std::array, 4>* monomial_basis_array) { + auto it = map_body_to_monomial_basis_array->find(body_pair); + if (it == map_body_to_monomial_basis_array->end()) { const std::vector mobilizer_indices = multibody::internal::FindMobilizersOnPath(rational_forward_kin.plant(), body_pair.first(), @@ -137,15 +146,24 @@ void FindMonomialBasis( } if (s_set.empty()) { // No s variable. The monomial basis is just [1]. - monomial_basis->resize(1); - (*monomial_basis)(0) = symbolic::Monomial(); + (*monomial_basis_array)[0].resize(1); + (*monomial_basis_array)[0](0) = symbolic::Monomial(); } else { - *monomial_basis = symbolic::CalcMonomialBasisOrderUpToOne(s_set); + (*monomial_basis_array)[0] = + symbolic::CalcMonomialBasisOrderUpToOne(s_set); + } + // monomial_basis_array[i+1] = y(i) * monomial_basis_array[0] + for (int i = 0; i < 3; ++i) { + const symbolic::Monomial yi(y_slack(i)); + (*monomial_basis_array)[i + 1].resize((*monomial_basis_array)[0].rows()); + for (int j = 0; j < (*monomial_basis_array)[0].rows(); ++j) { + (*monomial_basis_array)[i + 1](j) = yi * (*monomial_basis_array)[0](j); + } } - it = map_body_to_monomial_basis->emplace_hint(it, body_pair, - *monomial_basis); + it = map_body_to_monomial_basis_array->emplace_hint(it, body_pair, + *monomial_basis_array); } else { - *monomial_basis = it->second; + *monomial_basis_array = it->second; } } @@ -167,28 +185,17 @@ void SymmetricMatrixFromLowerTriangularPart( } } -// Computes the Lagrangian, and add the constraints on the Lagrangian Gram -// matrix to the program. If it is redundant, then set the Lagrangian to zero -// and constrain the Gram matrix to zero; otherwise constrain the Gram matrix to -// be PSD. -void AddLagrangian( - solvers::MathematicalProgram* prog, - const VectorX& monomial_basis, - const Eigen::Ref>& gram_lower, - bool redundant, symbolic::Polynomial* lagrangian, - MatrixX* lagrangian_gram) { - SymmetricMatrixFromLowerTriangularPart( - monomial_basis.rows(), gram_lower, lagrangian_gram); - DRAKE_DEMAND(gram_lower.rows() == - monomial_basis.rows() * (monomial_basis.rows() + 1) / 2); - if (redundant) { - // Lagrangian is 0. - *lagrangian = symbolic::Polynomial(); - prog->AddBoundingBoxConstraint(0, 0, gram_lower); +// TODO(hongkai.dai): move this change to MathematicalProgram. +void AddPsdConstraint(solvers::MathematicalProgram* prog, + const MatrixX& X) { + DRAKE_DEMAND(X.rows() == X.cols()); + if (X.rows() == 1) { + prog->AddBoundingBoxConstraint(0, kInf, X(0, 0)); + } else if (X.rows() == 2) { + prog->AddRotatedLorentzConeConstraint( + Vector3(X(0, 0), X(1, 1), X(0, 1))); } else { - *lagrangian = symbolic::CalcPolynomialWLowerTriangularPart(monomial_basis, - gram_lower); - prog->AddPositiveSemidefiniteConstraint(*lagrangian_gram); + prog->AddPositiveSemidefiniteConstraint(X); } } @@ -204,40 +211,6 @@ bool IsFutureReady(const std::future& future) { return (status == std::future_status::ready); } -[[nodiscard]] CspaceFreePolytope::SeparationCertificateResult GetSolution( - int plane_index, - const CspaceFreePolytope::SeparationCertificate& certificate, - const std::optional& separating_margin, - const Vector3& a, const symbolic::Polynomial& b, - const solvers::MathematicalProgramResult& result) { - CspaceFreePolytope::SeparationCertificateResult ret; - ret.plane_index = plane_index; - ret.positive_side_lagrangians.reserve( - certificate.positive_side_lagrangians.size()); - for (const auto& lagrangians : certificate.positive_side_lagrangians) { - ret.positive_side_lagrangians.push_back(lagrangians.GetSolution(result)); - } - ret.negative_side_lagrangians.reserve( - certificate.negative_side_lagrangians.size()); - for (const auto& lagrangians : certificate.negative_side_lagrangians) { - ret.negative_side_lagrangians.push_back(lagrangians.GetSolution(result)); - } - ret.unit_length_lagrangians.reserve( - certificate.unit_length_lagrangians.size()); - for (const auto& lagrangians : certificate.unit_length_lagrangians) { - ret.unit_length_lagrangians.push_back(lagrangians.GetSolution(result)); - } - if (separating_margin.has_value()) { - ret.separating_margin.emplace( - result.GetSolution(separating_margin.value())); - } - for (int i = 0; i < 3; ++i) { - ret.a(i) = result.GetSolution(a(i)); - } - ret.b = result.GetSolution(b); - return ret; -} - solvers::MathematicalProgramResult SolveWithBackoff( solvers::MathematicalProgram* prog, std::optional backoff_scale, const std::optional& solver_options, @@ -246,40 +219,174 @@ solvers::MathematicalProgramResult SolveWithBackoff( auto solver = solvers::MakeSolver(solver_id); solvers::MathematicalProgramResult result; solver->Solve(*prog, std::nullopt, solver_options, &result); - if (result.is_success()) { - if (backoff_scale.has_value() && !(prog->linear_costs().empty())) { - DRAKE_DEMAND(prog->linear_costs().size() == 1); - const double cost_val = result.get_optimal_cost(); - const double cost_upper_bound = - cost_val > 0 ? (1 + backoff_scale.value()) * cost_val - : (1 - backoff_scale.value()) * cost_val; - prog->AddLinearConstraint( - prog->linear_costs()[0].evaluator()->a(), -kInf, - cost_upper_bound - prog->linear_costs()[0].evaluator()->b(), - prog->linear_costs()[0].variables()); - prog->RemoveCost(prog->linear_costs()[0]); - solver->Solve(*prog, std::nullopt, solver_options, &result); - if (!result.is_success()) { - drake::log()->info("Failed in backoff."); - } + if (!result.is_success()) { + drake::log()->warn("Failed before backoff."); + } + if (backoff_scale.has_value() && !(prog->linear_costs().empty())) { + DRAKE_DEMAND(prog->linear_costs().size() == 1); + const double cost_val = result.get_optimal_cost(); + const double cost_upper_bound = + cost_val > 0 ? (1 + backoff_scale.value()) * cost_val + : (1 - backoff_scale.value()) * cost_val; + prog->AddLinearConstraint( + prog->linear_costs()[0].evaluator()->a(), -kInf, + cost_upper_bound - prog->linear_costs()[0].evaluator()->b(), + prog->linear_costs()[0].variables()); + prog->RemoveCost(prog->linear_costs()[0]); + solver->Solve(*prog, std::nullopt, solver_options, &result); + if (!result.is_success()) { + drake::log()->info("Failed in backoff."); } - } else { - drake::log()->error("Failed before backoff."); } return result; } -void AddSosPolynomial( - solvers::MathematicalProgram* prog, - const VectorX& monomial_basis, - const Eigen::Ref>& gram_lower, - symbolic::Polynomial* poly, MatrixX* gram) { - SymmetricMatrixFromLowerTriangularPart( - monomial_basis.rows(), gram_lower, gram); - prog->AddPositiveSemidefiniteConstraint(*gram); - *poly = - symbolic::CalcPolynomialWLowerTriangularPart(monomial_basis, gram_lower); +int GetYSize(const CollisionGeometry& collision_geometry, + bool using_matrix_sos) { + return using_matrix_sos ? collision_geometry.y_slack_size() : 0; } + +// Return the total size of the lower triangular variables in the Gram +// matrices. +int GetGramVarSize( + const std::array, 4>& monomial_basis_array, + bool with_cross_y, int num_y) { + auto gram_lower_size = [](int gram_rows) { + return gram_rows * (gram_rows + 1) / 2; + }; + if (num_y == 0) { + // We only need to use monomial_basis_array[0]. + return gram_lower_size(monomial_basis_array[0].rows()); + } else { + // We will use the monomials that contain y for the psd_mat. + // We will denote monomial_basis_array[0] as m(s), and + // monomial_basis_array[i+1] as yᵢ * m(s). + if (with_cross_y) { + // The monomials basis we use are [m(s); y₀*m(s), ..., yₙ * m(s)] where n + // = num_y - 1. + int gram_rows = monomial_basis_array[0].rows(); + for (int i = 0; i < num_y; ++i) { + gram_rows += monomial_basis_array[i + 1].rows(); + } + return gram_lower_size(gram_rows); + } else { + // Use multiple monomial basis, each monomials basis is [m(s); yᵢ*m(s)]. + int ret = 0; + for (int i = 0; i < num_y; ++i) { + ret += gram_lower_size(monomial_basis_array[0].rows() + + monomial_basis_array[i + 1].rows()); + } + return ret; + } + } +} + +// Given the monomial_basis_array, compute the sos polynomial. +// monomial_basis_array contains [m(s), y₀*m(s), y₁*m(s), y₂*m(s)]. +// +// If num_y == 0, then the sos polynomial is just +// m(s)ᵀ * X * m(s) +// where X is a Gram matrix, `grams` is a length-1 vector containing X. +// +// If num_y != 0 and with_cross_y = true, then the sos polynomial is +// ⌈ m(s)⌉ᵀ * Y * ⌈ m(s)⌉ +// | y₀*m(s)| | y₀*m(s)| +// | ... | | ... | +// ⌊ yₙ*m(s)⌋ ⌊ yₙ*m(s)⌋ +// where n = num_y-1. Y is a Gram matrix, `grams` is a length-1 vector +// containing Y. +// +// if num_y != 0 and with_cross_y = false, then the sos polynomial is +// ∑ᵢ ⌈ m(s)⌉ᵀ * Zᵢ * ⌈ m(s)⌉ +// ⌊ yᵢ*m(s)⌋ ⌊ yᵢ*m(s)⌋ +// where Zᵢ is a Gram matrix, i = 0, ..., num_y-1. `grams` is a vector of +// length `num_y`, and grams[i] = Zᵢ +struct GramAndMonomialBasis { + GramAndMonomialBasis( + const std::array, 4>& monomial_basis_array, + bool with_cross_y, int num_y) { + this->gram_var_size = + GetGramVarSize(monomial_basis_array, with_cross_y, num_y); + if (num_y == 0) { + // We only need to use monomial_basis_array[0]. + this->grams.emplace_back(monomial_basis_array[0].rows(), + monomial_basis_array[0].rows()); + this->monomial_basis.push_back(monomial_basis_array[0]); + } else { + // We will use the monomials that contain y for the psd_mat. + // We will denote monomial_basis_array[0] as m(s), and + // monomial_basis_array[i+1] as yᵢ * m(s). + if (with_cross_y) { + // The monomials basis we use is [m(s); y₀*m(s), ..., yₙ * m(s)] where + // n = num_y - 1. + int gram_rows = monomial_basis_array[0].rows(); + for (int i = 0; i < num_y; ++i) { + gram_rows += monomial_basis_array[i + 1].rows(); + } + this->grams.emplace_back(gram_rows, gram_rows); + this->monomial_basis.emplace_back(gram_rows); + this->monomial_basis[0].topRows(monomial_basis_array[0].rows()) = + monomial_basis_array[0]; + gram_rows = monomial_basis_array[0].rows(); + for (int i = 0; i < num_y; ++i) { + this->monomial_basis[0].segment(gram_rows, + monomial_basis_array[i + 1].rows()) = + monomial_basis_array[i + 1]; + gram_rows += monomial_basis_array[i + 1].rows(); + } + } else { + // Use multiple monomial bases, each monomial basis is [m(s); yᵢ*m(s)]. + for (int i = 0; i < num_y; ++i) { + const int gram_rows = monomial_basis_array[0].rows() + + monomial_basis_array[i + 1].rows(); + this->grams.emplace_back(gram_rows, gram_rows); + this->monomial_basis.emplace_back(gram_rows); + this->monomial_basis.back().topRows(monomial_basis_array[0].rows()) = + monomial_basis_array[0]; + this->monomial_basis.back().bottomRows( + monomial_basis_array[i + 1].rows()) = monomial_basis_array[i + 1]; + } + } + } + } + + // Add the constraint that the polynomial represented by this Gram and + // monomial basis is sos. + // @param is_zero_poly If true, then constrain all the Gram matrices to be + // zero. + void AddSos(solvers::MathematicalProgram* prog, + const Eigen::Ref>& gram_lower, + bool is_zero_poly, symbolic::Polynomial* poly) { + int gram_var_count = 0; + for (auto& gram : this->grams) { + const int gram_lower_size = gram.rows() * (gram.rows() + 1) / 2; + SymmetricMatrixFromLowerTriangularPart( + gram.rows(), gram_lower.segment(gram_var_count, gram_lower_size), + &gram); + gram_var_count += gram_lower_size; + } + if (is_zero_poly) { + prog->AddBoundingBoxConstraint(0, 0, gram_lower); + *poly = symbolic::Polynomial(); + } else { + *poly = symbolic::Polynomial(); + gram_var_count = 0; + for (int i = 0; i < static_cast(this->grams.size()); ++i) { + AddPsdConstraint(prog, this->grams[i]); + const int gram_lower_size = + this->grams[i].rows() * (this->grams[i].rows() + 1) / 2; + *poly += symbolic::CalcPolynomialWLowerTriangularPart( + this->monomial_basis[i], + gram_lower.segment(gram_var_count, gram_lower_size)); + gram_var_count += gram_lower_size; + } + } + } + + int gram_var_size; + std::vector> grams; + std::vector> monomial_basis; +}; } // namespace CspaceFreePolytope::CspaceFreePolytope( @@ -292,13 +399,8 @@ CspaceFreePolytope::CspaceFreePolytope( link_geometries_{GetCollisionGeometries(*plant, *scene_graph)}, plane_order_{plane_order}, s_set_{rational_forward_kin_.s()}, - q_star_{q_star} { - for (const auto& [body, geometries] : link_geometries_) { - for (auto& geometry : geometries) { - geometry->set_polytope_chebyshev_radius_multiplier( - options.polytope_chebyshev_radius_multiplier); - } - } + q_star_{q_star}, + with_cross_y_{options.with_cross_y} { // Create separating planes. // collision_pairs maps each pair of body to the pair of collision geometries // on that pair of body. @@ -354,10 +456,8 @@ CspaceFreePolytope::CspaceFreePolytope( static_cast(separating_planes_.size()) - 1); } } - // Currently we only need to impose the constraint that a 3D vector to have - // unit length, which requires a 3 + 1 = 4 dimensional y_slack. - y_slack_.resize(4); - for (int i = 0; i < 4; ++i) { + + for (int i = 0; i < 3; ++i) { y_slack_(i) = symbolic::Variable("y" + std::to_string(i)); } @@ -367,15 +467,12 @@ CspaceFreePolytope::CspaceFreePolytope( rational_forward_kin_.plant().GetPositionUpperLimits(), q_star_); CalcSBoundsPolynomial(); - plane_geometries_w_margin_ = GenerateRationals(true); - plane_geometries_wo_margin_ = GenerateRationals(false); + this->GenerateRationals(); this->CalcMonomialBasis(); } -std::vector CspaceFreePolytope::GenerateRationals( - bool search_separating_margin) const { - std::vector ret; +void CspaceFreePolytope::GenerateRationals() { // There can be multiple geometries on the same pair, hence the body pose will // be reused. We use this map to store the body pose to avoid redundant // computation. @@ -391,11 +488,8 @@ std::vector CspaceFreePolytope::GenerateRationals( // Compute X_AB for both side of the geometries. std::vector positive_side_rationals; std::vector negative_side_rationals; - std::vector> unit_length_vectors; - std::optional separating_margin = std::nullopt; - if (search_separating_margin) { - separating_margin.emplace(symbolic::Variable("margin")); - } + std::vector positive_side_psd_mat; + std::vector negative_side_psd_mat; for (const PlaneSide plane_side : {PlaneSide::kPositive, PlaneSide::kNegative}) { const CollisionGeometry* link_geometry = @@ -413,52 +507,56 @@ std::vector CspaceFreePolytope::GenerateRationals( } const multibody::RationalForwardKinematics::Pose& X_AB_multilinear = it->second; - std::optional> unit_length_vec; auto& rationals = plane_side == PlaneSide::kPositive ? positive_side_rationals : negative_side_rationals; + auto& psd_mat = plane_side == PlaneSide::kPositive + ? positive_side_psd_mat + : negative_side_psd_mat; link_geometry->OnPlaneSide(separating_plane.a, separating_plane.b, X_AB_multilinear, rational_forward_kin_, - separating_margin, plane_side, &rationals, - &unit_length_vec); - - if (unit_length_vec.has_value()) { - if (unit_length_vectors.empty()) { - unit_length_vectors.push_back(unit_length_vec.value()); - } else { - // The unit length vector for the positive side geometry might be - // the same as the unit length vector for the negative side - // geometry, for example if both geometries are spheres. So we check - // if unit_length_vec is the same as the one in unit_length_vectors; - // if they are the same, then we don't add it. - bool existing_unit_length_vec = false; - for (const auto& vec : unit_length_vectors) { - bool match = true; - if (vec.rows() == unit_length_vec->rows()) { - for (int i = 0; i < vec.rows(); ++i) { - if (!vec(i).EqualTo((*unit_length_vec)(i))) { - match = false; - break; - } - } - } else { - match = false; - } - if (match) { - existing_unit_length_vec = true; - break; - } - } - if (!existing_unit_length_vec) { - unit_length_vectors.push_back(unit_length_vec.value()); - } - } - } + plane_side, y_slack_, &rationals, &psd_mat); } - ret.emplace_back(positive_side_rationals, negative_side_rationals, - unit_length_vectors, plane_index, separating_margin); + // For a non-polytopic geometry (sphere, capsule, etc) to be on the positive + // side of the plane, we require that aᵀ*p_AS + b ≥ r|a|. To avoid a trivial + // solution (a = b = 0), we also include the constraint that aᵀ*p_AS + b ≥ 1 + // (see ImplementGeometry for the Sphere type). This second constraint is + // redundant if the negative side geometry is a polytope or if the negative + // side geometry is also non-polytopic, and we have included the constraint + // aᵀ*p_AS2 + b ≤ -1. We avoid adding the redundant constraint to reduce the + // program size. + if (separating_plane.positive_side_geometry->type() == + GeometryType::kPolytope && + separating_plane.negative_side_geometry->type() == + GeometryType::kPolytope) { + plane_geometries_.emplace_back( + positive_side_rationals, negative_side_rationals, + positive_side_psd_mat, negative_side_psd_mat, plane_index); + } else if (separating_plane.positive_side_geometry->type() == + GeometryType::kPolytope && + separating_plane.negative_side_geometry->type() != + GeometryType::kPolytope) { + // Do not add the negative side rationals. + plane_geometries_.emplace_back( + positive_side_rationals, std::vector(), + positive_side_psd_mat, negative_side_psd_mat, plane_index); + } else if (separating_plane.positive_side_geometry->type() != + GeometryType::kPolytope && + separating_plane.negative_side_geometry->type() == + GeometryType::kPolytope) { + // Do not add the positive side rationals. + plane_geometries_.emplace_back( + std::vector(), negative_side_rationals, + positive_side_psd_mat, negative_side_psd_mat, plane_index); + } else { + // Both sides are non-polytopic, we only need the rationals from one side, + // we choose the positive side. + plane_geometries_.emplace_back( + positive_side_rationals, std::vector(), + positive_side_psd_mat, negative_side_psd_mat, plane_index); + } + DRAKE_DEMAND(plane_geometries_[plane_index].plane_index == plane_index); } - return ret; } // Find the redundant inequalities in C * s <= d, s_lower <= s <= s_upper. @@ -560,251 +658,172 @@ CspaceFreePolytope::SeparatingPlaneLagrangians::GetSolution( return ret; } -CspaceFreePolytope::UnitLengthLagrangians -CspaceFreePolytope::UnitLengthLagrangians::GetSolution( - const solvers::MathematicalProgramResult& result) const { - CspaceFreePolytope::UnitLengthLagrangians ret(this->polytope.rows(), - this->s_lower.rows()); - for (int i = 0; i < this->polytope.rows(); ++i) { - ret.polytope(i) = result.GetSolution(this->polytope(i)); - } - for (int i = 0; i < this->s_lower.rows(); ++i) { - ret.s_lower(i) = result.GetSolution(this->s_lower(i)); - ret.s_upper(i) = result.GetSolution(this->s_upper(i)); - } - ret.y_square = result.GetSolution(this->y_square); - return ret; -} - void CspaceFreePolytope::CalcMonomialBasis() { - for (int i = 0; i < static_cast(separating_planes_.size()); ++i) { - const auto& plane = separating_planes_[i]; + for (int plane_index = 0; + plane_index < static_cast(separating_planes_.size()); + ++plane_index) { + const auto& plane = separating_planes_[plane_index]; for (const auto collision_geometry : {plane.positive_side_geometry, plane.negative_side_geometry}) { const SortedPair body_pair( plane.expressed_body, collision_geometry->body_index()); - VectorX monomial_basis; - FindMonomialBasis(rational_forward_kin_, body_pair, - &map_body_to_monomial_basis_, &monomial_basis); + std::array, 4> monomial_basis_array; + FindMonomialBasisArray(rational_forward_kin_, y_slack_, body_pair, + &map_body_to_monomial_basis_array_, + &monomial_basis_array); } } } -CspaceFreePolytope::SeparationCertificate +CspaceFreePolytope::SeparationCertificateProgram CspaceFreePolytope::ConstructPlaneSearchProgram( const PlaneSeparatesGeometries& plane_geometries, const VectorX& d_minus_Cs, const std::unordered_set& C_redundant_indices, const std::unordered_set& s_lower_redundant_indices, const std::unordered_set& s_upper_redundant_indices) const { - SeparationCertificate ret; + SeparationCertificateProgram ret; ret.prog->AddIndeterminates(rational_forward_kin_.s()); const auto& plane = separating_planes_[plane_geometries.plane_index]; ret.prog->AddDecisionVariables(plane.decision_variables); - if (plane_geometries.separating_margin.has_value()) { - ret.prog->AddDecisionVariables(Vector1( - plane_geometries.separating_margin.value())); - } - VectorX positive_side_monomial_basis; - VectorX negative_side_monomial_basis; - auto add_geometry = - [&plane, &d_minus_Cs, &C_redundant_indices, &s_lower_redundant_indices, - &s_upper_redundant_indices, this]( + auto add_rationals_nonnegative = + [this, &plane, &d_minus_Cs, &C_redundant_indices, + &s_lower_redundant_indices, &s_upper_redundant_indices]( solvers::MathematicalProgram* prog, const std::vector& rationals, - multibody::BodyIndex body, - std::vector* search_plane_lagrangians) { - const VectorX monomial_basis = - this->map_body_to_monomial_basis_.at( - SortedPair(plane.expressed_body, body)); - const int num_grams = - rationals.size() * (1 + d_minus_Cs.rows() + - this->rational_forward_kin_.s().rows() * 2); - const int gram_size = - (monomial_basis.rows() + 1) * monomial_basis.rows() / 2; - const int num_gram_vars = gram_size * num_grams; - auto gram_vars = prog->NewContinuousVariables(num_gram_vars, "Gram"); - - int gram_var_count = 0; - search_plane_lagrangians->reserve(rationals.size()); - MatrixX gram(gram_size, gram_size); - for (int i = 0; i < static_cast(rationals.size()); ++i) { - search_plane_lagrangians->emplace_back( - d_minus_Cs.rows(), this->rational_forward_kin_.s().rows()); - auto& lagrangians = search_plane_lagrangians->back(); - for (int j = 0; j < d_minus_Cs.rows(); ++j) { - AddLagrangian(prog, monomial_basis, - gram_vars.segment(gram_var_count, gram_size), - C_redundant_indices.count(j) > 0, - &(lagrangians.polytope(j)), &gram); - gram_var_count += gram_size; - } - for (int j = 0; j < this->rational_forward_kin_.s().rows(); ++j) { - AddLagrangian(prog, monomial_basis, - gram_vars.segment(gram_var_count, gram_size), - s_lower_redundant_indices.count(j) > 0, - &(lagrangians.s_lower(j)), &gram); - gram_var_count += gram_size; - AddLagrangian(prog, monomial_basis, - gram_vars.segment(gram_var_count, gram_size), - s_upper_redundant_indices.count(j) > 0, - &(lagrangians.s_upper(j)), &gram); - gram_var_count += gram_size; + bool is_matrix_sos_condition, + const CollisionGeometry& collision_geometry, + std::vector* lagrangians_vec) { + if (!rationals.empty()) { + const SortedPair body_pair( + plane.expressed_body, collision_geometry.body_index()); + const auto& monomial_basis_array = + this->map_body_to_monomial_basis_array_.at(body_pair); + const int s_size = this->rational_forward_kin_.s().rows(); + const int num_lagrangians = + rationals.size() * (1 + d_minus_Cs.rows() + 2 * s_size); + const int y_size = + GetYSize(collision_geometry, is_matrix_sos_condition); + GramAndMonomialBasis gram_and_monomial_basis( + monomial_basis_array, this->with_cross_y_, y_size); + const int num_gram_vars_per_lagrangian = + gram_and_monomial_basis.gram_var_size; + const int num_gram_vars = + num_lagrangians * num_gram_vars_per_lagrangian; + auto gram_vars = prog->NewContinuousVariables(num_gram_vars, "Gram"); + + int gram_var_count = 0; + lagrangians_vec->reserve(rationals.size()); + + for (int i = 0; i < static_cast(rationals.size()); ++i) { + lagrangians_vec->emplace_back(d_minus_Cs.rows(), s_size); + auto& lagrangians = lagrangians_vec->back(); + // TODO(hongkai.dai): instead of adding the Lagrangian for + // redundant constraint and then setting these Lagrangians to zero, + // we could avoid allocating variables for these Lagrangians. + // + // Set lagrangians.polytope, add sos constraints. + for (int j = 0; j < d_minus_Cs.rows(); ++j) { + gram_and_monomial_basis.AddSos( + prog, + gram_vars.segment(gram_var_count, + num_gram_vars_per_lagrangian), + C_redundant_indices.count(j) > 0, &(lagrangians.polytope(j))); + gram_var_count += num_gram_vars_per_lagrangian; + } + // Set lagrangians.s_lower and lagrangians.s_upper, add sos + // constraints. + for (int j = 0; j < s_size; ++j) { + gram_and_monomial_basis.AddSos( + prog, + gram_vars.segment(gram_var_count, + num_gram_vars_per_lagrangian), + s_lower_redundant_indices.count(j), + &(lagrangians.s_lower(j))); + gram_var_count += num_gram_vars_per_lagrangian; + gram_and_monomial_basis.AddSos( + prog, + gram_vars.segment(gram_var_count, + num_gram_vars_per_lagrangian), + s_upper_redundant_indices.count(j), + &(lagrangians.s_upper(j))); + gram_var_count += num_gram_vars_per_lagrangian; + } + const symbolic::Polynomial poly = + rationals[i].numerator() - + lagrangians.polytope.dot(d_minus_Cs) - + lagrangians.s_lower.dot(this->s_minus_s_lower_) - + lagrangians.s_upper.dot(this->s_upper_minus_s_); + symbolic::Polynomial poly_sos; + gram_and_monomial_basis.AddSos( + prog, + gram_vars.segment(gram_var_count, num_gram_vars_per_lagrangian), + false /* is_zero_poly */, &poly_sos); + prog->AddEqualityConstraintBetweenPolynomials(poly, poly_sos); } - const symbolic::Polynomial poly = - rationals[i].numerator() - lagrangians.polytope.dot(d_minus_Cs) - - lagrangians.s_lower.dot(this->s_minus_s_lower_) - - lagrangians.s_upper.dot(this->s_upper_minus_s_); - // Add the constraint that poly is sos. - // Use the gram variable gram_vars that has been allocated. - symbolic::Polynomial poly_sos; - AddSosPolynomial(prog, monomial_basis, - gram_vars.segment(gram_var_count, gram_size), - &poly_sos, &gram); - gram_var_count += gram_size; - prog->AddEqualityConstraintBetweenPolynomials(poly, poly_sos); } - DRAKE_DEMAND(gram_var_count == num_gram_vars); }; - add_geometry(ret.prog.get(), plane_geometries.positive_side_rationals, - plane.positive_side_geometry->body_index(), - &ret.positive_side_lagrangians); - add_geometry(ret.prog.get(), plane_geometries.negative_side_rationals, - plane.negative_side_geometry->body_index(), - &ret.negative_side_lagrangians); - if (!plane_geometries.unit_length_vectors.empty()) { + + add_rationals_nonnegative( + ret.prog.get(), plane_geometries.positive_side_rationals, + false /* is_matrix_sos_condition */, *(plane.positive_side_geometry), + &ret.certificate.positive_side_rational_lagrangians); + add_rationals_nonnegative( + ret.prog.get(), plane_geometries.negative_side_rationals, + false /* is_matrix_sos_condition */, *(plane.negative_side_geometry), + &ret.certificate.negative_side_rational_lagrangians); + if (!plane_geometries.positive_side_psd_mat.empty() || + !plane_geometries.negative_side_psd_mat.empty()) { ret.prog->AddIndeterminates(y_slack_); - for (const auto& vec : plane_geometries.unit_length_vectors) { - ret.unit_length_lagrangians.push_back(AddUnitLengthConstraint( - ret.prog.get(), vec, d_minus_Cs, C_redundant_indices, - s_lower_redundant_indices, s_upper_redundant_indices, std::nullopt)); - } - } - if (plane_geometries.separating_margin.has_value()) { - // Add the constraint that margin >= 0 and maximize margin. - ret.prog->AddBoundingBoxConstraint( - 0, kInf, plane_geometries.separating_margin.value()); - ret.prog->AddLinearCost(-Vector1d(1), 0, - Vector1( - plane_geometries.separating_margin.value())); + add_rationals_nonnegative( + ret.prog.get(), plane_geometries.positive_side_psd_mat, + true /* is_matrix_sos_condition */, *(plane.positive_side_geometry), + &ret.certificate.positive_side_psd_mat_lagrangians); + add_rationals_nonnegative( + ret.prog.get(), plane_geometries.negative_side_psd_mat, + true /* is_matrix_sos_condition */, *(plane.negative_side_geometry), + &ret.certificate.negative_side_psd_mat_lagrangians); } + return ret; } -CspaceFreePolytope::UnitLengthLagrangians -CspaceFreePolytope::AddUnitLengthConstraint( - solvers::MathematicalProgram* prog, - const VectorX& unit_length_vec, - const VectorX& d_minus_Cs, - const std::unordered_set& C_redundant_indices, - const std::unordered_set& s_lower_redundant_indices, - const std::unordered_set& s_upper_redundant_indices, - const std::optional>& polytope_lagrangians) - const { - const int s_size = rational_forward_kin_.s().rows(); - CspaceFreePolytope::UnitLengthLagrangians ret(d_minus_Cs.rows(), s_size); - // Currently we only handle unit_length_vec.rows() == 3, and each polynomial - // in unit_length_vec has degree <= 1. - // TODO(hongkai.dai): handle unit_length_vec.rows() != 3. - if (unit_length_vec.rows() != 3) { - throw std::runtime_error( - "AddUnitLengthConstraint(): unit_length_vec should have 3 rows."); - } - // TODO(hongkai.dai): handle higher degree unit length vector. - for (int i = 0; i < unit_length_vec.rows(); ++i) { - // We set the Lagrangian multiplier degrees based on the assumption that - // unit_length_vec is an affine polynomial of s. - if (unit_length_vec(i).TotalDegree() > 1) { - throw std::runtime_error(fmt::format( - "AddUnitLengthConstraint(): unit_length_vec({}) has degree >1", i)); - } - } - // Compute p(y, s) = yᵀ * ⌈ 1 a(s)ᵀ⌉ * y - // ⌊a(s) I ⌋ - symbolic::Polynomial::MapType p_map; - for (int i = 0; i < 1 + unit_length_vec.rows(); ++i) { - p_map.emplace(symbolic::Monomial(y_slack_(i), 2), 1); - } - symbolic::Polynomial p{p_map}; - for (int i = 0; i < unit_length_vec.rows(); ++i) { - p += 2 * unit_length_vec(i) * - symbolic::Polynomial(symbolic::Monomial(y_slack_(i + 1))); - } - int num_grams = 1 + 2 * s_size; - if (!polytope_lagrangians.has_value()) { - num_grams += d_minus_Cs.rows(); - } - // Since p(y, s) is quadratic in y and linear in s, with the highest-degree - // monomial in the form yᵢ²sⱼ, we know that to make p(y, s) - λ₁(y, s)ᵀ(d−Cs) - // − λ₂(y, s)ᵀ(s−s_lower) − λ₃(y, s)ᵀ(s_upper−s) - ν₄(y, s)(1−yᵀy) being sos, - // we can set λ₁(y, s) λ₂(y, s) and λ₃(y, s) to be quadratic polynomials of y - // and ν₄(y, s) to be an affine polynomial of s. These Lagrangian multipliers - // degrees meet the minimum requirement to match the degree of p(y, s). - VectorX monomial_basis(2 + unit_length_vec.rows() + 1); - monomial_basis(0) = symbolic::Monomial(); - for (int i = 0; i < 1 + unit_length_vec.rows(); ++i) { - monomial_basis(1 + i) = symbolic::Monomial(y_slack_(i)); - } - const int gram_size = monomial_basis.rows() * (monomial_basis.rows() + 1) / 2; - const int num_gram_vars = gram_size * num_grams; - const VectorX gram_vars = - prog->NewContinuousVariables(num_gram_vars, "Gram"); - int gram_var_count = 0; - MatrixX gram(monomial_basis.rows(), - monomial_basis.rows()); - if (polytope_lagrangians.has_value()) { - DRAKE_DEMAND(polytope_lagrangians->rows() == d_minus_Cs.rows()); - ret.polytope = polytope_lagrangians.value(); - } else { - for (int i = 0; i < d_minus_Cs.rows(); ++i) { - AddLagrangian( - prog, monomial_basis, gram_vars.segment(gram_var_count, gram_size), - C_redundant_indices.count(i) > 0, &(ret.polytope(i)), &gram); - gram_var_count += gram_size; - } - } - for (int i = 0; i < this->s_minus_s_lower_.rows(); ++i) { - AddLagrangian( - prog, monomial_basis, gram_vars.segment(gram_var_count, gram_size), - s_lower_redundant_indices.count(i) > 0, &(ret.s_lower(i)), &gram); - gram_var_count += gram_size; - AddLagrangian( - prog, monomial_basis, gram_vars.segment(gram_var_count, gram_size), - s_upper_redundant_indices.count(i) > 0, &(ret.s_upper(i)), &gram); - gram_var_count += gram_size; - } - // This Lagrangian is just an affine polynomial of s, it is for the algebraic - // set {y | yᵀy=1}. - ret.y_square = prog->NewFreePolynomial(s_set_, 1); - // Compute polynomial 1 - yᵀy - symbolic::Polynomial::MapType one_minus_y_square_map; - one_minus_y_square_map.emplace(symbolic::Monomial(), 1); - for (int i = 0; i < 1 + unit_length_vec.rows(); ++i) { - one_minus_y_square_map.emplace(symbolic::Monomial(y_slack_(i), 2), -1); +CspaceFreePolytope::SeparationCertificateResult +CspaceFreePolytope::SeparationCertificate::GetSolution( + int plane_index, const Vector3& a, + const symbolic::Polynomial& b, + const VectorX& plane_decision_vars, + const solvers::MathematicalProgramResult& result) const { + CspaceFreePolytope::SeparationCertificateResult ret; + ret.plane_index = plane_index; + + auto set_lagrangians = + [&result]( + const std::vector& + lagrangians_vec, + std::vector* + lagrangians_result) { + lagrangians_result->reserve(lagrangians_vec.size()); + for (const auto& lagrangians : lagrangians_vec) { + lagrangians_result->push_back(lagrangians.GetSolution(result)); + } + }; + set_lagrangians(this->positive_side_rational_lagrangians, + &ret.positive_side_rational_lagrangians); + set_lagrangians(this->negative_side_rational_lagrangians, + &ret.negative_side_rational_lagrangians); + set_lagrangians(this->positive_side_psd_mat_lagrangians, + &ret.positive_side_psd_mat_lagrangians); + set_lagrangians(this->negative_side_psd_mat_lagrangians, + &ret.negative_side_psd_mat_lagrangians); + for (int i = 0; i < 3; ++i) { + ret.a(i) = result.GetSolution(a(i)); } - const symbolic::Polynomial one_minus_y_square{one_minus_y_square_map}; - const symbolic::Polynomial poly = p - ret.polytope.dot(d_minus_Cs) - - ret.s_lower.dot(this->s_minus_s_lower_) - - ret.s_upper.dot(this->s_upper_minus_s_) - - ret.y_square * one_minus_y_square; - // Now constrain poly to be sos. - // `poly` is affine in `s`, specifically it only contains monomials in the - // form of yᵢ²sⱼ, yᵢ², yᵢ, sⱼ, 1. Hence to make `poly` a sos polynomial, we - // only need to consider the monomial basis [1, y], and cancel out the terms - // that are linear in s. - SymmetricMatrixFromLowerTriangularPart( - monomial_basis.rows(), gram_vars.segment(gram_var_count, gram_size), - &gram); - prog->AddPositiveSemidefiniteConstraint(gram); - const symbolic::Polynomial poly_sos = - symbolic::CalcPolynomialWLowerTriangularPart( - monomial_basis, gram_vars.segment(gram_var_count, gram_size)); - gram_var_count += gram_size; - DRAKE_DEMAND(gram_var_count == num_gram_vars); - prog->AddEqualityConstraintBetweenPolynomials(poly, poly_sos); + ret.b = result.GetSolution(b); + ret.plane_decision_var_vals = result.GetSolution(plane_decision_vars); return ret; } @@ -812,7 +831,7 @@ std::vector> CspaceFreePolytope::FindSeparationCertificateGivenPolytope( const IgnoredCollisionPairs& ignored_collision_pairs, const Eigen::Ref& C, - const Eigen::Ref& d, bool search_separating_margin, + const Eigen::Ref& d, const FindSeparationCertificateGivenPolytopeOptions& options) const { const VectorX d_minus_Cs = this->CalcDminusCs(C, d); std::unordered_set C_redundant_indices; @@ -844,33 +863,30 @@ CspaceFreePolytope::FindSeparationCertificateGivenPolytope( // This lambda function formulates and solves a small SOS program for each // pair of geometries. - auto solve_small_sos = - [this, &d_minus_Cs, &C_redundant_indices, &s_lower_redundant_indices, - &s_upper_redundant_indices, &active_plane_indices, - search_separating_margin, &options, &is_success, &ret](int plane_count) { - const int plane_index = active_plane_indices[plane_count]; - const auto& plane_geometries = - search_separating_margin - ? this->plane_geometries_w_margin_[plane_index] - : this->plane_geometries_wo_margin_[plane_index]; - auto certificate = this->ConstructPlaneSearchProgram( - plane_geometries, d_minus_Cs, C_redundant_indices, - s_lower_redundant_indices, s_upper_redundant_indices); - const auto result = - SolveWithBackoff(certificate.prog.get(), options.backoff_scale, - options.solver_options, options.solver_id); - if (result.is_success()) { - ret[plane_count].emplace(GetSolution( - plane_index, certificate, plane_geometries.separating_margin, - separating_planes_[plane_index].a, - separating_planes_[plane_index].b, result)); - is_success[plane_count].emplace(true); - } else { - ret[plane_count].reset(); - is_success[plane_count].emplace(false); - } - return plane_count; - }; + auto solve_small_sos = [this, &d_minus_Cs, &C_redundant_indices, + &s_lower_redundant_indices, + &s_upper_redundant_indices, &active_plane_indices, + &options, &is_success, &ret](int plane_count) { + const int plane_index = active_plane_indices[plane_count]; + auto certificate_program = this->ConstructPlaneSearchProgram( + this->plane_geometries_[plane_index], d_minus_Cs, C_redundant_indices, + s_lower_redundant_indices, s_upper_redundant_indices); + solvers::MathematicalProgramResult result; + solvers::MakeSolver(options.solver_id) + ->Solve(*certificate_program.prog, std::nullopt, options.solver_options, + &result); + if (result.is_success()) { + ret[plane_count].emplace(certificate_program.certificate.GetSolution( + plane_index, separating_planes_[plane_index].a, + separating_planes_[plane_index].b, + separating_planes_[plane_index].decision_variables, result)); + is_success[plane_count].emplace(true); + } else { + ret[plane_count].reset(); + is_success[plane_count].emplace(false); + } + return plane_count; + }; const int num_threads = options.num_threads > 0 @@ -895,9 +911,11 @@ CspaceFreePolytope::FindSeparationCertificateGivenPolytope( // This call to future.get() is necessary to propagate any exception // thrown during SOS setup/solve. const int plane_count = operation->get(); - drake::log()->debug("SOS {}/{} completed, is_success {}", plane_count, - active_plane_indices.size(), - is_success[plane_count].value()); + if (options.verbose) { + drake::log()->info("SOS {}/{} completed, is_success {}", plane_count, + active_plane_indices.size(), + is_success[plane_count].value()); + } if (!(is_success[plane_count].value()) && options.terminate_at_failure) { stop_dispatching = true; @@ -916,8 +934,10 @@ CspaceFreePolytope::FindSeparationCertificateGivenPolytope( !stop_dispatching) { active_operations.emplace_back(std::async( std::launch::async, std::move(solve_small_sos), sos_dispatched)); - drake::log()->debug("SOS {}/{} dispatched", sos_dispatched, - active_plane_indices.size()); + if (options.verbose) { + drake::log()->info("SOS {}/{} dispatched", sos_dispatched, + active_plane_indices.size()); + } ++sos_dispatched; } @@ -963,7 +983,6 @@ bool CspaceFreePolytope::FindSeparationCertificateGivenPolytope( const Eigen::Ref& C, const Eigen::Ref& d, const IgnoredCollisionPairs& ignored_collision_pairs, - bool search_separating_margin, const CspaceFreePolytope::FindSeparationCertificateGivenPolytopeOptions& options, std::unordered_map, @@ -979,7 +998,7 @@ bool CspaceFreePolytope::FindSeparationCertificateGivenPolytope( &s_lower_redundant_indices, &s_upper_redundant_indices); const auto certificates_vec = this->FindSeparationCertificateGivenPolytope( - ignored_collision_pairs, C, d, search_separating_margin, options); + ignored_collision_pairs, C, d, options); certificates->clear(); bool is_success = true; @@ -1002,32 +1021,45 @@ int CspaceFreePolytope::GetGramVarSizeForPolytopeSearchProgram( bool search_s_bounds_lagrangians) const { int ret = 0; + // Count the gram size for rational >= 0 in C-space polytope. auto count_gram_per_plane_side = [this, search_s_bounds_lagrangians, &ret]( int num_rationals, multibody::BodyIndex expressed_body, - multibody::BodyIndex collision_body) { - const auto& monomial_basis = this->map_body_to_monomial_basis_.at( - SortedPair(expressed_body, collision_body)); - // Each rational will add Lagrangian multipliers for s-s_lower and - // s_upper-s (if search_s_bounds_lagrangian=true), together with one sos - // that rational.numerator() - λ(s)ᵀ * (d - C*s) - λ_lower(s)ᵀ * (s - - // s_lower) -λ_upper(s)ᵀ * (s_upper - s) is sos + multibody::BodyIndex collision_body, + int num_y) { + const auto& monomial_basis_array = + this->map_body_to_monomial_basis_array_.at( + SortedPair(expressed_body, collision_body)); const int s_size = this->rational_forward_kin_.s().rows(); + // Each rational will add Lagrangian multipliers for s-s_lower and + // s_upper-s (if search_s_bounds_lagrangian=true), together with one + // sos that rational.numerator() - λ(s)ᵀ * (d - C*s) - λ_lower(s)ᵀ * + // (s - s_lower) -λ_upper(s)ᵀ * (s_upper - s) is sos const int num_sos = num_rationals * (1 + (search_s_bounds_lagrangians ? 2 * s_size : 0)); - ret += num_sos * (monomial_basis.rows() + 1) * monomial_basis.rows() / 2; + ret += num_sos * + GetGramVarSize(monomial_basis_array, this->with_cross_y_, num_y); }; - for (const auto& plane_geometries : plane_geometries_wo_margin_) { + + for (const auto& plane_geometries : plane_geometries_) { const auto& plane = separating_planes_[plane_geometries.plane_index]; if (ignored_collision_pairs.count(SortedPair( plane.positive_side_geometry->id(), plane.negative_side_geometry->id())) == 0) { - count_gram_per_plane_side(plane_geometries.positive_side_rationals.size(), + count_gram_per_plane_side( + plane_geometries.positive_side_rationals.size(), plane.expressed_body, + plane.positive_side_geometry->body_index(), 0 /* y_size */); + count_gram_per_plane_side( + plane_geometries.negative_side_rationals.size(), plane.expressed_body, + plane.negative_side_geometry->body_index(), 0 /* y_size */); + count_gram_per_plane_side(plane_geometries.positive_side_psd_mat.size(), plane.expressed_body, - plane.positive_side_geometry->body_index()); - count_gram_per_plane_side(plane_geometries.negative_side_rationals.size(), + plane.positive_side_geometry->body_index(), + plane.positive_side_geometry->y_slack_size()); + count_gram_per_plane_side(plane_geometries.negative_side_psd_mat.size(), plane.expressed_body, - plane.negative_side_geometry->body_index()); + plane.negative_side_geometry->body_index(), + plane.negative_side_geometry->y_slack_size()); } } return ret; @@ -1040,14 +1072,16 @@ CspaceFreePolytope::InitializePolytopeSearchProgram( const VectorX& d_minus_Cs, const std::vector>& certificates_vec, - bool search_s_bounds_lagrangians, int gram_total_size) const { + bool search_s_bounds_lagrangians, int gram_total_size, + std::unordered_map* new_certificates_map) + const { auto prog = std::make_unique(); prog->AddIndeterminates(rational_forward_kin_.s()); - // Add the indeterminates y if we need to impose unit length constraint. - if (std::any_of(plane_geometries_wo_margin_.begin(), - plane_geometries_wo_margin_.end(), + // Add the indeterminates y if we need to impose matrix sos constraints. + if (std::any_of(plane_geometries_.begin(), plane_geometries_.end(), [](const PlaneSeparatesGeometries& plane_geometries) { - return !plane_geometries.unit_length_vectors.empty(); + return !plane_geometries.positive_side_psd_mat.empty() || + !plane_geometries.negative_side_psd_mat.empty(); })) { prog->AddIndeterminates(y_slack_); } @@ -1064,59 +1098,8 @@ CspaceFreePolytope::InitializePolytopeSearchProgram( for (int i = 0; i < static_cast(certificates_vec.size()); ++i) { plane_to_certificate_map.emplace(certificates_vec[i]->plane_index, i); } - int gram_var_count = 0; const int s_size = rational_forward_kin_.s().rows(); - // Allocate memory for the Lagrangians for s-s_lower and s_upper-s. - VectorX s_upper_lagrangians(s_size); - VectorX s_lower_lagrangians(s_size); - // Add the sos constraints for each pair of geometries. - auto add_sos = [this, &prog, &d_minus_Cs, &gram_vars, &gram_var_count, - &s_upper_lagrangians, &s_lower_lagrangians, - search_s_bounds_lagrangians]( - const std::vector& rationals, - int plane_index, multibody::BodyIndex collision_body, - const std::vector& - plane_side_lagrangians) { - DRAKE_DEMAND(plane_side_lagrangians.size() == rationals.size()); - const VectorX monomial_basis = - this->map_body_to_monomial_basis_.at(SortedPair( - this->separating_planes_[plane_index].expressed_body, - collision_body)); - const int gram_lower_size = - (monomial_basis.rows() + 1) * monomial_basis.rows() / 2; - MatrixX gram_mat(monomial_basis.rows(), - monomial_basis.rows()); - for (int i = 0; i < static_cast(rationals.size()); ++i) { - // Add Lagrangian multipliers for joint limits. - if (search_s_bounds_lagrangians) { - for (int j = 0; j < this->rational_forward_kin_.s().rows(); ++j) { - AddSosPolynomial(prog.get(), monomial_basis, - gram_vars.segment(gram_var_count, gram_lower_size), - &(s_lower_lagrangians(j)), &gram_mat); - gram_var_count += gram_lower_size; - AddSosPolynomial(prog.get(), monomial_basis, - gram_vars.segment(gram_var_count, gram_lower_size), - &(s_upper_lagrangians(j)), &gram_mat); - gram_var_count += gram_lower_size; - } - } else { - s_lower_lagrangians = plane_side_lagrangians[i].s_lower; - s_upper_lagrangians = plane_side_lagrangians[i].s_upper; - } - const symbolic::Polynomial poly = - rationals[i].numerator() - - plane_side_lagrangians[i].polytope.dot(d_minus_Cs) - - s_lower_lagrangians.dot(this->s_minus_s_lower_) - - s_upper_lagrangians.dot(this->s_upper_minus_s_); - // Constrain poly to be sos. - symbolic::Polynomial poly_sos; - AddSosPolynomial(prog.get(), monomial_basis, - gram_vars.segment(gram_var_count, gram_lower_size), - &poly_sos, &gram_mat); - gram_var_count += gram_lower_size; - prog->AddEqualityConstraintBetweenPolynomials(poly, poly_sos); - } - }; + int gram_var_count = 0; for (int plane_index = 0; plane_index < static_cast(separating_planes_.size()); ++plane_index) { @@ -1128,26 +1111,117 @@ CspaceFreePolytope::InitializePolytopeSearchProgram( const auto& certificate = certificates_vec[plane_to_certificate_map.at(plane_index)]; DRAKE_DEMAND(certificate.has_value()); - add_sos(plane_geometries_wo_margin_[plane_index].positive_side_rationals, - plane_index, plane.positive_side_geometry->body_index(), - certificate->positive_side_lagrangians); - add_sos(plane_geometries_wo_margin_[plane_index].negative_side_rationals, - plane_index, plane.negative_side_geometry->body_index(), - certificate->negative_side_lagrangians); - // Add the unit length constraint. - for (int i = 0; - i < static_cast(plane_geometries_wo_margin_[plane_index] - .unit_length_vectors.size()); - ++i) { - const auto& unit_length_vec = - plane_geometries_wo_margin_[plane_index].unit_length_vectors[i]; - this->AddUnitLengthConstraint( - prog.get(), unit_length_vec, d_minus_Cs, - std::unordered_set{} /* C_redundant_indices */, - std::unordered_set{} /* s_lower_redundant_indices */, - std::unordered_set{} /* s_upper_redundant_indices */, - certificate->unit_length_lagrangians[i].polytope); + DRAKE_DEMAND(certificate->plane_index == plane_index); + SeparationCertificate* new_certificate = nullptr; + if (new_certificates_map != nullptr) { + auto insertion_pair = + new_certificates_map->emplace(plane_index, SeparationCertificate()); + new_certificate = &(insertion_pair.first->second); } + const multibody::BodyIndex expressed_body = plane.expressed_body; + + auto add_rationals_nonnegative_given_lagrangians = + [this, &prog, &d_minus_Cs, &gram_vars, s_size, + search_s_bounds_lagrangians, &expressed_body, &gram_var_count]( + const std::vector& rationals, + bool is_matrix_sos_condition, + const CollisionGeometry& collision_geometry, + const std::vector& lagrangians_vec, + std::vector* new_lagrangians_vec) { + DRAKE_DEMAND(lagrangians_vec.size() == rationals.size()); + if (!rationals.empty()) { + const auto& monomial_basis_array = + this->map_body_to_monomial_basis_array_.at( + SortedPair( + expressed_body, collision_geometry.body_index())); + const int num_y = + GetYSize(collision_geometry, is_matrix_sos_condition); + const int num_gram_vars_per_lagrangian = GetGramVarSize( + monomial_basis_array, this->with_cross_y_, num_y); + GramAndMonomialBasis gram_and_monomial_basis( + monomial_basis_array, this->with_cross_y_, num_y); + VectorX s_lower_lagrangians(s_size); + VectorX s_upper_lagrangians(s_size); + // Add Lagrangian multipliers for joint limits. + for (int i = 0; i < static_cast(rationals.size()); ++i) { + if (search_s_bounds_lagrangians) { + for (int j = 0; j < s_size; ++j) { + gram_and_monomial_basis.AddSos( + prog.get(), + gram_vars.segment(gram_var_count, + num_gram_vars_per_lagrangian), + false /* is_zero_poly */, &(s_lower_lagrangians(j))); + gram_var_count += num_gram_vars_per_lagrangian; + gram_and_monomial_basis.AddSos( + prog.get(), + gram_vars.segment(gram_var_count, + num_gram_vars_per_lagrangian), + false /* is_zero_poly */, &(s_upper_lagrangians(j))); + gram_var_count += num_gram_vars_per_lagrangian; + } + } else { + s_lower_lagrangians = lagrangians_vec[i].s_lower; + s_upper_lagrangians = lagrangians_vec[i].s_upper; + } + + if (new_lagrangians_vec != nullptr) { + new_lagrangians_vec->emplace_back(d_minus_Cs.rows(), s_size); + (*new_lagrangians_vec)[i].polytope = + lagrangians_vec[i].polytope; + (*new_lagrangians_vec)[i].s_lower = s_lower_lagrangians; + (*new_lagrangians_vec)[i].s_upper = s_upper_lagrangians; + } + + const symbolic::Polynomial poly = + rationals[i].numerator() - + lagrangians_vec[i].polytope.dot(d_minus_Cs) - + s_lower_lagrangians.dot(this->s_minus_s_lower_) - + s_upper_lagrangians.dot(this->s_upper_minus_s_); + symbolic::Polynomial poly_sos; + gram_and_monomial_basis.AddSos( + prog.get(), + gram_vars.segment(gram_var_count, + num_gram_vars_per_lagrangian), + false /* is_zero_poly */, &poly_sos); + gram_var_count += num_gram_vars_per_lagrangian; + prog->AddEqualityConstraintBetweenPolynomials(poly, poly_sos); + } + } + }; + + // Add the constraint that positive_side_rationals are nonnegative in + // C-space polytope. + add_rationals_nonnegative_given_lagrangians( + plane_geometries_[plane_index].positive_side_rationals, + false /* is_matrix_sos_condition */, *(plane.positive_side_geometry), + certificate->positive_side_rational_lagrangians, + new_certificate == nullptr + ? nullptr + : &(new_certificate->positive_side_rational_lagrangians)); + // Add the constraint that negative_side_rationals are nonnegative in + // C-space polytope. + add_rationals_nonnegative_given_lagrangians( + plane_geometries_[plane_index].negative_side_rationals, + false /* is_matrix_sos_condition */, *(plane.negative_side_geometry), + certificate->negative_side_rational_lagrangians, + new_certificate == nullptr + ? nullptr + : &(new_certificate->negative_side_rational_lagrangians)); + + add_rationals_nonnegative_given_lagrangians( + plane_geometries_[plane_index].positive_side_psd_mat, + true /* is_matrix_sos_condition */, *(plane.positive_side_geometry), + certificate->positive_side_psd_mat_lagrangians, + new_certificate == nullptr + ? nullptr + : &(new_certificate->positive_side_psd_mat_lagrangians)); + add_rationals_nonnegative_given_lagrangians( + plane_geometries_[plane_index].negative_side_psd_mat, + true /* is_matrix_sos_condition */, *(plane.negative_side_geometry), + certificate->negative_side_psd_mat_lagrangians, + new_certificate == nullptr + ? nullptr + : &(new_certificate->negative_side_psd_mat_lagrangians)); } } DRAKE_DEMAND(gram_var_count == gram_total_size); @@ -1213,8 +1287,8 @@ void CspaceFreePolytope::AddCspacePolytopeContainment( } } } - // We have the constraint C.row(i).dot(s_inner_pts.col(j)) <= d(i) for all i, - // j. We can write this as s_inner_ptsᵀ * C.row(i)ᵀ <= [d(i);...;d(i)] We + // We have the constraint C.row(i).dot(s_inner_pts.col(j)) <= d(i) for all + // i, j. We can write this as s_inner_ptsᵀ * C.row(i)ᵀ <= [d(i);...;d(i)] We // repeat this constraint for each row and concantenate it into the matrix // form blockdiag(s_inner_ptsᵀ, ..., s_inner_ptsᵀ) * [C.row(0)ᵀ; // C.row(1)ᵀ;...;C.row(n-1)] - blockdiag(𝟏, 𝟏, ..., 𝟏) * d <= 0 @@ -1244,21 +1318,25 @@ CspaceFreePolytope::FindPolytopeGivenLagrangian( certificates_vec, const Eigen::MatrixXd& Q, const Eigen::VectorXd& s0, const VectorX& ellipsoid_margins, int gram_total_size, - const FindPolytopeGivenLagrangianOptions& options) const { + const FindPolytopeGivenLagrangianOptions& options, + std::unordered_map* certificates_result) + const { + std::unordered_map new_certificates_map; auto prog = this->InitializePolytopeSearchProgram( ignored_collision_pairs, C, d, d_minus_Cs, certificates_vec, - options.search_s_bounds_lagrangians, gram_total_size); + options.search_s_bounds_lagrangians, gram_total_size, + certificates_result == nullptr ? nullptr : &new_certificates_map); prog->AddDecisionVariables(ellipsoid_margins); AddEllipsoidContainmentConstraint(prog.get(), Q, s0, C, d, ellipsoid_margins); - // We know that the verified polytope has to be contained in the box s_lower - // <= s <= s_upper. Hence there is no point to grow the polytope such that any - // of its halfspace C.row(i) * s <= d(i) contains the entire box s_lower <= s - // <= s_upper. Therefore an upper bound of the margin δ is the maximal - // distance from any vertices of the box s_lower <= s <= s_upper to the - // ellipsoid. Computing the distance from a point to the hyperellipsoid is - // non-trivial (there is not closed-form solution). Here we use an upper bound - // of this distance, which is the maximal distance between any two points - // within the box. + // We know that the verified polytope has to be contained in the box + // s_lower <= s <= s_upper. Hence there is no point to grow the polytope such + // that any of its halfspace C.row(i) * s <= d(i) contains the entire box + // s_lower <= s <= s_upper. Therefore an upper bound of the margin δ is the + // maximal distance from any vertices of the box s_lower <= s <= s_upper to + // the ellipsoid. Computing the distance from a point to the hyperellipsoid is + // non-trivial (there is not closed-form solution). Here we use an upper + // bound of this distance, which is the maximal distance between any two + // points within the box. const double margin_upper_bound = (s_upper_ - s_lower_).norm(); prog->AddBoundingBoxConstraint(0, margin_upper_bound, ellipsoid_margins); if (options.s_inner_pts.has_value()) { @@ -1272,13 +1350,22 @@ CspaceFreePolytope::FindPolytopeGivenLagrangian( AddCspacePolytopeContainment(prog.get(), C, d, options.s_inner_pts.value()); } - // Maximize ∏ᵢ (δᵢ + ε) - prog->AddMaximizeGeometricMeanCost( - Eigen::MatrixXd::Identity(ellipsoid_margins.rows(), - ellipsoid_margins.rows()), - Eigen::VectorXd::Constant(ellipsoid_margins.rows(), - options.ellipsoid_margin_epsilon), - ellipsoid_margins); + switch (options.ellipsoid_margin_cost) { + case CspaceFreePolytope::EllipsoidMarginCost::kSum: + // Maximize ∑ᵢ δᵢ + prog->AddLinearCost(-Eigen::VectorXd::Ones(ellipsoid_margins.rows()), 0, + ellipsoid_margins); + break; + case CspaceFreePolytope::EllipsoidMarginCost::kGeometricMean: + // Maximize ∏ᵢ (δᵢ + ε) + prog->AddMaximizeGeometricMeanCost( + Eigen::MatrixXd::Identity(ellipsoid_margins.rows(), + ellipsoid_margins.rows()), + Eigen::VectorXd::Constant(ellipsoid_margins.rows(), + options.ellipsoid_margin_epsilon), + ellipsoid_margins); + break; + } const solvers::MathematicalProgramResult result = SolveWithBackoff(prog.get(), options.backoff_scale, @@ -1304,6 +1391,27 @@ CspaceFreePolytope::FindPolytopeGivenLagrangian( ret.ellipsoid_margins = result.GetSolution(ellipsoid_margins); } } + + if (certificates_result != nullptr) { + certificates_result->clear(); + for (int plane_index = 0; + plane_index < static_cast(separating_planes_.size()); + ++plane_index) { + const auto& plane = separating_planes_[plane_index]; + if (ignored_collision_pairs.count(SortedPair( + plane.positive_side_geometry->id(), + plane.negative_side_geometry->id())) == 0) { + certificates_result->emplace( + plane_index, + new_certificates_map.at(plane_index) + .GetSolution( + plane_index, separating_planes_[plane_index].a, + separating_planes_[plane_index].b, + separating_planes_[plane_index].decision_variables, + result)); + } + } + } return ret; } else { return std::nullopt; @@ -1335,16 +1443,15 @@ void CspaceFreePolytope::SearchResult::SetSeparatingPlanes( } } -std::optional +std::vector CspaceFreePolytope::SearchWithBilinearAlternation( const IgnoredCollisionPairs& ignored_collision_pairs, const Eigen::Ref& C_init, - const Eigen::Ref& d_init, bool search_margin, + const Eigen::Ref& d_init, const BilinearAlternationOptions& options) const { DRAKE_DEMAND(C_init.rows() == d_init.rows()); DRAKE_DEMAND(C_init.cols() == this->rational_forward_kin_.s().rows()); - std::optional ret{ - std::nullopt}; + std::vector ret{}; int iter = 0; // When we search for the C-space polytope {s |C*s<=d, s_lower<=s<=s_upper}, // we will require that each row of C has length <= 1. Hence to start with a @@ -1376,17 +1483,18 @@ CspaceFreePolytope::SearchWithBilinearAlternation( ignored_collision_pairs, options.find_polytope_options.search_s_bounds_lagrangians); // Find the inscribed ellipsoid. - HPolyhedron cspace_polytope = - this->GetPolyhedronWithJointLimits(C_init, d_init); + HPolyhedron cspace_polytope = this->GetPolyhedronWithJointLimits(C, d); Hyperellipsoid ellipsoid = cspace_polytope.MaximumVolumeInscribedEllipsoid(); - Eigen::MatrixXd ellipsoid_Q = ellipsoid.A().inverse(); + DRAKE_DEMAND(options.ellipsoid_scaling > 0); + DRAKE_DEMAND(options.ellipsoid_scaling <= 1); + Eigen::MatrixXd ellipsoid_Q = + options.ellipsoid_scaling * (ellipsoid.A().inverse()); double prev_cost = ellipsoid_Q.determinant(); drake::log()->info("det(Q) at the beginning is {}", prev_cost); while (iter < options.max_iter) { const std::vector> certificates_result = this->FindSeparationCertificateGivenPolytope( - ignored_collision_pairs, C, d, search_margin, - options.find_lagrangian_options); + ignored_collision_pairs, C, d, options.find_lagrangian_options); if (std::any_of( certificates_result.begin(), certificates_result.end(), [](const std::optional& certificate) { @@ -1398,32 +1506,32 @@ CspaceFreePolytope::SearchWithBilinearAlternation( iter); break; } else { - if (!ret.has_value()) { - ret.emplace(); - } - // Copy the result to ret. - ret->C = C; - ret->d = d; - ret->SetSeparatingPlanes(certificates_result); + ret.emplace_back(); + ret.back().C = C; + ret.back().d = d; + ret.back().num_iter = iter; + ret.back().SetSeparatingPlanes(certificates_result); } // Now fix the Lagrangian and search for C-space polytope and separating // planes. const auto polytope_result = this->FindPolytopeGivenLagrangian( ignored_collision_pairs, C_var, d_var, d_minus_Cs, certificates_result, ellipsoid_Q, ellipsoid.center(), ellipsoid_margins, - gram_total_size_in_polytope_program, options.find_polytope_options); + gram_total_size_in_polytope_program, options.find_polytope_options, + nullptr /* certificates_result */); if (polytope_result.has_value()) { C = polytope_result->C; d = polytope_result->d; - ret->C = polytope_result->C; - ret->d = polytope_result->d; - ret->a = std::move(polytope_result->a); - ret->b = std::move(polytope_result->b); - ret->num_iter = iter; + ret.back().C = polytope_result->C; + ret.back().d = polytope_result->d; + ret.back().a = std::move(polytope_result->a); + ret.back().b = std::move(polytope_result->b); + ret.back().num_iter = iter; // Now find the inscribed ellipsoid. - cspace_polytope = this->GetPolyhedronWithJointLimits(ret->C, ret->d); + cspace_polytope = + this->GetPolyhedronWithJointLimits(ret.back().C, ret.back().d); ellipsoid = cspace_polytope.MaximumVolumeInscribedEllipsoid(); - ellipsoid_Q = ellipsoid.A().inverse(); + ellipsoid_Q = options.ellipsoid_scaling * (ellipsoid.A().inverse()); const double cost = ellipsoid_Q.determinant(); drake::log()->info("Iteration {}: det(Q)={}", iter, cost); if (cost - prev_cost < options.convergence_tol) { @@ -1436,6 +1544,7 @@ CspaceFreePolytope::SearchWithBilinearAlternation( "Cannot find the separation certificate at iteration {} given the " "Lagrangians.", iter); + break; } ++iter; } @@ -1463,8 +1572,8 @@ CspaceFreePolytope::BinarySearch( } // Determines if we can certify the scaled C-space polytope {s |C*s<=d, - // s_lower<=s<=s_upper} is collision free or not. Also update `ret` if eps is - // feasible. + // s_lower<=s<=s_upper} is collision free or not. Also update `ret` if eps + // is feasible. auto is_scale_feasible = [this, &ignored_collision_pairs, &C, &d_init, &s_center, &options, &ret](double scale) { // (d - C*s_center) / |C| = scale * (d_init - C*s_center) / |C|, hence d = @@ -1472,8 +1581,7 @@ CspaceFreePolytope::BinarySearch( const Eigen::VectorXd d = scale * d_init + (1 - scale) * C * s_center; const auto certificates_result = this->FindSeparationCertificateGivenPolytope( - ignored_collision_pairs, C, d, false /* search_separating_margin */, - options.find_lagrangian_options); + ignored_collision_pairs, C, d, options.find_lagrangian_options); if (std::any_of( certificates_result.begin(), certificates_result.end(), [](const std::optional& certificate) { @@ -1495,7 +1603,7 @@ CspaceFreePolytope::BinarySearch( return std::nullopt; } if (is_scale_feasible(options.scale_max)) { - drake::log()->error( + drake::log()->info( "CspaceFreePolytope::BinarySearch(): scale_max={} is feasible.", options.scale_max); ret.num_iter = 0; diff --git a/geometry/optimization/dev/cspace_free_polytope.h b/geometry/optimization/dev/cspace_free_polytope.h index 75cd410f81e3..e641adf901f7 100644 --- a/geometry/optimization/dev/cspace_free_polytope.h +++ b/geometry/optimization/dev/cspace_free_polytope.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -25,24 +26,28 @@ namespace optimization { Contains the information to enforce a pair of geometries are separated by a plane. The conditions are that certain rational functions should be always positive, - and a certain vector has length <= 1. + and certain polynomials (coming from matrix-sos conditions) should always be + positive. */ struct PlaneSeparatesGeometries { PlaneSeparatesGeometries( std::vector m_positive_side_rationals, std::vector m_negative_side_rationals, - std::vector> m_unit_length_vectors, - int m_plane_index, std::optional m_separating_margin) + std::vector m_positive_side_psd_mat, + std::vector m_negative_side_psd_mat, + int m_plane_index) : positive_side_rationals{std::move(m_positive_side_rationals)}, negative_side_rationals{std::move(m_negative_side_rationals)}, - unit_length_vectors{std::move(m_unit_length_vectors)}, - plane_index{m_plane_index}, - separating_margin{std::move(m_separating_margin)} {} + positive_side_psd_mat(std::move(m_positive_side_psd_mat)), + negative_side_psd_mat(std::move(m_negative_side_psd_mat)), + plane_index{m_plane_index} {} + // TODO(hongkai.dai): only save positive_side_rationals and + // negative_side_rationals. const std::vector positive_side_rationals; const std::vector negative_side_rationals; - const std::vector> unit_length_vectors; + const std::vector positive_side_psd_mat; + const std::vector negative_side_psd_mat; int plane_index; - std::optional separating_margin; }; /** @@ -65,9 +70,39 @@ class CspaceFreePolytope { /** Optional argument for constructing CspaceFreePolytope */ struct Options { Options() {} - /** Refer to CollisionGeometry::polytope_chebyshev_radius_multiplier(). - */ - double polytope_chebyshev_radius_multiplier{1E-3}; + + // For non-polytopic collision geometries, we will impose a matrix-sos + // constraint X(s) being psd, with a slack indeterminates y, such that the + // polynomial + // p(s, y) = ⌈ 1 ⌉ᵀ * X(s) * ⌈ 1 ⌉ + // ⌊ y ⌋ ⌊ y ⌋ + // is positive. This p(s, y) polynomial doesn't contain the cross term of y + // (namely it doesn't have y(i)*y(j), i≠j). When we select the monomial + // basis for this polynomial, we can also exclude the cross term of y in the + // monomial basis. + // + // To illustrate the idea, let's consider the following toy example: if we + // want to certify that + // a(0) + a(1)*y₀ + a(2)*y₁ + a(3)*y₀² + a(4)*y₁² is positive + // (this polynomial doesn't have the cross term y₀*y₁), we can write it as + // ⌈ 1⌉ᵀ * A₀ * ⌈ 1⌉ + ⌈ 1⌉ᵀ * A₁ * ⌈ 1⌉ + // ⌊y₀⌋ ⌊y₀⌋ ⌊y₁⌋ ⌊y₁⌋ + // with two small psd matrices A₀, A₁ + // Instead of + // ⌈ 1⌉ᵀ * A * ⌈ 1⌉ + // |y₀| |y₀| + // ⌊y₁⌋ ⌊y₁⌋ + // with one large psd matrix A. The first parameterization won't have the + // cross term y₀*y₁ by construction, while the second parameterization + // requires imposing extra constraints on certain off-diagonal terms in A + // so that the cross term vanishes. + // + // If we set with_cross_y = false, then we will use the monomial basis that + // doesn't generate cross terms of y, leading to smaller size sos problems. + // If we set with_cross_y = true, then we will use the monomial basis that + // will generate cross terms of y, causing larger size sos problems, but + // possibly able to certify a larger C-space polytope. + bool with_cross_y{false}; }; /** @@ -105,7 +140,7 @@ class CspaceFreePolytope { return separating_planes_; } - [[nodiscard]] const VectorX& y_slack() const { + [[nodiscard]] const Vector3& y_slack() const { return y_slack_; } @@ -131,37 +166,24 @@ class CspaceFreePolytope { }; /** - When searching for the separating plane, sometimes we want to certify that a - vector of polynomials has length <= 1 in the C-space region {s | C*s<=d, - s_lower<=s<=s_upper}. To this end, we will impose a matrix-sos constraint, - that certain polynomial p(y, s) >=0 in the polytope, namely - p(y, s) - λ₁(y, s)ᵀ(d−Cs) − λ₂(y, s)ᵀ(s−s_lower) − λ₃(y, s)ᵀ(s_upper−s) - - ν₄(y, s)(1−yᵀy) is sos. - λ₁(y, s), λ₂(y, s), λ₃(y, s) are all sos. - ν₄(y, s) is a free polynomial. - Notice that we add the extra restriction yᵀy=1 through the term λ₄(y, - s)(1−yᵀy). This restriction makes the semialgebraic set of y being compact, - enabling us using stronger Positivstellensatz theorem. For more explanation - on this term, refer to the documentation in the private function - AddUnitLengthConstraint. - - This struct records the Lagrangian multipliers λ₁(y, - s), λ₂(y, s), λ₃(y, s) and ν₄(y, s). + We certify that a pair of geometries is collision free in the C-space region + {s | Cs<=d, s_lower<=s<=s_upper}, by finding the separating plane and the + Lagrangian multipliers. This struct contains the certificate, that the + separating plane {x | aᵀx+b=0 } separates the two geometries in + separating_planes()[plane_index] in the C-space polytope. */ - struct UnitLengthLagrangians { - UnitLengthLagrangians(int C_rows, int s_size) - : polytope(C_rows), s_lower(s_size), s_upper(s_size) {} - // λ₁(y, s) in the documentation. - VectorX polytope; - // λ₂(y, s) in the documentation. - VectorX s_lower; - // λ₃(y, s) in the documentation. - VectorX s_upper; - // ν₄(y, s) in the documentation. - symbolic::Polynomial y_square; - - [[nodiscard]] UnitLengthLagrangians GetSolution( - const solvers::MathematicalProgramResult& result) const; + struct SeparationCertificateResult { + int plane_index; + std::vector positive_side_rational_lagrangians; + std::vector negative_side_rational_lagrangians; + std::vector positive_side_psd_mat_lagrangians; + std::vector negative_side_psd_mat_lagrangians; + // The separating plane is { x | aᵀx+b=0 } + Vector3 a; + symbolic::Polynomial b; + // The value of the plane.decision_variables at solution. This field is used + // for debugging. + Eigen::VectorXd plane_decision_var_vals; }; /** @@ -176,46 +198,43 @@ class CspaceFreePolytope { λ(s) are sos, λ_lower(s) are sos, λ_upper(s) are sos. */ struct SeparationCertificate { - SeparationCertificate() : prog{new solvers::MathematicalProgram()} {} + SeparationCertificate() {} + + [[nodiscard]] SeparationCertificateResult GetSolution( + int plane_index, const Vector3& a, + const symbolic::Polynomial& b, + const VectorX& plane_decision_vars, + const solvers::MathematicalProgramResult& result) const; + + // positive_side_rational_lagrangians[i] is the Lagrangian multipliers for + // PlaneSeparatesGeometries::positive_side_rationals[i]. + std::vector positive_side_rational_lagrangians; + // negative_side_rational_lagrangians[i] is the Lagrangian multipliers for + // PlaneSeparatesGeometries::negative_side_rationals[i]. + std::vector negative_side_rational_lagrangians; + + // positive_side_psd_mat_lagrangians[i] is the Lagrangian multipliers for + // PlaneSeparatesGeometries::positive_side_psd_mat[i]. + std::vector positive_side_psd_mat_lagrangians; + // negative_side_psd_mat_lagrangians[i] is the Lagrangian multipliers for + // PlaneSeparatesGeometries::negative_side_psd_mat[i]. + std::vector negative_side_psd_mat_lagrangians; + }; + struct SeparationCertificateProgram { + SeparationCertificateProgram() + : prog{new solvers::MathematicalProgram()}, certificate{} {} /// The program that stores all the constraints to search for the separating /// plane and Lagrangian multipliers as certificate. std::unique_ptr prog; - /// positive_side_lagrangians[i] is the Lagrangian multipliers for - /// PlaneSeparatesGeometries::positive_side_rationals[i]. - std::vector positive_side_lagrangians; - /// negative_side_lagrangians[i] is the Lagrangian multipliers for - /// PlaneSeparatesGeometries::negative_side_rationals[i]. - std::vector negative_side_lagrangians; - - // unit_length_lagrangians[i] is the Lagrangian multipliers for - // PlaneSeparatesGeometries::unit_length_vectors[i]. - std::vector unit_length_lagrangians; - }; - - /** - We certify that a pair of geometries is collision free in the C-space region - {s | Cs<=d, s_lower<=s<=s_upper}, by finding the separating plane and the - Lagrangian multipliers. This struct contains the certificate, that the - separating plane {x | aᵀx+b=0 } separates the two geometries in - separating_planes()[plane_index] in the C-space polytope. - */ - struct SeparationCertificateResult { - int plane_index; - std::vector positive_side_lagrangians; - std::vector negative_side_lagrangians; - std::vector unit_length_lagrangians; - std::optional separating_margin; - // The separating plane is { x | aᵀx+b=0 } - Vector3 a; - symbolic::Polynomial b; + SeparationCertificate certificate; }; struct FindSeparationCertificateGivenPolytopeOptions { // We can find the certificate for each pair of geometries in parallel. // num_threads specifies how many threads we run in parallel. If num_threads // <=0, then we use all available threads on the computer. - int num_threads{1}; + int num_threads{-1}; // If verbose set to true, then we will print some information to the // terminal. @@ -227,16 +246,6 @@ class CspaceFreePolytope { // If the SOS in one thread fails, then don't launch any more threads. bool terminate_at_failure{true}; - // If backoff_scale is not empty, then after solving the problem with an - // optimal cost, we "back-off" the program to solve a feasibility program. - // Namely we remove the cost and add a constraint cost(x) <= - // cost_upper_bound, where cost_upper_bound = (1 + backoff_scale) * - // optimal_cost if optimal_cost >= 0, and cost_upper_bound = (1 - - // backoff_scale) * optimal_cost if optimal_cost <= 0. This back-off - // technique is useful in bilinear alternation to ensure solutions in the - // strict interior of the set. - std::optional backoff_scale{std::nullopt}; - // The solver options used for the SOS program. std::optional solver_options{std::nullopt}; @@ -253,8 +262,6 @@ class CspaceFreePolytope { * @param d The C-space polytope is {s | C*s<=d, s_lower<=s<=s_upper} * @param ignored_collision_pairs We will ignore the pair of geometries in * `ignored_collision_pairs`. - * @param search_separating_margin If set to true, we will attempt to maximize - * the separating margin between each pair of geometries. * @param[out] certificates Contains the certificate we successfully found for * each pair of geometries. Notice that depending on `options`, the program * could search for the certificate for each geometry pair in parallel, and @@ -268,7 +275,6 @@ class CspaceFreePolytope { const Eigen::Ref& C, const Eigen::Ref& d, const IgnoredCollisionPairs& ignored_collision_pairs, - bool search_separating_margin, const FindSeparationCertificateGivenPolytopeOptions& options, std::unordered_map, SeparationCertificateResult>* certificates) const; @@ -282,6 +288,18 @@ class CspaceFreePolytope { const VectorX& d, const Eigen::MatrixXd& s_inner_pts) const; + /** + The cost used when fixing the Lagrangian multiplier and search for C and d in + the C-space polytope {s | C*s <=d, s_lower<=s<=s_upper}. We denote δᵢ as the + margin between the i'th face C.row(i)<=d(i) to the inscribed ellipsoid. + */ + enum EllipsoidMarginCost { + kSum, ///< Maximize ∑ᵢδᵢ + kGeometricMean, ///< Maximize the geometric mean power(∏ᵢ (δᵢ + ε), 1/n) + ///< where n is C.rows(), + ///< ε=FindPolytopeGivenLagrangianOptions.ellipsoid_margin_epsilon. # NOLINT + }; + struct FindPolytopeGivenLagrangianOptions { std::optional backoff_scale{std::nullopt}; @@ -304,6 +322,9 @@ class CspaceFreePolytope { // Lagrangian multiplier to the solution found when we fix the C-space // polytope {s | C*s<=d, s_lower<=s<=s_upper}. bool search_s_bounds_lagrangians{true}; + + EllipsoidMarginCost ellipsoid_margin_cost{ + EllipsoidMarginCost::kGeometricMean}; }; struct SearchResult { @@ -325,6 +346,12 @@ class CspaceFreePolytope { double convergence_tol{1E-3}; FindPolytopeGivenLagrangianOptions find_polytope_options; FindSeparationCertificateGivenPolytopeOptions find_lagrangian_options; + /** After finding the maximal inscribed ellipsoid in C-space polytope {s | + * C*s<=d, s_lower<=s<=s_upper}, we scale this ellipsoid by + * ellipsoid_scaling, and require the new C-space polytope to contain this + * scaled ellipsoid. ellipsoid_scaling=1 corresponds to no scaling. + */ + double ellipsoid_scaling{0.99}; }; /** Search for a collision-free C-space polytope. @@ -336,19 +363,14 @@ class CspaceFreePolytope { searching for separation certificates. @param C_init The initial value of C. @param d_init The initial value of d. - @param search_margin If set to true, then we also maximize the - margin between the separation plane and the separated geometries, when we fix - the polytope {s | C*s<=d, s_lower<=s<=s_upper} and search for the separating - planes and Lagrangian multipliers. @param options The options for the bilinear alternation. - @retval result If we successfully find a collision-free C-space polytope, - then `result` contains the search result; otherwise result = std::nullopt. + @retval results Stores the certification result in each iteration of the + bilinear alternation. */ - [[nodiscard]] std::optional - SearchWithBilinearAlternation( + [[nodiscard]] std::vector SearchWithBilinearAlternation( const IgnoredCollisionPairs& ignored_collision_pairs, const Eigen::Ref& C_init, - const Eigen::Ref& d_init, bool search_margin, + const Eigen::Ref& d_init, const BilinearAlternationOptions& options) const; struct BinarySearchOptions { @@ -397,13 +419,8 @@ class CspaceFreePolytope { Generate all the conditions (certain rationals being non-negative, and certain vectors with length <= 1) such that the robot configuration is collision free. - @param filtered_collision_pairs - @param search_separating_margin If set to true, then when we search for the - separating planes, we will attempt to maximize the margin of the separating - planes. */ - [[nodiscard]] std::vector GenerateRationals( - bool search_separating_margin) const; + void GenerateRationals(); /** Computes the monomial basis for each pair of bodies. @@ -432,52 +449,13 @@ class CspaceFreePolytope { @param[in] s_upper_redundant_indices. Store the indices of the redundant rows in s <= s_upper. */ - [[nodiscard]] SeparationCertificate ConstructPlaneSearchProgram( + [[nodiscard]] SeparationCertificateProgram ConstructPlaneSearchProgram( const PlaneSeparatesGeometries& plane_geometries, const VectorX& d_minus_Cs, const std::unordered_set& C_redundant_indices, const std::unordered_set& s_lower_redundant_indices, const std::unordered_set& s_upper_redundant_indices) const; - // Impose the condition that |a(s)| <= 1 in the polytope - // {s | C*s <= d, s_lower <= s <= s_upper}. where a(s) is a vector of - // polynomials. This can be imposed using matrix-sos condition - // ⌈ 1 a(s)ᵀ⌉ is psd in the polytope. - // ⌊a(s) I ⌋ - // Namely the polynomial of (y, s) - // yᵀ * ⌈ 1 a(s)ᵀ⌉ * y >= 0 in the polytope. - // ⌊a(s) I ⌋ - // If we denote p(y, s) = yᵀ * ⌈ 1 a(s)ᵀ⌉ * y - // ⌊a(s) I ⌋ - // We want p(y, s) >= 0 on the polytope. - // - // Note that this polytope is NOT a compact set on y (it is a compact set on - // s). We wish to make this set compact so that we can use a stronger - // Positivstellensatz. To do so, we notice that p(y, s) is homogeneous in y, - // hence p(y, s) >= 0 on the polytope if and only if we further restrain y to - // be on the unit circle. Namely we consider the condition p(y, s) >= 0 if (y, - // s) ∈ {s | C*s<=d, s_lower<=s<=s_upper} ∩ {y | yᵀy=1} - // - // Then we impose the condition - // p(y, s) - λ₁(y, s)ᵀ(d−Cs) − λ₂(y, s)ᵀ(s−s_lower) − λ₃(y, s)ᵀ(s_upper−s) - - // ν₄(y, s)(1−yᵀy) is sos. - // λ₁(y, s), λ₂(y, s), λ₃(y, s) are all sos. - // ν₄(y, s) is a free polynomial. - // - // @param polytope_lagrangians If this is not std::nullopt, then we set λ₁(y, - // s) to be polytope_lagrangians, and don't need to impose the constraint - // λ₁(y, s) is sos. We use this optional polytope_lagrangians when we search - // for C and d, keeping the polytope Lagrangians fixed. - UnitLengthLagrangians AddUnitLengthConstraint( - solvers::MathematicalProgram* prog, - const VectorX& unit_length_vec, - const VectorX& d_minus_Cs, - const std::unordered_set& C_redundant_indices, - const std::unordered_set& s_lower_redundant_indices, - const std::unordered_set& s_upper_redundant_indices, - const std::optional>& polytope_lagrangians) - const; - /** For each pair of geometries, find the certificate that the pair is collision free in the C-space region {s | C*s<=d, s_lower<=s<=s_upper}. @@ -497,7 +475,7 @@ class CspaceFreePolytope { FindSeparationCertificateGivenPolytope( const IgnoredCollisionPairs& ignored_collision_pairs, const Eigen::Ref& C, - const Eigen::Ref& d, bool search_separating_margin, + const Eigen::Ref& d, const FindSeparationCertificateGivenPolytopeOptions& options) const; /** When we fix the Lagrangian multipliers and search for the C-space polytope @@ -520,6 +498,9 @@ class CspaceFreePolytope { Lagrangian multiplier for the bounds s_lower <=s<=s_upper. @param gram_total_size The return of GetGramVarSizeForPolytopeSearchProgram(). + @param[out] new_certificates The new certificates to certify the new C-space + polytope {s | C*s<=d, s_lower<=s<=s_upper} is collision free. If + new_certificates=nullptr, then we don't update it. This is used for testing. */ [[nodiscard]] std::unique_ptr InitializePolytopeSearchProgram( @@ -529,7 +510,9 @@ class CspaceFreePolytope { const VectorX& d_minus_Cs, const std::vector>& certificates_vec, - bool search_s_bounds_lagrangians, int gram_total_size) const; + bool search_s_bounds_lagrangians, int gram_total_size, + std::unordered_map* new_certificates = + nullptr) const; /** Adds the constraint that the ellipsoid {Q*u+s₀ | uᵀu≤1} is inside the polytope {s | C*s <= d} with margin δ. Namely for the i'th face cᵢᵀs≤dᵢ, we @@ -551,6 +534,11 @@ class CspaceFreePolytope { Eigen::VectorXd ellipsoid_margins; }; + /** + @param[out] certificates_result If certificates_result=nullptr, then we don't + update its value; otherwise we set it to map the plane index to the + separation certificates result for the plane. + */ [[nodiscard]] std::optional FindPolytopeGivenLagrangian( const IgnoredCollisionPairs& ignored_collision_pairs, @@ -561,7 +549,9 @@ class CspaceFreePolytope { certificates_vec, const Eigen::MatrixXd& Q, const Eigen::VectorXd& s0, const VectorX& ellipsoid_margins, int gram_total_size, - const FindPolytopeGivenLagrangianOptions& options) const; + const FindPolytopeGivenLagrangianOptions& options, + std::unordered_map* certificates_result) + const; /** Gets the H-polyhedron {s | C*s<=d, s_lower<=s<=s_upper}. */ HPolyhedron GetPolyhedronWithJointLimits(const Eigen::MatrixXd& C, @@ -581,7 +571,7 @@ class CspaceFreePolytope { // Sometimes we need to impose that a certain matrix of polynomials are always // psd (for example with sphere or capsule collision geometries). We will use // this slack variable to help us impose the matrix-sos constraint. - VectorX y_slack_; + Vector3 y_slack_; symbolic::Variables s_set_; @@ -590,12 +580,20 @@ class CspaceFreePolytope { Eigen::VectorXd s_upper_; VectorX s_minus_s_lower_; VectorX s_upper_minus_s_; - std::vector plane_geometries_w_margin_; - std::vector plane_geometries_wo_margin_; - + // We have the invariant plane_geometries_[i].plane_index == i. + std::vector plane_geometries_; + + // 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] std::unordered_map, - VectorX> - map_body_to_monomial_basis_; + std::array, 4>> + map_body_to_monomial_basis_array_; + + // See Options::with_cross_y for its meaning. + bool with_cross_y_; }; /** @@ -606,7 +604,6 @@ class CspaceFreePolytope { std::vector>> GetCollisionGeometries(const multibody::MultibodyPlant& plant, const geometry::SceneGraph& scene_graph); - } // namespace optimization } // namespace geometry } // namespace drake diff --git a/geometry/optimization/dev/test/c_iris_test_utilities.cc b/geometry/optimization/dev/test/c_iris_test_utilities.cc index 295bc8b20a3e..00ebf8f7bcde 100644 --- a/geometry/optimization/dev/test/c_iris_test_utilities.cc +++ b/geometry/optimization/dev/test/c_iris_test_utilities.cc @@ -129,6 +129,105 @@ CIrisToyRobotTest::CIrisToyRobotTest() { plant_->Finalize(); diagram_ = builder.Build(); } + +CIrisRobotPolytopicGeometryTest::CIrisRobotPolytopicGeometryTest() { + systems::DiagramBuilder builder; + plant_ = builder.AddSystem>(0.); + scene_graph_ = builder.AddSystem>(); + plant_->RegisterAsSourceForSceneGraph(scene_graph_); + + builder.Connect(scene_graph_->get_query_output_port(), + plant_->get_geometry_query_input_port()); + builder.Connect( + plant_->get_geometry_poses_output_port(), + scene_graph_->get_source_pose_port(plant_->get_source_id().value())); + + ProximityProperties proximity_properties{}; + // C-IRIS doesn't care about robot dynamics. Use arbitrary material + // properties. + AddContactMaterial(0.1, 250.0, multibody::CoulombFriction{0.9, 0.5}, + &proximity_properties); + + world_boxes_.push_back(plant_->RegisterCollisionGeometry( + plant_->world_body(), + math::RigidTransform(math::RollPitchYawd(0.5, 0.2, -0.3), + Eigen::Vector3d(0.2, -0.5, 0.1)), + geometry::Box(0.02, 0.03, 0.01), "world_box0", proximity_properties)); + world_boxes_.push_back(plant_->RegisterCollisionGeometry( + plant_->world_body(), + math::RigidTransform(math::RollPitchYawd(0.1, 0.2, -0.), + Eigen::Vector3d(0.2, 0.3, 0.1)), + geometry::Box(0.02, 0.1, 0.05), "world_box1", proximity_properties)); + world_boxes_.push_back(plant_->RegisterCollisionGeometry( + plant_->world_body(), + math::RigidTransform(math::RollPitchYawd(0.1, 0.2, -0.), + Eigen::Vector3d(0.2, 0.2, 0.1)), + geometry::Box(0.04, 0.1, 0.05), "world_box2", proximity_properties)); + const std::string convex_obj = + FindResourceOrThrow("drake/geometry/optimization/dev/test/convex.obj"); + world_convex_ = plant_->RegisterCollisionGeometry( + plant_->world_body(), + math::RigidTransform(Eigen::Vector3d(-0.1, -0.5, 0.2)), + Convex(convex_obj), "world_convex", proximity_properties); + + // C-IRIS only considers robot kinematics, not dynamics. So we use an + // arbitrary inertia. + const multibody::SpatialInertia spatial_inertia( + 1, Eigen::Vector3d::Zero(), + multibody::UnitInertia(0.01, 0.01, 0.01, 0, 0, 0)); + + auto add_body = [this, &spatial_inertia, &proximity_properties]( + const math::RigidTransformd& X_PF, + const math::RigidTransformd& X_BM, + const Eigen::Vector3d& axis, double theta_lower, + double theta_upper, const math::RigidTransformd& X_BG, + const Eigen::Vector3d& box_size) { + const int body_index = this->body_indices_.size(); + const auto& parent_body = + this->body_indices_.empty() + ? this->plant_->world_body() + : this->plant_->get_body(this->body_indices_.back()); + this->body_indices_.push_back( + this->plant_ + ->AddRigidBody("body" + std::to_string(body_index), spatial_inertia) + .index()); + + const auto& body = this->plant_->get_body(this->body_indices_.back()); + const auto& joint = this->plant_->AddJoint( + "joint" + std::to_string(body_index), parent_body, X_PF, body, X_BM, + axis); + plant_->get_mutable_joint(joint.index()) + .set_position_limits(Vector1d(theta_lower), Vector1d(theta_upper)); + this->body_boxes_.push_back(this->plant_->RegisterCollisionGeometry( + body, X_BG, Box(box_size(0), box_size(1), box_size(2)), + "body" + std::to_string(body_index) + "_box", proximity_properties)); + }; + + // body0 + add_body(math::RigidTransformd(Eigen::Vector3d(0.1, 0.2, 0)), + math::RigidTransformd(Eigen::Vector3d(0.05, 0.01, 0)), + Eigen::Vector3d::UnitX(), -0.5 * M_PI, 0.8 * M_PI, + math::RigidTransformd(Eigen::Vector3d(0.05, 0.1, 0)), + Eigen::Vector3d(0.1, 0.15, 0.1)); + // body1 + add_body(math::RigidTransformd(Eigen::Vector3d(0.05, 0, 0.1)), + math::RigidTransformd(), Eigen::Vector3d::UnitY(), -0.6 * M_PI, + 0.7 * M_PI, math::RigidTransformd(Eigen::Vector3d(0.02, 0, 0.05)), + Eigen::Vector3d(0.04, 0.05, 0.08)); + // body2 + add_body(math::RigidTransformd(Eigen::Vector3d(0., 0.2, 0.1)), + math::RigidTransformd(), Eigen::Vector3d::UnitX(), -0.3 * M_PI, + 0.7 * M_PI, math::RigidTransformd(Eigen::Vector3d(0.0, 0.1, 0.05)), + Eigen::Vector3d(0.04, 0.1, 0.08)); + // body3 + add_body(math::RigidTransformd(Eigen::Vector3d(0.1, 0.2, -0.1)), + math::RigidTransformd(), Eigen::Vector3d::UnitZ(), -0.5 * M_PI, + 0.7 * M_PI, math::RigidTransformd(Eigen::Vector3d(0.0, 0.1, 0.04)), + Eigen::Vector3d(0.05, 0.1, 0.02)); + + plant_->Finalize(); + diagram_ = builder.Build(); +} } // namespace optimization } // namespace geometry } // namespace drake diff --git a/geometry/optimization/dev/test/c_iris_test_utilities.h b/geometry/optimization/dev/test/c_iris_test_utilities.h index 13a47062c38b..ea6c27712098 100644 --- a/geometry/optimization/dev/test/c_iris_test_utilities.h +++ b/geometry/optimization/dev/test/c_iris_test_utilities.h @@ -38,6 +38,23 @@ class CIrisToyRobotTest : public ::testing::Test { geometry::GeometryId body3_sphere_; }; +// Create a robot with only polytopic collision geometry. +// world - revolute - body0 - revolute - body1 - revolute - body2 - revolute - +// body3 +class CIrisRobotPolytopicGeometryTest : public ::testing::Test { + public: + CIrisRobotPolytopicGeometryTest(); + + protected: + std::unique_ptr> diagram_; + multibody::MultibodyPlant* plant_; + geometry::SceneGraph* scene_graph_; + std::vector body_indices_; + std::vector world_boxes_; + geometry::GeometryId world_convex_; + std::vector body_boxes_; +}; + } // namespace optimization } // namespace geometry } // namespace drake diff --git a/geometry/optimization/dev/test/collision_geometry_test.cc b/geometry/optimization/dev/test/collision_geometry_test.cc index 32d0aa5f730f..236628167eb4 100644 --- a/geometry/optimization/dev/test/collision_geometry_test.cc +++ b/geometry/optimization/dev/test/collision_geometry_test.cc @@ -14,6 +14,7 @@ #include "drake/math/rigid_transform.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/rational/rational_forward_kinematics.h" +#include "drake/multibody/rational/rational_forward_kinematics_internal.h" namespace drake { namespace geometry { @@ -53,6 +54,10 @@ class CollisionGeometryTest : public CIrisToyRobotTest { diagram_->GetMutableSubsystemContext(*plant_, diagram_context_.get())); scene_graph_context_ = &(diagram_->GetMutableSubsystemContext( *scene_graph_, diagram_context_.get())); + + for (int i = 0; i < 3; ++i) { + y_slack_(i) = symbolic::Variable("y" + std::to_string(i)); + } } Eigen::VectorXd SetQ(const Eigen::Vector3d& q_star, @@ -76,6 +81,7 @@ class CollisionGeometryTest : public CIrisToyRobotTest { systems::Context* scene_graph_context_; Eigen::Vector3d q_val_; + Vector3 y_slack_; }; void CheckRationalExpression(const symbolic::RationalFunction& rational, @@ -102,14 +108,17 @@ TEST_F(CollisionGeometryTest, Box) { rational_forward_kin_.CalcBodyPoseAsMultilinearPolynomial( q_star, geometry_body, expressed_body); - std::vector rationals; - std::optional> unit_length_vector; - // Positive side, separating_margin is empty. - box.OnPlaneSide(a_, b_, X_AB_multilinear, rational_forward_kin_, std::nullopt, - PlaneSide::kPositive, &rationals, &unit_length_vector); - EXPECT_FALSE(unit_length_vector.has_value()); - EXPECT_EQ(box.num_rationals_per_side(), 9); - EXPECT_EQ(rationals.size(), 9); + std::vector rationals_for_points; + std::vector rationals_for_matrix_sos; + // Positive side. + box.OnPlaneSide(a_, b_, X_AB_multilinear, rational_forward_kin_, + PlaneSide::kPositive, y_slack_, &rationals_for_points, + &rationals_for_matrix_sos); + EXPECT_EQ(box.num_rationals_for_points(), 8); + EXPECT_EQ(rationals_for_points.size(), 8); + EXPECT_TRUE(rationals_for_matrix_sos.empty()); + EXPECT_EQ(box.num_rationals_for_matrix_sos(), 0); + EXPECT_EQ(box.y_slack_size(), 0); // The order of the vertices should be the same in collision_geometry.cc Eigen::Matrix p_GV; @@ -122,17 +131,8 @@ TEST_F(CollisionGeometryTest, Box) { p_GV.row(0) *= box_geometry.width() / 2; p_GV.row(1) *= box_geometry.depth() / 2; p_GV.row(2) *= box_geometry.height() / 2; - // C is the Chebyshev center. The Chebyshev center of the box is not unique, - // we choose the one computed from HPolyhedron. - const HPolyhedron box_polyhedron{VPolytope(p_GV)}; - const Eigen::Vector3d p_GC = box_polyhedron.ChebyshevCenter(); - const double radius = - std::min(std::min(box_geometry.width(), box_geometry.depth()), - box_geometry.height()) / - 2; const Eigen::Matrix p_BV = box.X_BG() * p_GV; - const Eigen::Vector3d p_BC = box.X_BG() * p_GC; // Evaluate the rationals, and compare the evaluation result with a.dot(p_AV) // + b @@ -145,49 +145,30 @@ TEST_F(CollisionGeometryTest, Box) { plant_->CalcPointsPositions( *plant_context_, plant_->get_body(geometry_body).body_frame(), p_BV, plant_->get_body(expressed_body).body_frame(), &p_AV); - Eigen::Vector3d p_AC; - plant_->CalcPointsPositions( - *plant_context_, plant_->get_body(geometry_body).body_frame(), p_BC, - plant_->get_body(expressed_body).body_frame(), &p_AC); Vector3 a_expr; for (int j = 0; j < 3; ++j) { a_expr(j) = a_(j).EvaluatePartial(env).ToExpression(); } const symbolic::Expression b_expr = b_.EvaluatePartial(env).ToExpression(); for (int i = 0; i < 8; ++i) { - const symbolic::Expression expr_expected = a_expr.dot(p_AV.col(i)) + b_expr; - CheckRationalExpression(rationals[i], env, expr_expected); + const symbolic::Expression expr_expected = + a_expr.dot(p_AV.col(i)) + b_expr - 1; + CheckRationalExpression(rationals_for_points[i], env, expr_expected); } - CheckRationalExpression( - rationals.back(), env, - a_expr.dot(p_AC) + b_expr - - box.polytope_chebyshev_radius_multiplier() * radius); - - // Negative side, with separating margin. - rationals.clear(); - symbolic::Variable separating_margin{"delta"}; + + // Negative side. + rationals_for_points.clear(); box.OnPlaneSide(a_, b_, X_AB_multilinear, rational_forward_kin_, - separating_margin, PlaneSide::kNegative, &rationals, - &unit_length_vector); - EXPECT_TRUE(unit_length_vector.has_value()); - EXPECT_EQ(unit_length_vector->rows(), 3); - for (int i = 0; i < 3; ++i) { - EXPECT_EQ((*unit_length_vector)(i), a_(i)); - } - EXPECT_EQ(rationals.size(), 9); - EXPECT_EQ(box.num_rationals_per_side(), 9); + PlaneSide::kNegative, y_slack_, &rationals_for_points, + &rationals_for_matrix_sos); + EXPECT_EQ(rationals_for_points.size(), 8); + EXPECT_EQ(box.num_rationals_for_points(), 8); for (int i = 0; i < 8; ++i) { const symbolic::Expression expr_expected = - -separating_margin - a_expr.dot(p_AV.col(i)) - b_expr; - CheckRationalExpression(rationals[i], env, expr_expected); - EXPECT_TRUE(rationals[i].numerator().decision_variables().include( - separating_margin)); + -1 - a_expr.dot(p_AV.col(i)) - b_expr; + CheckRationalExpression(rationals_for_points[i], env, expr_expected); } - CheckRationalExpression(rationals.back(), env, - -box.polytope_chebyshev_radius_multiplier() * radius - - a_expr.dot(p_AC) - b_expr); - EXPECT_FALSE(rationals.back().numerator().decision_variables().include( - separating_margin)); + EXPECT_TRUE(rationals_for_matrix_sos.empty()); } TEST_F(CollisionGeometryTest, Convex) { @@ -198,6 +179,7 @@ TEST_F(CollisionGeometryTest, Convex) { geometry_body, body1_convex_, model_inspector.GetPoseInFrame(body1_convex_)); EXPECT_EQ(convex.type(), GeometryType::kPolytope); + EXPECT_EQ(convex.y_slack_size(), 0); const multibody::BodyIndex expressed_body = body_indices_[3]; const Eigen::Vector3d q_star(0, 0, 0); @@ -205,8 +187,8 @@ TEST_F(CollisionGeometryTest, Convex) { rational_forward_kin_.CalcBodyPoseAsMultilinearPolynomial( q_star, geometry_body, expressed_body); - std::vector rationals; - std::optional> unit_length_vector; + std::vector rationals_for_points; + std::vector rationals_for_matrix_sos; auto query_object = scene_graph_->get_query_output_port().Eval>( @@ -228,23 +210,15 @@ TEST_F(CollisionGeometryTest, Convex) { plant_->CalcPointsPositions( *plant_context_, plant_->get_body(geometry_body).body_frame(), p_BV, plant_->get_body(expressed_body).body_frame(), &p_AV); - const Eigen::Vector3d p_GC = h_poly.ChebyshevCenter(); - const Eigen::Vector3d p_BC = convex.X_BG() * p_GC; - Eigen::Vector3d p_AC; - plant_->CalcPointsPositions( - *plant_context_, plant_->get_body(geometry_body).body_frame(), p_BC, - plant_->get_body(expressed_body).body_frame(), &p_AC); - const double radius = ((h_poly.b() - h_poly.A() * p_GC).array() / - h_poly.A().rowwise().norm().array()) - .minCoeff(); - // negative side, no separating margin. + // negative side. convex.OnPlaneSide(a_, b_, X_AB_multilinear, rational_forward_kin_, - std::nullopt, PlaneSide::kNegative, &rationals, - &unit_length_vector); - EXPECT_FALSE(unit_length_vector.has_value()); - EXPECT_EQ(rationals.size(), polytope.vertices().cols() + 1); - EXPECT_EQ(convex.num_rationals_per_side(), polytope.vertices().cols() + 1); + PlaneSide::kNegative, y_slack_, &rationals_for_points, + &rationals_for_matrix_sos); + EXPECT_EQ(rationals_for_points.size(), polytope.vertices().cols()); + EXPECT_EQ(convex.num_rationals_for_points(), polytope.vertices().cols()); + EXPECT_TRUE(rationals_for_matrix_sos.empty()); + EXPECT_EQ(convex.num_rationals_for_matrix_sos(), 0); symbolic::Environment env; env.insert(rational_forward_kin_.s(), s_val); Vector3 a_expr; @@ -254,43 +228,56 @@ TEST_F(CollisionGeometryTest, Convex) { const symbolic::Expression b_expr = b_.EvaluatePartial(env).ToExpression(); for (int i = 0; i < polytope.vertices().cols(); ++i) { const symbolic::Expression expr_expected = - -a_expr.dot(p_AV.col(i)) - b_expr; - CheckRationalExpression(rationals[i], env, expr_expected); + -1 - a_expr.dot(p_AV.col(i)) - b_expr; + CheckRationalExpression(rationals_for_points[i], env, expr_expected); } - CheckRationalExpression( - rationals.back(), env, - -convex.polytope_chebyshev_radius_multiplier() * radius - - a_expr.dot(p_AC) - b_expr); - - // Positive side, with separating margin. - const symbolic::Variable separating_margin("delta"); - // Note that here I didn't clear rationals, so that I can test the new - // rationals are appended to the existing ones. + + // Positive side. + // Note that here I didn't clear rationals_for_points, so that I can test that + // the new rationals_for_points are appended to the existing ones. convex.OnPlaneSide(a_, b_, X_AB_multilinear, rational_forward_kin_, - separating_margin, PlaneSide::kPositive, &rationals, - &unit_length_vector); - EXPECT_TRUE(unit_length_vector.has_value()); - EXPECT_EQ(unit_length_vector->rows(), 3); - for (int i = 0; i < 3; ++i) { - EXPECT_EQ((*unit_length_vector)(i), a_(i)); - } - // The new rationals are appended to the existing ones. - EXPECT_EQ(rationals.size(), 2 * polytope.vertices().cols() + 2); - EXPECT_EQ(convex.num_rationals_per_side(), 1 + polytope.vertices().cols()); + PlaneSide::kPositive, y_slack_, &rationals_for_points, + &rationals_for_matrix_sos); + // The new rationals_for_points are appended to the existing ones. + EXPECT_EQ(rationals_for_points.size(), 2 * polytope.vertices().cols()); + EXPECT_EQ(convex.num_rationals_for_points(), polytope.vertices().cols()); for (int i = 0; i < polytope.vertices().cols(); ++i) { const symbolic::Expression expr_expected = - a_expr.dot(p_AV.col(i)) + b_expr - separating_margin; - CheckRationalExpression(rationals[i + polytope.vertices().cols() + 1], env, - expr_expected); - EXPECT_TRUE(rationals[i + polytope.vertices().cols() + 1] - .numerator() - .decision_variables() - .include(separating_margin)); + a_expr.dot(p_AV.col(i)) + b_expr - 1; + CheckRationalExpression( + rationals_for_points[i + polytope.vertices().cols()], env, + expr_expected); + } +} + +// Assume the rationals_for_matrix_sos is obtained by requiring +// lhs_expr >= r * |a| +// @param env contains the value of s. +// @param lhs_expr has been evaluated at env. +// @param a_expr has been evaluated at env. +void CheckPolynomialWSlack(const symbolic::RationalFunction& psd_mat_rational, + const symbolic::Environment& env, + const symbolic::Expression lhs_expr, + const Vector3& a_expr, + const VectorX& y, + double radius) { + for (int i = 0; i < y.rows(); ++i) { + EXPECT_TRUE(psd_mat_rational.numerator().indeterminates().include(y(i))); } - CheckRationalExpression( - rationals.back(), env, - a_expr.dot(p_AC) + b_expr - - convex.polytope_chebyshev_radius_multiplier() * radius); + + const double denominator_val = psd_mat_rational.denominator().Evaluate(env); + + const symbolic::Expression expr_expected = + (lhs_expr + 2 * a_expr.dot(y) + + lhs_expr / (radius * radius) * y.cast().dot(y)); + + EXPECT_PRED3( + symbolic::test::PolynomialEqual, + symbolic::Polynomial( + psd_mat_rational.numerator().EvaluatePartial(env).ToExpression()) + .Expand() / + denominator_val, + symbolic::Polynomial(expr_expected), 1E-10); } TEST_F(CollisionGeometryTest, Sphere) { @@ -300,7 +287,9 @@ TEST_F(CollisionGeometryTest, Sphere) { plant_->world_body().index(), world_sphere_, model_inspector.GetPoseInFrame(world_sphere_)); EXPECT_EQ(sphere.type(), GeometryType::kSphere); - EXPECT_EQ(sphere.num_rationals_per_side(), 1); + EXPECT_EQ(sphere.num_rationals_for_points(), 1); + EXPECT_EQ(sphere.num_rationals_for_matrix_sos(), 1); + EXPECT_EQ(sphere.y_slack_size(), 3); const Eigen::Vector3d q_star(0., 0., 0.); const multibody::BodyIndex expressed_body = body_indices_[3]; @@ -318,18 +307,13 @@ TEST_F(CollisionGeometryTest, Sphere) { } const symbolic::Expression b_expr = b_.EvaluatePartial(env).ToExpression(); - // Negative side, no margin. - std::vector rationals; - std::optional> unit_length_vector; + // Negative side. + std::vector rationals_for_points; + std::vector rationals_for_matrix_sos; sphere.OnPlaneSide(a_, b_, X_AB_multilinear, rational_forward_kin_, - std::nullopt, PlaneSide::kNegative, &rationals, - &unit_length_vector); - EXPECT_TRUE(unit_length_vector.has_value()); - EXPECT_EQ(unit_length_vector->rows(), 3); - for (int i = 0; i < 3; ++i) { - EXPECT_EQ((*unit_length_vector)(i), a_(i)); - } - EXPECT_EQ(rationals.size(), 1); + PlaneSide::kNegative, y_slack_, &rationals_for_points, + &rationals_for_matrix_sos); + EXPECT_EQ(rationals_for_points.size(), 1); const Eigen::Vector3d p_BS = sphere.X_BG().translation(); Eigen::Vector3d p_AS; plant_->CalcPointsPositions(*plant_context_, plant_->world_frame(), p_BS, @@ -338,19 +322,26 @@ TEST_F(CollisionGeometryTest, Sphere) { const double radius = static_cast(model_inspector.GetShape(world_sphere_)) .radius(); - symbolic::Expression expr_expected = -radius - a_expr.dot(p_AS) - b_expr; - CheckRationalExpression(rationals[0], env, expr_expected); + EXPECT_EQ(rationals_for_matrix_sos.size(), 1); + + CheckPolynomialWSlack(rationals_for_matrix_sos[0], env, + -a_expr.dot(p_AS) - b_expr, a_expr, y_slack_, radius); + CheckRationalExpression(rationals_for_points[0], env, + -1 - a_expr.dot(p_AS) - b_expr); - // Positive side, with margin. - rationals.clear(); - const symbolic::Variable separating_margin("delta"); + // Positive side. + rationals_for_points.clear(); sphere.OnPlaneSide(a_, b_, X_AB_multilinear, rational_forward_kin_, - separating_margin, PlaneSide::kPositive, &rationals, - &unit_length_vector); - EXPECT_TRUE(unit_length_vector.has_value()); - EXPECT_EQ(unit_length_vector->rows(), 3); - expr_expected = a_expr.dot(p_AS) + b_expr - radius - separating_margin; - CheckRationalExpression(rationals[0], env, expr_expected); + PlaneSide::kPositive, y_slack_, &rationals_for_points, + &rationals_for_matrix_sos); + EXPECT_EQ(rationals_for_points.size(), 1); + // We append a new polynomial to polynomials_w_slack, hence it should have + // size 2. + EXPECT_EQ(rationals_for_matrix_sos.size(), 2); + CheckPolynomialWSlack(rationals_for_matrix_sos.back(), env, + a_expr.dot(p_AS) + b_expr, a_expr, y_slack_, radius); + CheckRationalExpression(rationals_for_points[0], env, + a_expr.dot(p_AS) + b_expr - 1); } TEST_F(CollisionGeometryTest, Capsule) { @@ -361,7 +352,9 @@ TEST_F(CollisionGeometryTest, Capsule) { geometry_body, body2_capsule_, model_inspector.GetPoseInFrame(body2_capsule_)); EXPECT_EQ(capsule.type(), GeometryType::kCapsule); - EXPECT_EQ(capsule.num_rationals_per_side(), 2); + EXPECT_EQ(capsule.num_rationals_for_points(), 1); + EXPECT_EQ(capsule.num_rationals_for_matrix_sos(), 2); + EXPECT_EQ(capsule.y_slack_size(), 3); const Eigen::Vector3d q_star(0., 0., 0.); const multibody::BodyIndex expressed_body = body_indices_[0]; @@ -379,48 +372,53 @@ TEST_F(CollisionGeometryTest, Capsule) { } const symbolic::Expression b_expr = b_.EvaluatePartial(env).ToExpression(); - // Negative side, no margin. - std::vector rationals; - std::optional> unit_length_vector; + // Negative side + std::vector rationals_for_points; + std::vector rationals_for_matrix_sos; capsule.OnPlaneSide(a_, b_, X_AB_multilinear, rational_forward_kin_, - std::nullopt, PlaneSide::kNegative, &rationals, - &unit_length_vector); - EXPECT_TRUE(unit_length_vector.has_value()); - EXPECT_EQ(unit_length_vector->rows(), 3); - for (int i = 0; i < 3; ++i) { - EXPECT_EQ((*unit_length_vector)(i), a_(i)); - } - EXPECT_EQ(rationals.size(), 2); + PlaneSide::kNegative, y_slack_, &rationals_for_points, + &rationals_for_matrix_sos); + EXPECT_EQ(rationals_for_points.size(), 1); const Capsule& capsule_shape = static_cast(model_inspector.GetShape(body2_capsule_)); Eigen::Matrix p_GS; p_GS.col(0) << 0, 0, capsule_shape.length() / 2; p_GS.col(1) << 0, 0, -capsule_shape.length() / 2; + const Eigen::Vector3d p_GO(0, 0, 0); const Eigen::Matrix p_BS = capsule.X_BG() * p_GS; + const Eigen::Vector3d p_BO = capsule.X_BG() * p_GO; Eigen::Matrix p_AS; + Eigen::Vector3d p_AO; plant_->CalcPointsPositions( *plant_context_, plant_->get_body(geometry_body).body_frame(), p_BS, plant_->get_body(expressed_body).body_frame(), &p_AS); + plant_->CalcPointsPositions( + *plant_context_, plant_->get_body(geometry_body).body_frame(), p_BO, + plant_->get_body(expressed_body).body_frame(), &p_AO); + EXPECT_EQ(rationals_for_matrix_sos.size(), 2); for (int i = 0; i < 2; ++i) { - symbolic::Expression expr_expected = - -capsule_shape.radius() - a_expr.dot(p_AS.col(i)) - b_expr; - CheckRationalExpression(rationals[i], env, expr_expected); + CheckPolynomialWSlack(rationals_for_matrix_sos[i], env, + -a_expr.dot(p_AS.col(i)) - b_expr, a_expr, y_slack_, + capsule_shape.radius()); } + CheckRationalExpression(rationals_for_points[0], env, + -a_expr.dot(p_AO) - b_expr - 1); - // Positive side, with margin. - rationals.clear(); - const symbolic::Variable separating_margin("delta"); + // Positive side + rationals_for_points.clear(); + rationals_for_matrix_sos.clear(); capsule.OnPlaneSide(a_, b_, X_AB_multilinear, rational_forward_kin_, - separating_margin, PlaneSide::kPositive, &rationals, - &unit_length_vector); - EXPECT_TRUE(unit_length_vector.has_value()); - EXPECT_EQ(unit_length_vector->rows(), 3); + PlaneSide::kPositive, y_slack_, &rationals_for_points, + &rationals_for_matrix_sos); + EXPECT_EQ(rationals_for_matrix_sos.size(), 2); for (int i = 0; i < 2; ++i) { - const symbolic::Expression expr_expected = a_expr.dot(p_AS.col(i)) + - b_expr - capsule_shape.radius() - - separating_margin; - CheckRationalExpression(rationals[i], env, expr_expected); + CheckPolynomialWSlack(rationals_for_matrix_sos[i], env, + a_expr.dot(p_AS.col(i)) + b_expr, a_expr, y_slack_, + capsule_shape.radius()); } + EXPECT_EQ(rationals_for_points.size(), 1); + CheckRationalExpression(rationals_for_points[0], env, + a_expr.dot(p_AO) + b_expr - 1); } GTEST_TEST(DistanceToHalfspace, Test) { @@ -474,6 +472,16 @@ GTEST_TEST(DistanceToHalfspace, Test) { math::RigidTransformd(math::RollPitchYawd(0.02, -0.1, 0.05), Eigen::Vector3d(0.03, -0.01, 0.02)), HalfSpace(), "body3_halfspace", proximity_properties); + + const double cylinder_radius = 0.02; + const double cylinder_length = 0.2; + const geometry::GeometryId body4_cylinder = plant->RegisterCollisionGeometry( + plant->get_body(body_indices[4]), + math::RigidTransformd(math::RollPitchYawd(0.1, -0.2, 0.3), + Eigen::Vector3d(0.02, 0.1, -0.05)), + Cylinder(cylinder_radius, cylinder_length), "body4_cylinder", + proximity_properties); + plant->Finalize(); auto diagram = builder.Build(); @@ -575,6 +583,37 @@ GTEST_TEST(DistanceToHalfspace, Test) { const double distance_expected = (-p_HS.row(2)).minCoeff() - capsule_radius; EXPECT_NEAR(distance, distance_expected, kTol); } + + { + // Distance to cylinder + const CollisionGeometry cylinder( + &(model_inspector.GetShape(body4_cylinder)), body_indices[4], + body4_cylinder, model_inspector.GetPoseInFrame(body4_cylinder)); + const double distance = + DistanceToHalfspace(cylinder, a, b, expressed_body, + PlaneSide::kNegative, *plant, plant_context); + // SceneGraph doesn't support distance between cylinder and halfspace yet. + // Compute the cylinder in the halfspace frame. + Eigen::Matrix p_GS; + p_GS.col(0) << 0, 0, cylinder_length / 2; + p_GS.col(1) << 0, 0, -cylinder_length / 2; + Eigen::Matrix p_ES; + plant->CalcPointsPositions( + plant_context, plant->get_body(body_indices[4]).body_frame(), + model_inspector.GetPoseInFrame(body4_cylinder) * p_GS, + plant->get_body(expressed_body).body_frame(), &p_ES); + const Eigen::Matrix p_HS = X_HalfspaceE * p_ES; + + // axis_H is the unit length cylinder axis vector expressed in halfspace + // frame H. + const Eigen::Vector3d axis_H = + (p_HS.col(0) - p_HS.col(1)) / (cylinder_length); + const double distance_expected = + -std::max(p_HS(2, 0), p_HS(2, 1)) - + cylinder_radius * + std::sqrt(axis_H(0) * axis_H(0) + axis_H(1) * axis_H(1)); + EXPECT_NEAR(distance, distance_expected, 1E-10); + } } } // namespace optimization } // namespace geometry diff --git a/geometry/optimization/dev/test/cspace_free_polytope_test.cc b/geometry/optimization/dev/test/cspace_free_polytope_test.cc index 3d0f833cffeb..04c00c444286 100644 --- a/geometry/optimization/dev/test/cspace_free_polytope_test.cc +++ b/geometry/optimization/dev/test/cspace_free_polytope_test.cc @@ -12,8 +12,10 @@ #include "drake/geometry/optimization/dev/test/c_iris_test_utilities.h" #include "drake/multibody/rational/rational_forward_kinematics.h" #include "drake/multibody/rational/rational_forward_kinematics_internal.h" +#include "drake/solvers/common_solver_option.h" #include "drake/solvers/mosek_solver.h" #include "drake/solvers/solve.h" +#include "drake/solvers/sos_basis_generator.h" namespace drake { namespace geometry { @@ -29,9 +31,11 @@ class CspaceFreePolytopeTester { CspaceFreePolytopeTester(const multibody::MultibodyPlant* plant, const geometry::SceneGraph* scene_graph, SeparatingPlaneOrder plane_order, - const Eigen::Ref& q_star) - : cspace_free_polytope_{ - new CspaceFreePolytope(plant, scene_graph, plane_order, q_star)} {} + const Eigen::Ref& q_star, + const CspaceFreePolytope::Options& options = + CspaceFreePolytope::Options()) + : cspace_free_polytope_{new CspaceFreePolytope( + plant, scene_graph, plane_order, q_star, options)} {} const CspaceFreePolytope& cspace_free_polytope() const { return *cspace_free_polytope_; @@ -64,14 +68,8 @@ class CspaceFreePolytopeTester { return cspace_free_polytope_->s_upper_minus_s_; } - const std::vector& plane_geometries_w_margin() - const { - return cspace_free_polytope_->plane_geometries_w_margin_; - } - - const std::vector& plane_geometries_wo_margin() - const { - return cspace_free_polytope_->plane_geometries_wo_margin_; + const std::vector& plane_geometries() const { + return cspace_free_polytope_->plane_geometries_; } template @@ -81,13 +79,14 @@ class CspaceFreePolytopeTester { return cspace_free_polytope_->CalcDminusCs(C, d); } - [[nodiscard]] const std::unordered_map, - VectorX>& - map_body_to_monomial_basis() const { - return cspace_free_polytope_->map_body_to_monomial_basis_; + [[nodiscard]] const std::unordered_map< + SortedPair, + std::array, 4>>& + map_body_to_monomial_basis_array() const { + return cspace_free_polytope_->map_body_to_monomial_basis_array_; } - [[nodiscard]] CspaceFreePolytope::SeparationCertificate + [[nodiscard]] CspaceFreePolytope::SeparationCertificateProgram ConstructPlaneSearchProgram( const PlaneSeparatesGeometries& plane_geometries, const VectorX& d_minus_Cs, @@ -99,37 +98,16 @@ class CspaceFreePolytopeTester { s_lower_redundant_indices, s_upper_redundant_indices); } - [[nodiscard]] CspaceFreePolytope::UnitLengthLagrangians - AddUnitLengthConstraint( - solvers::MathematicalProgram* prog, - const VectorX& unit_length_vec, - const VectorX& d_minus_Cs, - const std::unordered_set& C_redundant_indices, - const std::unordered_set& s_lower_redundant_indices, - const std::unordered_set& s_upper_redundant_indices, - const std::optional>& polytope_lagrangians) - const { - return cspace_free_polytope_->AddUnitLengthConstraint( - prog, unit_length_vec, d_minus_Cs, C_redundant_indices, - s_lower_redundant_indices, s_upper_redundant_indices, - polytope_lagrangians); - } - [[nodiscard]] std::vector< std::optional> FindSeparationCertificateGivenPolytope( const CspaceFreePolytope::IgnoredCollisionPairs& ignored_collision_pairs, const Eigen::Ref& C, - const Eigen::Ref& d, bool search_separating_margin, + const Eigen::Ref& d, const CspaceFreePolytope::FindSeparationCertificateGivenPolytopeOptions& options) const { return cspace_free_polytope_->FindSeparationCertificateGivenPolytope( - ignored_collision_pairs, C, d, search_separating_margin, options); - } - - [[nodiscard]] std::vector GenerateRationals( - bool search_separating_margin) const { - return cspace_free_polytope_->GenerateRationals(search_separating_margin); + ignored_collision_pairs, C, d, options); } [[nodiscard]] int GetGramVarSizeForPolytopeSearchProgram( @@ -148,10 +126,12 @@ class CspaceFreePolytopeTester { const std::vector< std::optional>& certificates_vec, - bool search_s_bounds_lagrangians, int gram_total_size) const { + bool search_s_bounds_lagrangians, int gram_total_size, + std::unordered_map* + new_certificates) const { return cspace_free_polytope_->InitializePolytopeSearchProgram( ignored_collision_pairs, C, d, d_minus_Cs, certificates_vec, - search_s_bounds_lagrangians, gram_total_size); + search_s_bounds_lagrangians, gram_total_size, new_certificates); } void AddEllipsoidContainmentConstraint( @@ -170,6 +150,7 @@ class CspaceFreePolytopeTester { cspace_free_polytope_->AddCspacePolytopeContainment(prog, C, d, s_inner_pts); } + [[nodiscard]] std::optional< CspaceFreePolytope::FindPolytopeGivenLagrangianResult> FindPolytopeGivenLagrangian( @@ -182,11 +163,12 @@ class CspaceFreePolytopeTester { certificates_vec, const Eigen::MatrixXd& Q, const Eigen::VectorXd& s0, const VectorX& ellipsoid_margins, int gram_total_size, - const CspaceFreePolytope::FindPolytopeGivenLagrangianOptions& options) - const { + const CspaceFreePolytope::FindPolytopeGivenLagrangianOptions& options, + std::unordered_map* + certificates_result) const { return cspace_free_polytope_->FindPolytopeGivenLagrangian( ignored_collision_pairs, C, d, d_minus_Cs, certificates_vec, Q, s0, - ellipsoid_margins, gram_total_size, options); + ellipsoid_margins, gram_total_size, options, certificates_result); } HPolyhedron GetPolyhedronWithJointLimits(const Eigen::MatrixXd& C, @@ -235,6 +217,46 @@ void CheckPositivePolynomialBySamples( .all()); } +// Solve a sos program to check if a polynomial is sos. +// @param tol We seek another polynomial p2 being sos, and the difference +// between p1's coefficient and p2's coefficient is less than tol. +bool IsPolynomialSos(const symbolic::Polynomial& p, double tol) { + DRAKE_DEMAND(p.decision_variables().empty()); + if (p.monomial_to_coefficient_map().empty()) { + // p = 0. + return true; + } else if (p.monomial_to_coefficient_map().size() == 1 && + p.monomial_to_coefficient_map().count(symbolic::Monomial()) > 0) { + // p is a constant + symbolic::Environment env; + const double constant = + p.monomial_to_coefficient_map().at(symbolic::Monomial()).Evaluate(env); + return constant >= -tol; + } + solvers::MathematicalProgram prog; + VectorX indeterminates_vec(p.indeterminates().size()); + int indeterminate_count = 0; + for (const auto& v : p.indeterminates()) { + indeterminates_vec(indeterminate_count++) = v; + } + prog.AddIndeterminates(indeterminates_vec); + if (tol == 0) { + prog.AddSosConstraint(p); + } else { + const VectorX monomial_basis = + solvers::ConstructMonomialBasis(p); + const auto pair = prog.NewSosPolynomial( + monomial_basis, + solvers::MathematicalProgram::NonnegativePolynomial::kSos); + const symbolic::Polynomial poly_diff = pair.first - p; + for (const auto& term : poly_diff.monomial_to_coefficient_map()) { + prog.AddLinearConstraint(term.second, -tol, tol); + } + } + const auto result = solvers::Solve(prog); + return result.is_success(); +} + TEST_F(CIrisToyRobotTest, GetCollisionGeometries) { const auto link_geometries = GetCollisionGeometries(*plant_, *scene_graph_); // Each link has some geometries. @@ -314,28 +336,6 @@ TEST_F(CIrisToyRobotTest, CspaceFreePolytopeConstructor) { } EXPECT_EQ(plane.b.TotalDegree(), 1); EXPECT_EQ(plane.b.indeterminates(), s_set); - - // ensure the correct number of PlaneSeparatesGeometries objects are - // constructed. The content of these objects are tested in - // CspaceFreePolytopeGenerateRationals. - bool associated_to_PlaneSeparatesGeometries_with_margin = false; - bool associated_to_PlaneSeparatesGeometries_without_margin = false; - for (const auto& plane_separates_geometries : - tester.plane_geometries_w_margin()) { - if (plane_separates_geometries.plane_index == plane_index) { - associated_to_PlaneSeparatesGeometries_with_margin = true; - break; - } - } - EXPECT_TRUE(associated_to_PlaneSeparatesGeometries_with_margin); - for (const auto& plane_separates_geometries : - tester.plane_geometries_wo_margin()) { - if (plane_separates_geometries.plane_index == plane_index) { - associated_to_PlaneSeparatesGeometries_without_margin = true; - break; - } - } - EXPECT_TRUE(associated_to_PlaneSeparatesGeometries_without_margin); } } @@ -343,55 +343,45 @@ TEST_F(CIrisToyRobotTest, CspaceFreePolytopeGenerateRationals) { const Eigen::Vector3d q_star(0, 0, 0); CspaceFreePolytopeTester tester(plant_, scene_graph_, SeparatingPlaneOrder::kAffine, q_star); - auto ret = tester.GenerateRationals(false /* search_separating_margin */); - EXPECT_EQ(ret.size(), + EXPECT_EQ(tester.plane_geometries().size(), tester.cspace_free_polytope().separating_planes().size()); - for (const auto& plane_geometries : ret) { + for (const auto& plane_geometries : tester.plane_geometries()) { const auto& plane = tester.cspace_free_polytope() .separating_planes()[plane_geometries.plane_index]; if (plane.positive_side_geometry->type() == GeometryType::kCylinder || plane.negative_side_geometry->type() == GeometryType::kCylinder) { throw std::runtime_error( "Cylinder has not been implemented yet for C-IRIS."); - } else if (plane.positive_side_geometry->type() == GeometryType::kSphere || - plane.positive_side_geometry->type() == GeometryType::kCapsule || - plane.negative_side_geometry->type() == GeometryType::kSphere || - plane.negative_side_geometry->type() == GeometryType::kCapsule) { - // If one of the geometry is sphere or capsule, and neither geometry is - // cylinder, then the unit length vector is just a. - EXPECT_EQ(plane_geometries.unit_length_vectors.size(), 1); - for (int i = 0; i < 3; ++i) { - EXPECT_EQ(plane_geometries.unit_length_vectors[0][i], plane.a(i)); - } + } + if (plane.positive_side_geometry->type() == GeometryType::kPolytope && + plane.negative_side_geometry->type() == GeometryType::kPolytope) { + EXPECT_EQ(plane_geometries.positive_side_rationals.size(), + plane.positive_side_geometry->num_rationals_for_points()); + EXPECT_EQ(plane_geometries.negative_side_rationals.size(), + plane.negative_side_geometry->num_rationals_for_points()); } else if (plane.positive_side_geometry->type() == GeometryType::kPolytope && + plane.negative_side_geometry->type() != + GeometryType::kPolytope) { + EXPECT_EQ(plane_geometries.positive_side_rationals.size(), + plane.positive_side_geometry->num_rationals_for_points()); + EXPECT_TRUE(plane_geometries.negative_side_rationals.empty()); + } else if (plane.positive_side_geometry->type() != + GeometryType::kPolytope && plane.negative_side_geometry->type() == GeometryType::kPolytope) { - EXPECT_TRUE(plane_geometries.unit_length_vectors.empty()); - } - EXPECT_EQ(plane_geometries.positive_side_rationals.size(), - plane.positive_side_geometry->num_rationals_per_side()); - EXPECT_EQ(plane_geometries.negative_side_rationals.size(), - plane.negative_side_geometry->num_rationals_per_side()); - EXPECT_FALSE(plane_geometries.separating_margin.has_value()); - } - - ret = tester.GenerateRationals(true /* search_separating_margin */); - EXPECT_EQ(ret.size(), - tester.cspace_free_polytope().separating_planes().size()); - for (const auto& plane_geometries : ret) { - const auto& plane = tester.cspace_free_polytope() - .separating_planes()[plane_geometries.plane_index]; - if (plane.positive_side_geometry->type() != GeometryType::kCylinder && - plane.negative_side_geometry->type() != GeometryType::kCylinder) { - // The unit length vector is always a. - EXPECT_EQ(plane_geometries.unit_length_vectors.size(), 1); - EXPECT_EQ(plane_geometries.unit_length_vectors[0].rows(), 3); - for (int i = 0; i < 3; ++i) { - EXPECT_EQ(plane_geometries.unit_length_vectors[0](i), plane.a(i)); - } + EXPECT_TRUE(plane_geometries.positive_side_rationals.empty()); + EXPECT_EQ(plane_geometries.negative_side_rationals.size(), + plane.negative_side_geometry->num_rationals_for_points()); + } else { + EXPECT_EQ(plane_geometries.positive_side_rationals.size(), + plane.positive_side_geometry->num_rationals_for_points()); + EXPECT_TRUE(plane_geometries.negative_side_rationals.empty()); } - EXPECT_TRUE(plane_geometries.separating_margin.has_value()); + EXPECT_EQ(plane_geometries.positive_side_psd_mat.size(), + plane.positive_side_geometry->num_rationals_for_matrix_sos()); + EXPECT_EQ(plane_geometries.negative_side_psd_mat.size(), + plane.negative_side_geometry->num_rationals_for_matrix_sos()); } } @@ -469,22 +459,43 @@ TEST_F(CIrisToyRobotTest, CalcSBoundsPolynomial) { } TEST_F(CIrisToyRobotTest, CalcMonomialBasis) { + // Test CalcMonomialBasis const Eigen::Vector3d q_star(0, 0, 0); - CspaceFreePolytopeTester tester(plant_, scene_graph_, - SeparatingPlaneOrder::kAffine, q_star); - const auto map_body_to_monomial_basis = tester.map_body_to_monomial_basis(); - // Make sure map_body_to_monomial_basis contains all pairs of bodies. - for (const auto& plane : tester.cspace_free_polytope().separating_planes()) { - for (const auto collision_geometry : - {plane.positive_side_geometry, plane.negative_side_geometry}) { - auto it = - map_body_to_monomial_basis.find(SortedPair( - plane.expressed_body, collision_geometry->body_index())); - EXPECT_NE(it, map_body_to_monomial_basis.end()); - // Make sure the degree for each variable in the monomial is at most 1. - for (int i = 0; i < it->second.rows(); ++i) { - for (const auto& [var, degree] : it->second(i).get_powers()) { - EXPECT_LE(degree, 1); + CspaceFreePolytope::Options options; + for (bool with_cross_y : {false, true}) { + options.with_cross_y = with_cross_y; + CspaceFreePolytopeTester tester( + plant_, scene_graph_, SeparatingPlaneOrder::kAffine, q_star, options); + + const auto& map_body_to_monomial_basis_array = + tester.map_body_to_monomial_basis_array(); + // Make sure map_body_to_monomial_basis_array contains all pairs of bodies. + for (const auto& plane : + tester.cspace_free_polytope().separating_planes()) { + for (const auto collision_geometry : + {plane.positive_side_geometry, plane.negative_side_geometry}) { + const SortedPair body_pair( + plane.expressed_body, collision_geometry->body_index()); + auto it = map_body_to_monomial_basis_array.find(body_pair); + EXPECT_NE(it, map_body_to_monomial_basis_array.end()); + const auto& monomial_basis_array = it->second; + for (int i = 0; i < monomial_basis_array[0].rows(); ++i) { + // Make sure the degree for each variable in the + // monomial_basis_array[0] is at most 1. + for (const auto& [var, degree] : + monomial_basis_array[0](i).get_powers()) { + EXPECT_LE(degree, 1); + } + } + for (int i = 0; i < 3; ++i) { + EXPECT_EQ(monomial_basis_array[i + 1].rows(), + monomial_basis_array[0].rows()); + for (int j = 0; j < monomial_basis_array[0].rows(); ++j) { + EXPECT_EQ( + monomial_basis_array[i + 1](j), + symbolic::Monomial(tester.cspace_free_polytope().y_slack()(i)) * + monomial_basis_array[0](j)); + } } } } @@ -504,133 +515,135 @@ void SetupPolytope(const CspaceFreePolytopeTester& tester, s_lower_redundant_indices, s_upper_redundant_indices); } -TEST_F(CIrisToyRobotTest, ConstructPlaneSearchProgram1) { - // Test ConstructPlaneSearchProgram with no unit-length-vector constraint. - const Eigen::Vector3d q_star(0, 0, 0); - CspaceFreePolytopeTester tester(plant_, scene_graph_, - SeparatingPlaneOrder::kAffine, q_star); - Eigen::Matrix C; - // clang-format off - C << 1, 1, 0, - -1, -1, 0, - -1, 0, 1, - 1, 0, -1, - 0, 1, 1, - 0, -1, -1, - 1, 0, 1, - 1, 1, -1, - 1, -1, 1; - // clang-format on - Eigen::Matrix d; - d << 0.1, 0.2, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.2; +// Lagrangian should be positive if it is irredundant, otherwise it should be +// zero. +void CheckLagrangians(const VectorX& lagrangians, + const std::unordered_set redundant_indices, + const Eigen::MatrixXd& indeterminates_samples, + const VectorX& indeterminates) { + for (int i = 0; i < lagrangians.rows(); ++i) { + if (redundant_indices.count(i) == 0) { + CheckPositivePolynomialBySamples(lagrangians(i), indeterminates, + indeterminates_samples.transpose()); + } else { + EXPECT_PRED2(symbolic::test::PolyEqual, lagrangians(i), + symbolic::Polynomial()); + } + } +} - VectorX d_minus_Cs; - std::unordered_set C_redundant_indices; - std::unordered_set s_lower_redundant_indices; - std::unordered_set s_upper_redundant_indices; - SetupPolytope(tester, C, d, &d_minus_Cs, &C_redundant_indices, - &s_lower_redundant_indices, &s_upper_redundant_indices); +// @param a Maps the plane index to the separating plane parameter {x| aᵀx+b=0} +// @param b Maps the plane index to the separating plane parameter {x| aᵀx+b=0} +void CheckSeparationBySamples( + const CspaceFreePolytopeTester& tester, + const systems::Diagram& diagram, + const Eigen::Ref& s_samples, + const Eigen::MatrixXd& C, const Eigen::VectorXd& d, + const std::unordered_map>& a, + const std::unordered_map& b, + const Eigen::Ref& q_star, + const CspaceFreePolytope::IgnoredCollisionPairs& ignored_collision_pairs) { + auto diagram_context = diagram.CreateDefaultContext(); + const auto& plant = + tester.cspace_free_polytope().rational_forward_kin().plant(); + auto& plant_context = + plant.GetMyMutableContextFromRoot(diagram_context.get()); - const auto plane_geometries_vec = tester.GenerateRationals(false); - // Consider the plane between world_box_ and body3_box_. - // Notice that this chain only has one DOF, hence one of the rationals is - // actually a constant. - int plane_geometries_index = -1; - for (int i = 0; i < static_cast(plane_geometries_vec.size()); ++i) { - const auto& plane = - tester.cspace_free_polytope() - .separating_planes()[plane_geometries_vec[i].plane_index]; - if (SortedPair(plane.positive_side_geometry->id(), - plane.negative_side_geometry->id()) == - SortedPair(world_box_, body3_box_)) { - plane_geometries_index = i; - break; + for (int i = 0; i < s_samples.rows(); ++i) { + const Eigen::Vector3d s_val = ProjectToPolytope( + s_samples.row(i).transpose(), C, d, tester.s_lower(), tester.s_upper()); + symbolic::Environment env; + env.insert(tester.cspace_free_polytope().rational_forward_kin().s(), s_val); + const Eigen::VectorXd q_val = + tester.cspace_free_polytope().rational_forward_kin().ComputeQValue( + s_val, q_star); + plant.SetPositions(&plant_context, q_val); + for (int plane_index = 0; + plane_index < + static_cast( + tester.cspace_free_polytope().separating_planes().size()); + ++plane_index) { + const auto& plane = + tester.cspace_free_polytope().separating_planes()[plane_index]; + if (ignored_collision_pairs.count(SortedPair( + plane.positive_side_geometry->id(), + plane.negative_side_geometry->id())) == 0 && + a.count(plane_index) > 0 && b.count(plane_index) > 0) { + Eigen::Vector3d a_val; + for (int j = 0; j < 3; ++j) { + a_val(j) = a.at(plane_index)(j).Evaluate(env); + } + const double b_val = b.at(plane_index).Evaluate(env); + EXPECT_GE( + DistanceToHalfspace(*plane.positive_side_geometry, a_val, b_val, + plane.expressed_body, PlaneSide::kPositive, + plant, plant_context), + 0); + EXPECT_GE( + DistanceToHalfspace(*plane.negative_side_geometry, a_val, b_val, + plane.expressed_body, PlaneSide::kNegative, + plant, plant_context), + 0); + } } } - const auto& plane_geometries = plane_geometries_vec[plane_geometries_index]; - auto ret = tester.ConstructPlaneSearchProgram( - plane_geometries, d_minus_Cs, C_redundant_indices, - s_lower_redundant_indices, s_upper_redundant_indices); - - solvers::MosekSolver solver; - if (solver.available()) { - solvers::SolverOptions solver_options; - auto result = solver.Solve(*(ret.prog), std::nullopt, solver_options); - ASSERT_TRUE(result.is_success()); +} - Eigen::Matrix s_samples; - // clang-format off - s_samples << 1, 2, -1, - -0.5, 0.3, 0.2, - 0.2, 0.1, 0.4, - 0.5, -1.2, 0.3, - 0.2, 0.5, -0.4, - -0.3, 1.5, 2, - 0.5, 0.2, 1, - -0.4, 0.5, 1, - 0, 0, 0, - 0.2, -1.5, 1; - // clang-format on +void CheckSosLagrangians( + const std::vector& + lagrangians_vec) { + for (const auto& lagrangians : lagrangians_vec) { + for (int i = 0; i < lagrangians.polytope.rows(); ++i) { + EXPECT_TRUE(IsPolynomialSos(lagrangians.polytope(i), 0.0)); + } + for (int i = 0; i < lagrangians.s_lower.rows(); ++i) { + EXPECT_TRUE(IsPolynomialSos(lagrangians.s_lower(i), 0.0)); + } + for (int i = 0; i < lagrangians.s_upper.rows(); ++i) { + EXPECT_TRUE(IsPolynomialSos(lagrangians.s_upper(i), 0.0)); + } + } +} - const auto& s = tester.cspace_free_polytope().rational_forward_kin().s(); - - auto check_lagrangians = - [&s_samples, &s](const VectorX& lagrangians, - const std::unordered_set redundant_indices) { - for (int i = 0; i < lagrangians.rows(); ++i) { - if (redundant_indices.count(i) == 0) { - CheckPositivePolynomialBySamples(lagrangians(i), s, - s_samples.transpose()); - } else { - EXPECT_PRED2(symbolic::test::PolyEqual, lagrangians(i), - symbolic::Polynomial()); - } - } - }; - - const VectorX& s_minus_s_lower = - tester.s_minus_s_lower(); - const VectorX& s_upper_minus_s = - tester.s_upper_minus_s(); - auto check_rational_positive_in_polytope = - [&result, &d_minus_Cs, &s_minus_s_lower, &s_upper_minus_s, - check_lagrangians, &C_redundant_indices, &s_lower_redundant_indices, - &s_upper_redundant_indices, &s, &s_samples]( - const std::vector& rationals, - const std::vector& - plane_side_lagrangians) { - for (int i = 0; i < static_cast(rationals.size()); ++i) { - const auto& search_plane_lagrangians = plane_side_lagrangians[i]; - const auto search_plane_lagrangians_result = - search_plane_lagrangians.GetSolution(result); - check_lagrangians(search_plane_lagrangians_result.polytope, - C_redundant_indices); - check_lagrangians(search_plane_lagrangians_result.s_lower, - s_lower_redundant_indices); - check_lagrangians(search_plane_lagrangians_result.s_upper, - s_upper_redundant_indices); - const symbolic::Polynomial poly = - result.GetSolution(rationals[i].numerator()) - - search_plane_lagrangians_result.polytope.dot(d_minus_Cs) - - search_plane_lagrangians_result.s_lower.dot(s_minus_s_lower) - - search_plane_lagrangians_result.s_upper.dot(s_upper_minus_s); - CheckPositivePolynomialBySamples(poly, s, s_samples.transpose()); - } - }; - check_rational_positive_in_polytope( - plane_geometries.positive_side_rationals, - ret.positive_side_lagrangians); - check_rational_positive_in_polytope( - plane_geometries.negative_side_rationals, - ret.negative_side_lagrangians); +// Check if rationals are all positive in the C-space polytope. Note that for +// some reason Mosek doesn't solve to a very high precision (the constraint +// violation can be in the order of 1E-3, even if Mosek reports success), so we +// could use a pretty large tolerance `tol`. +void CheckRationalsPositiveInCspacePolytope( + const std::vector& rationals, + const std::vector& + lagrangians_vec, + const VectorX& plane_decision_vars, + const Eigen::VectorXd& plane_decision_var_vals, const Eigen::MatrixXd& C, + const Eigen::VectorXd& d, const CspaceFreePolytopeTester& tester, + double tol) { + const VectorX d_minus_Cs = + tester.CalcDminusCs(C, d); + symbolic::Environment env; + env.insert(plane_decision_vars, plane_decision_var_vals); + EXPECT_EQ(rationals.size(), lagrangians_vec.size()); + for (int i = 0; i < static_cast(rationals.size()); ++i) { + const symbolic::Polynomial rational_numerator = + rationals[i].numerator().EvaluatePartial(env); + const symbolic::Polynomial sos_poly = + rational_numerator - lagrangians_vec[i].polytope.dot(d_minus_Cs) - + lagrangians_vec[i].s_lower.dot(tester.s_minus_s_lower()) - + lagrangians_vec[i].s_upper.dot(tester.s_upper_minus_s()); + EXPECT_TRUE(IsPolynomialSos(sos_poly, tol)); } + CheckSosLagrangians(lagrangians_vec); } -TEST_F(CIrisToyRobotTest, ConstructPlaneSearchProgram2) { - // Test ConstructPlaneSearchProgram with unit-length-vector constraint. +void TestConstructPlaneSearchProgram( + const systems::Diagram& diagram, + const multibody::MultibodyPlant& plant, + const SceneGraph& scene_graph, + const SortedPair& geometry_pair, bool with_cross_y) { const Eigen::Vector3d q_star(0, 0, 0); - CspaceFreePolytopeTester tester(plant_, scene_graph_, - SeparatingPlaneOrder::kAffine, q_star); + CspaceFreePolytope::Options options; + options.with_cross_y = with_cross_y; + CspaceFreePolytopeTester tester( + &plant, &scene_graph, SeparatingPlaneOrder::kAffine, q_star, options); Eigen::Matrix C; // clang-format off C << 1, 1, 0, @@ -653,31 +666,92 @@ TEST_F(CIrisToyRobotTest, ConstructPlaneSearchProgram2) { SetupPolytope(tester, C, d, &d_minus_Cs, &C_redundant_indices, &s_lower_redundant_indices, &s_upper_redundant_indices); - const auto plane_geometries_vec = tester.GenerateRationals(false); - // Consider the plane between world_sphere_ and body2_capsule_. + // Consider the plane between world_box_ and body3_box_. + // Notice that this chain only has one DOF, hence one of the rationals is + // actually a constant. int plane_geometries_index = -1; - for (int i = 0; i < static_cast(plane_geometries_vec.size()); ++i) { + for (int i = 0; i < static_cast(tester.plane_geometries().size()); ++i) { const auto& plane = tester.cspace_free_polytope() - .separating_planes()[plane_geometries_vec[i].plane_index]; + .separating_planes()[tester.plane_geometries()[i].plane_index]; if (SortedPair(plane.positive_side_geometry->id(), plane.negative_side_geometry->id()) == - SortedPair(world_sphere_, body2_capsule_)) { + geometry_pair) { plane_geometries_index = i; break; } } - const auto& plane_geometries = plane_geometries_vec[plane_geometries_index]; + const auto& plane_geometries = + tester.plane_geometries()[plane_geometries_index]; + const auto& plane = tester.cspace_free_polytope() + .separating_planes()[plane_geometries.plane_index]; auto ret = tester.ConstructPlaneSearchProgram( plane_geometries, d_minus_Cs, C_redundant_indices, s_lower_redundant_indices, s_upper_redundant_indices); - EXPECT_EQ(ret.unit_length_lagrangians.size(), - plane_geometries.unit_length_vectors.size()); + EXPECT_EQ(ret.certificate.positive_side_rational_lagrangians.size(), + plane_geometries.positive_side_rationals.size()); + EXPECT_EQ(ret.certificate.negative_side_rational_lagrangians.size(), + plane_geometries.negative_side_rationals.size()); + EXPECT_EQ(ret.certificate.positive_side_psd_mat_lagrangians.size(), + plane_geometries.positive_side_psd_mat.size()); + EXPECT_EQ(ret.certificate.negative_side_psd_mat_lagrangians.size(), + plane_geometries.negative_side_psd_mat.size()); solvers::MosekSolver solver; if (solver.available()) { - const auto result = solver.Solve(*(ret.prog)); - ASSERT_TRUE(result.is_success()); + solvers::SolverOptions solver_options; + const auto result = solver.Solve(*(ret.prog), std::nullopt, solver_options); + EXPECT_TRUE(result.is_success()); + + auto get_lagrangian_result = + [](const solvers::MathematicalProgramResult& prog_result, + const std::vector& + lagrangians_vec) + -> std::vector { + std::vector + lagrangians_result; + for (const auto& lagrangian : lagrangians_vec) { + lagrangians_result.push_back(lagrangian.GetSolution(prog_result)); + } + return lagrangians_result; + }; + const auto positive_side_rational_lagrangians_result = + get_lagrangian_result( + result, ret.certificate.positive_side_rational_lagrangians); + const auto negative_side_rational_lagrangians_result = + get_lagrangian_result( + result, ret.certificate.negative_side_rational_lagrangians); + const auto positive_side_psd_mat_lagrangians_result = get_lagrangian_result( + result, ret.certificate.positive_side_psd_mat_lagrangians); + const auto negative_side_psd_mat_lagrangians_result = get_lagrangian_result( + result, ret.certificate.negative_side_psd_mat_lagrangians); + + Vector3 a_result; + for (int i = 0; i < 3; ++i) { + a_result(i) = result.GetSolution(plane.a(i)); + } + const symbolic::Polynomial b_result = result.GetSolution(plane.b); + const Eigen::VectorXd plane_decision_var_vals = + result.GetSolution(plane.decision_variables); + + CheckRationalsPositiveInCspacePolytope( + plane_geometries.positive_side_rationals, + positive_side_rational_lagrangians_result, plane.decision_variables, + plane_decision_var_vals, C, d, tester, 0); + CheckRationalsPositiveInCspacePolytope( + plane_geometries.negative_side_rationals, + negative_side_rational_lagrangians_result, plane.decision_variables, + plane_decision_var_vals, C, d, tester, 0); + const double small_coeff_tol = 1E-5; + CheckRationalsPositiveInCspacePolytope( + plane_geometries.positive_side_psd_mat, + positive_side_psd_mat_lagrangians_result, plane.decision_variables, + plane_decision_var_vals, C, d, tester, small_coeff_tol); + CheckRationalsPositiveInCspacePolytope( + plane_geometries.negative_side_psd_mat, + negative_side_psd_mat_lagrangians_result, plane.decision_variables, + plane_decision_var_vals, C, d, tester, small_coeff_tol); + Eigen::Matrix s_samples; // clang-format off s_samples << 1, 2, -1, @@ -691,168 +765,47 @@ TEST_F(CIrisToyRobotTest, ConstructPlaneSearchProgram2) { 0, 0, 0, 0.2, -1.5, 1; // clang-format on - for (int i = 0; i < s_samples.rows(); ++i) { - const Eigen::Vector3d s_val = - ProjectToPolytope(s_samples.row(i).transpose(), C, d, - tester.s_lower(), tester.s_upper()); - // Check if the unit_length_vector really has norm <= 1. - for (const auto& unit_length_vec : plane_geometries.unit_length_vectors) { - Vector3 unit_length_vec_result; - for (int j = 0; j < 3; ++j) { - unit_length_vec_result(j) = result.GetSolution(unit_length_vec(j)); - } - Eigen::Vector3d unit_length_vec_val; - symbolic::Environment env; - env.insert(tester.cspace_free_polytope().rational_forward_kin().s(), - s_val); - for (int j = 0; j < 3; ++j) { - unit_length_vec_val(j) = unit_length_vec_result(j).Evaluate(env); - } - EXPECT_LE(unit_length_vec_val.norm(), 1); - } - } + + CheckSeparationBySamples(tester, diagram, s_samples, C, d, + {{plane_geometries.plane_index, a_result}}, + {{plane_geometries.plane_index, b_result}}, q_star, + {}); } } -TEST_F(CIrisToyRobotTest, AddUnitLengthConstraint) { - // Test CspaceFreePolytope::AddUnitLengthConstraint - const Eigen::Vector3d q_star(0, 0, 0); - CspaceFreePolytopeTester tester(plant_, scene_graph_, - SeparatingPlaneOrder::kAffine, q_star); - Eigen::Matrix C; - // clang-format off - C << 1, 1, 0, - -1, -1, 0, - -1, 0, 1, - 1, 0, -1, - 0, 1, 1, - 0, -1, -1, - 1, 0, 1, - 1, 1, -1, - 1, -1, 1; - // clang-format on - Eigen::Matrix d; - d << 0.1, 0.2, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.2; - - VectorX d_minus_Cs; - std::unordered_set C_redundant_indices; - std::unordered_set s_lower_redundant_indices; - std::unordered_set s_upper_redundant_indices; - SetupPolytope(tester, C, d, &d_minus_Cs, &C_redundant_indices, - &s_lower_redundant_indices, &s_upper_redundant_indices); - solvers::MathematicalProgram prog; - prog.AddIndeterminates( - tester.cspace_free_polytope().rational_forward_kin().s()); - - const auto plane_geometries_vec = tester.GenerateRationals(false); - // Consider the separating plane between world_sphere_ and body2_capsule_. - int plane_geometries_index = -1; - for (int i = 0; i < static_cast(plane_geometries_vec.size()); ++i) { - const auto& plane = - tester.cspace_free_polytope() - .separating_planes()[plane_geometries_vec[i].plane_index]; - if (SortedPair(plane.positive_side_geometry->id(), - plane.negative_side_geometry->id()) == - SortedPair(world_sphere_, body2_capsule_)) { - plane_geometries_index = i; - prog.AddDecisionVariables(plane.decision_variables); - break; - } - } - const auto& plane_geometries = plane_geometries_vec[plane_geometries_index]; - - prog.AddIndeterminates(tester.cspace_free_polytope().y_slack()); - const auto ret = tester.AddUnitLengthConstraint( - &prog, plane_geometries.unit_length_vectors[0], d_minus_Cs, - C_redundant_indices, s_lower_redundant_indices, s_upper_redundant_indices, - std::nullopt /* polytope lagrangians */); - EXPECT_EQ(ret.polytope.rows(), C.rows()); - const int s_size = - tester.cspace_free_polytope().rational_forward_kin().s().rows(); - EXPECT_EQ(ret.s_lower.rows(), s_size); - EXPECT_EQ(ret.s_upper.rows(), s_size); - const symbolic::Variables y_set{tester.cspace_free_polytope().y_slack()}; - // Make sure that the Lagrangians are quadratic polynomials of y, if they are - // not redundant. - for (int i = 0; i < ret.polytope.rows(); ++i) { - if (C_redundant_indices.count(i) == 0) { - EXPECT_EQ(ret.polytope(i).indeterminates(), y_set); - EXPECT_EQ(ret.polytope(i).TotalDegree(), 2); - } else { - EXPECT_PRED2(symbolic::test::PolyEqual, ret.polytope(i), - symbolic::Polynomial()); - } +TEST_F(CIrisToyRobotTest, ConstructPlaneSearchProgram1) { + // Test ConstructPlaneSearchProgram with both geometries being polytopes. + for (bool with_cross_y : {false, true}) { + TestConstructPlaneSearchProgram( + *diagram_, *plant_, *scene_graph_, + SortedPair(world_box_, body3_box_), with_cross_y); } - for (int i = 0; i < s_size; ++i) { - if (s_lower_redundant_indices.count(i) == 0) { - EXPECT_EQ(ret.s_lower(i).indeterminates(), y_set); - EXPECT_EQ(ret.s_lower(i).TotalDegree(), 2); - } else { - EXPECT_PRED2(symbolic::test::PolyEqual, ret.s_lower(i), - symbolic::Polynomial()); - } - if (s_upper_redundant_indices.count(i) == 0) { - EXPECT_EQ(ret.s_upper(i).indeterminates(), y_set); - EXPECT_EQ(ret.s_upper(i).TotalDegree(), 2); - } else { - EXPECT_PRED2(symbolic::test::PolyEqual, ret.s_upper(i), - symbolic::Polynomial()); - } +} + +TEST_F(CIrisToyRobotTest, ConstructPlaneSearchProgram2) { + // Test ConstructPlaneSearchProgram with neither geometries being polytope. + for (bool with_cross_y : {false, true}) { + TestConstructPlaneSearchProgram( + *diagram_, *plant_, *scene_graph_, + SortedPair(world_sphere_, body2_capsule_), + with_cross_y); } - const symbolic::Variables s_set( - tester.cspace_free_polytope().rational_forward_kin().s()); - EXPECT_EQ(ret.y_square.indeterminates(), s_set); - EXPECT_EQ(ret.y_square.TotalDegree(), 1); +} - solvers::MosekSolver solver; - if (solver.available()) { - const auto result = solver.Solve(prog); - ASSERT_TRUE(result.is_success()); - Vector3 unit_length_vec; - for (int i = 0; i < 3; ++i) { - unit_length_vec(i) = - result.GetSolution(plane_geometries.unit_length_vectors[0](i)); - } - Eigen::Matrix s_samples; - // clang-format off - s_samples << 1, 2, -1, - -0.5, 0.3, 0.2, - 0.2, 0.1, 0.4, - 0.5, -1.2, 0.3, - 0.2, 0.5, -0.4, - -0.3, 1.5, 2, - 0.5, 0.2, 1, - -0.4, 0.5, 1, - 0, 0, 0, - 0.2, -1.5, 1; - // clang-format on - const Eigen::Vector3d s_lower = - tester.cspace_free_polytope().rational_forward_kin().ComputeSValue( - plant_->GetPositionLowerLimits(), q_star); - const Eigen::Vector3d s_upper = - tester.cspace_free_polytope().rational_forward_kin().ComputeSValue( - plant_->GetPositionUpperLimits(), q_star); - for (int i = 0; i < s_samples.rows(); ++i) { - const Eigen::Vector3d s_val = ProjectToPolytope( - s_samples.row(i).transpose(), C, d, s_lower, s_upper); - Eigen::Vector3d unit_length_vec_val; - symbolic::Environment env; - env.insert(tester.cspace_free_polytope().rational_forward_kin().s(), - s_val); - for (int j = 0; j < 3; ++j) { - unit_length_vec_val(j) = unit_length_vec(j).Evaluate(env); - } - EXPECT_LE(unit_length_vec_val.norm(), 1); - } +TEST_F(CIrisToyRobotTest, ConstructPlaneSearchProgram3) { + // Test ConstructPlaneSearchProgram with one geometry being polytope and the + // other not. + for (bool with_cross_y : {false, true}) { + TestConstructPlaneSearchProgram( + *diagram_, *plant_, *scene_graph_, + SortedPair(body1_convex_, body3_sphere_), + with_cross_y); } } TEST_F(CIrisToyRobotTest, FindSeparationCertificateGivenPolytopeSuccess) { // Test CspaceFreePolytope::FindSeparationCertificateGivenPolytope for a // collision-free C-space polytope. - auto diagram_context = diagram_->CreateDefaultContext(); - auto& plant_context = - plant_->GetMyMutableContextFromRoot(diagram_context.get()); const Eigen::Vector3d q_star(0, 0, 0); CspaceFreePolytopeTester tester(plant_, scene_graph_, SeparatingPlaneOrder::kAffine, q_star); @@ -887,20 +840,11 @@ TEST_F(CIrisToyRobotTest, FindSeparationCertificateGivenPolytopeSuccess) { 0, 0, 0, 0.2, -1.5, 1; // clang-format on - Eigen::Matrix3Xd s_vals(3, s_samples.rows()); - Eigen::Matrix3Xd q_vals(3, s_samples.rows()); - for (int i = 0; i < s_samples.rows(); ++i) { - s_vals.col(i) = ProjectToPolytope(s_samples.row(i).transpose(), C, d, - tester.s_lower(), tester.s_upper()); - q_vals.col(i) = - tester.cspace_free_polytope().rational_forward_kin().ComputeQValue( - s_vals.col(i), q_star); - } const CspaceFreePolytope::IgnoredCollisionPairs ignored_collision_pairs{ SortedPair(world_box_, body2_sphere_)}; CspaceFreePolytope::FindSeparationCertificateGivenPolytopeOptions options; - options.verbose = true; + options.verbose = false; solvers::MosekSolver solver; options.solver_id = solver.id(); if (solver.available()) { @@ -908,9 +852,8 @@ TEST_F(CIrisToyRobotTest, FindSeparationCertificateGivenPolytopeSuccess) { options.num_threads = num_threads; const auto certificates_result = - tester.FindSeparationCertificateGivenPolytope( - ignored_collision_pairs, C, d, true /* search_separating_plane */, - options); + tester.FindSeparationCertificateGivenPolytope(ignored_collision_pairs, + C, d, options); EXPECT_EQ(certificates_result.size(), tester.cspace_free_polytope().separating_planes().size() - ignored_collision_pairs.size()); @@ -923,33 +866,36 @@ TEST_F(CIrisToyRobotTest, FindSeparationCertificateGivenPolytopeSuccess) { plane.positive_side_geometry->id(), plane.negative_side_geometry->id())), 0); - EXPECT_TRUE(certificate->separating_margin.has_value()); - EXPECT_GT(certificate->separating_margin.value(), 0); - - // Make sure that the unit length constraint is satisfied. - for (int j = 0; j < s_vals.cols(); ++j) { - symbolic::Environment env; - env.insert(tester.cspace_free_polytope().rational_forward_kin().s(), - s_vals.col(j)); - Eigen::Vector3d a_val; - for (int k = 0; k < 3; ++k) { - a_val(k) = certificate->a(k).Evaluate(env); - } - EXPECT_LE(a_val.norm(), 1); - const double b_val = certificate->b.Evaluate(env); - - plant_->SetPositions(&plant_context, q_vals.col(j)); - EXPECT_GE( - DistanceToHalfspace(*(plane.positive_side_geometry), a_val, b_val, - plane.expressed_body, PlaneSide::kPositive, - *plant_, plant_context), - certificate->separating_margin.value()); - EXPECT_GE( - DistanceToHalfspace(*(plane.negative_side_geometry), a_val, b_val, - plane.expressed_body, PlaneSide::kNegative, - *plant_, plant_context), - certificate->separating_margin.value()); - } + + CheckSeparationBySamples(tester, *diagram_, s_samples, C, d, + {{certificate->plane_index, certificate->a}}, + {{certificate->plane_index, certificate->b}}, + q_star, ignored_collision_pairs); + + // Now check if the polynomials are actually sos. + const auto& plane_geometries = + tester.plane_geometries()[certificate->plane_index]; + CheckRationalsPositiveInCspacePolytope( + plane_geometries.positive_side_rationals, + certificate->positive_side_rational_lagrangians, + plane.decision_variables, certificate->plane_decision_var_vals, C, + d, tester, 0); + CheckRationalsPositiveInCspacePolytope( + plane_geometries.negative_side_rationals, + certificate->negative_side_rational_lagrangians, + plane.decision_variables, certificate->plane_decision_var_vals, C, + d, tester, 0); + const double small_coeff_tol = 2E-5; + CheckRationalsPositiveInCspacePolytope( + plane_geometries.positive_side_psd_mat, + certificate->positive_side_psd_mat_lagrangians, + plane.decision_variables, certificate->plane_decision_var_vals, C, + d, tester, small_coeff_tol); + CheckRationalsPositiveInCspacePolytope( + plane_geometries.negative_side_psd_mat, + certificate->negative_side_psd_mat_lagrangians, + plane.decision_variables, certificate->plane_decision_var_vals, C, + d, tester, small_coeff_tol); } } std::unordered_map, @@ -957,50 +903,21 @@ TEST_F(CIrisToyRobotTest, FindSeparationCertificateGivenPolytopeSuccess) { certificates_map; bool is_success = tester.cspace_free_polytope().FindSeparationCertificateGivenPolytope( - C, d, ignored_collision_pairs, true /* search_separating_margin*/, - options, &certificates_map); + C, d, ignored_collision_pairs, options, &certificates_map); EXPECT_EQ(certificates_map.size(), tester.cspace_free_polytope().separating_planes().size() - ignored_collision_pairs.size()); EXPECT_TRUE(is_success); - // Test search_separating_margin = false. - certificates_map.clear(); - is_success = - tester.cspace_free_polytope().FindSeparationCertificateGivenPolytope( - C, d, ignored_collision_pairs, false /* search_separating_margin */, - options, &certificates_map); - EXPECT_EQ(certificates_map.size(), - tester.cspace_free_polytope().separating_planes().size() - - ignored_collision_pairs.size()); - EXPECT_TRUE(is_success); // Check the separating plane does separate the geometries. + std::unordered_map> a_map; + std::unordered_map b_map; for (const auto& [geometry_pair, certificate] : certificates_map) { - const auto& plane = tester.cspace_free_polytope() - .separating_planes()[certificate.plane_index]; - for (int j = 0; j < s_vals.cols(); ++j) { - symbolic::Environment env; - env.insert(tester.cspace_free_polytope().rational_forward_kin().s(), - s_vals.col(j)); - Eigen::Vector3d a_val; - for (int k = 0; k < 3; ++k) { - a_val(k) = certificate.a(k).Evaluate(env); - } - const double b_val = certificate.b.Evaluate(env); - - plant_->SetPositions(&plant_context, q_vals.col(j)); - EXPECT_GE( - DistanceToHalfspace(*(plane.positive_side_geometry), a_val, b_val, - plane.expressed_body, PlaneSide::kPositive, - *plant_, plant_context), - 0); - EXPECT_GE( - DistanceToHalfspace(*(plane.negative_side_geometry), a_val, b_val, - plane.expressed_body, PlaneSide::kNegative, - *plant_, plant_context), - 0); - } + a_map.emplace(certificate.plane_index, certificate.a); + b_map.emplace(certificate.plane_index, certificate.b); } + CheckSeparationBySamples(tester, *diagram_, s_samples, C, d, a_map, b_map, + q_star, ignored_collision_pairs); } } @@ -1020,12 +937,12 @@ TEST_F(CIrisToyRobotTest, FindSeparationCertificateGivenPolytopeFailure) { 1, 0, -1; // clang-format on Eigen::Matrix d; - d << 0.1, 0.3, 0.4, 0.2; + d << 0.8, 1.3, 2.4, 2.2; const CspaceFreePolytope::IgnoredCollisionPairs ignored_collision_pairs{ SortedPair(world_box_, body2_sphere_)}; CspaceFreePolytope::FindSeparationCertificateGivenPolytopeOptions options; - options.verbose = true; + options.verbose = false; solvers::MosekSolver solver; options.solver_id = solver.id(); if (solver.available()) { @@ -1033,9 +950,8 @@ TEST_F(CIrisToyRobotTest, FindSeparationCertificateGivenPolytopeFailure) { options.num_threads = num_threads; const auto certificates_result = - tester.FindSeparationCertificateGivenPolytope( - ignored_collision_pairs, C, d, - true /* search_separating_margin */, options); + tester.FindSeparationCertificateGivenPolytope(ignored_collision_pairs, + C, d, options); EXPECT_EQ(certificates_result.size(), tester.cspace_free_polytope().separating_planes().size() - ignored_collision_pairs.size()); @@ -1051,9 +967,8 @@ TEST_F(CIrisToyRobotTest, FindSeparationCertificateGivenPolytopeFailure) { // separating plane. options.terminate_at_failure = false; const auto certificates_result = - tester.FindSeparationCertificateGivenPolytope( - ignored_collision_pairs, C, d, true /* search_separating_margin */, - options); + tester.FindSeparationCertificateGivenPolytope(ignored_collision_pairs, + C, d, options); EXPECT_EQ(certificates_result.size(), tester.cspace_free_polytope().separating_planes().size() - ignored_collision_pairs.size()); @@ -1062,17 +977,16 @@ TEST_F(CIrisToyRobotTest, FindSeparationCertificateGivenPolytopeFailure) { certificates_map; const bool is_success = tester.cspace_free_polytope().FindSeparationCertificateGivenPolytope( - C, d, ignored_collision_pairs, true /* search_separating_margin */, - options, &certificates_map); + C, d, ignored_collision_pairs, options, &certificates_map); EXPECT_FALSE(is_success); std::unordered_set> inseparable_geometries; inseparable_geometries.emplace(body0_box_, body2_capsule_); inseparable_geometries.emplace(body0_box_, body2_sphere_); - inseparable_geometries.emplace(body0_sphere_, body2_capsule_); - inseparable_geometries.emplace(body0_sphere_, body2_sphere_); + inseparable_geometries.emplace(body1_convex_, body3_box_); inseparable_geometries.emplace(body1_convex_, body3_sphere_); - inseparable_geometries.emplace(body2_sphere_, body3_sphere_); + inseparable_geometries.emplace(body2_capsule_, body3_box_); + inseparable_geometries.emplace(body2_sphere_, body3_box_); for (const auto& certificate : certificates_result) { if (certificate.has_value()) { const auto& plane = tester.cspace_free_polytope() @@ -1163,58 +1077,275 @@ TEST_F(CIrisToyRobotTest, AddCspacePolytopeContainment) { } } -void CheckSeparationBySamples( - const CspaceFreePolytopeTester& tester, - const systems::Diagram& diagram, - const Eigen::Ref& s_samples, - const Eigen::MatrixXd& C, const Eigen::VectorXd& d, - const std::unordered_map>& a, - const std::unordered_map& b, - const Eigen::Ref& q_star, - const CspaceFreePolytope::IgnoredCollisionPairs& ignored_collision_pairs) { - auto diagram_context = diagram.CreateDefaultContext(); - const auto& plant = - tester.cspace_free_polytope().rational_forward_kin().plant(); - auto& plant_context = - plant.GetMyMutableContextFromRoot(diagram_context.get()); +void CompareLagrangians( + const std::vector& + lagrangians_vec1, + const std::vector& + lagrangians_vec2, + bool compare_s_bounds) { + EXPECT_EQ(lagrangians_vec1.size(), lagrangians_vec2.size()); + for (int i = 0; i < static_cast(lagrangians_vec1.size()); ++i) { + EXPECT_EQ(lagrangians_vec1[i].polytope.size(), + lagrangians_vec2[i].polytope.size()); + for (int j = 0; j < lagrangians_vec1[i].polytope.rows(); ++j) { + EXPECT_PRED2(symbolic::test::PolyEqual, lagrangians_vec1[i].polytope(j), + lagrangians_vec2[i].polytope(j)); + } + if (compare_s_bounds) { + EXPECT_EQ(lagrangians_vec1[i].s_lower.size(), + lagrangians_vec2[i].s_lower.size()); + EXPECT_EQ(lagrangians_vec1[i].s_upper.size(), + lagrangians_vec2[i].s_upper.size()); + for (int j = 0; j < lagrangians_vec1[i].s_lower.rows(); ++j) { + EXPECT_PRED2(symbolic::test::PolyEqual, lagrangians_vec1[i].s_lower(j), + lagrangians_vec2[i].s_lower(j)); + EXPECT_PRED2(symbolic::test::PolyEqual, lagrangians_vec1[i].s_upper(j), + lagrangians_vec2[i].s_upper(j)); + } + } + } +} - for (int i = 0; i < s_samples.rows(); ++i) { - const Eigen::Vector3d s_val = ProjectToPolytope( - s_samples.row(i).transpose(), C, d, tester.s_lower(), tester.s_upper()); - symbolic::Environment env; - env.insert(tester.cspace_free_polytope().rational_forward_kin().s(), s_val); - const Eigen::VectorXd q_val = - tester.cspace_free_polytope().rational_forward_kin().ComputeQValue( - s_val, q_star); - plant.SetPositions(&plant_context, q_val); - for (int plane_index = 0; - plane_index < - static_cast( - tester.cspace_free_polytope().separating_planes().size()); - ++plane_index) { - const auto& plane = - tester.cspace_free_polytope().separating_planes()[plane_index]; - if (ignored_collision_pairs.count(SortedPair( - plane.positive_side_geometry->id(), - plane.negative_side_geometry->id())) == 0) { - Eigen::Vector3d a_val; - for (int j = 0; j < 3; ++j) { - a_val(j) = a.at(plane_index)(j).Evaluate(env); +void CheckPolytopeSearchResult( + const CspaceFreePolytopeTester& tester, const Eigen::MatrixXd& C_sol, + const Eigen::VectorXd& d_sol, + const solvers::MathematicalProgramResult& result, + const std::vector< + std::optional>& + certificates_result, + const std::unordered_map& + new_certificates, + bool search_s_bounds_lagrangians, double tol) { + // Now check rationals. + std::unordered_map + new_certificates_result; + for (const auto& [plane_index, new_certificate] : new_certificates) { + const auto& plane = + tester.cspace_free_polytope().separating_planes()[plane_index]; + const auto& plane_geometries = tester.plane_geometries()[plane_index]; + const CspaceFreePolytope::SeparationCertificateResult + new_certificate_result = new_certificate.GetSolution( + plane_index, plane.a, plane.b, plane.decision_variables, result); + new_certificates_result.emplace(plane_index, new_certificate_result); + CheckRationalsPositiveInCspacePolytope( + plane_geometries.positive_side_rationals, + new_certificate_result.positive_side_rational_lagrangians, + plane.decision_variables, + new_certificate_result.plane_decision_var_vals, C_sol, d_sol, tester, + tol); + CheckRationalsPositiveInCspacePolytope( + plane_geometries.negative_side_rationals, + new_certificate_result.negative_side_rational_lagrangians, + plane.decision_variables, + new_certificate_result.plane_decision_var_vals, C_sol, d_sol, tester, + tol); + CheckRationalsPositiveInCspacePolytope( + plane_geometries.positive_side_psd_mat, + new_certificate_result.positive_side_psd_mat_lagrangians, + plane.decision_variables, + new_certificate_result.plane_decision_var_vals, C_sol, d_sol, tester, + tol); + CheckRationalsPositiveInCspacePolytope( + plane_geometries.negative_side_psd_mat, + new_certificate_result.negative_side_psd_mat_lagrangians, + plane.decision_variables, + new_certificate_result.plane_decision_var_vals, C_sol, d_sol, tester, + tol); + } + // Check if the Lagrangians are set correctly. + for (const auto& old_certificate : certificates_result) { + const int plane_index = old_certificate->plane_index; + const auto& new_certificate_result = + new_certificates_result.at(plane_index); + CompareLagrangians( + old_certificate->positive_side_rational_lagrangians, + new_certificate_result.positive_side_rational_lagrangians, + !search_s_bounds_lagrangians); + CompareLagrangians( + old_certificate->negative_side_rational_lagrangians, + new_certificate_result.negative_side_rational_lagrangians, + !search_s_bounds_lagrangians); + CompareLagrangians(old_certificate->positive_side_psd_mat_lagrangians, + new_certificate_result.positive_side_psd_mat_lagrangians, + !search_s_bounds_lagrangians); + CompareLagrangians(old_certificate->negative_side_psd_mat_lagrangians, + new_certificate_result.negative_side_psd_mat_lagrangians, + !search_s_bounds_lagrangians); + } +} + +TEST_F(CIrisRobotPolytopicGeometryTest, InitializePolytopeSearchProgram) { + const Eigen::Vector4d q_star(0, 0, 0, 0); + CspaceFreePolytopeTester tester(plant_, scene_graph_, + SeparatingPlaneOrder::kAffine, q_star); + Eigen::Matrix C; + // clang-format off + C << 1, 1, 0, 0, + -1, -1, 0, 1, + -1, 0, 1, -1, + 1, 0, -1, 0, + 0, 1, 1, 0, + 0, -1, -1, 1, + 1, 0, 1, 0, + 1, 1, -1, 1, + 1, -1, 1, -1, + 0, 1, 1, 1, + 1, 1, 1, -1, + 1, 0, 0, 1; + // clang-format on + Eigen::Matrix d; + d << 0.1, 0.1, 0.1, 0.02, 0.02, 0.2, 0.1, 0.1, 0.2, 0.1, 0.1, 0.02; + for (int i = 0; i < C.rows(); ++i) { + const double C_row_norm = C.row(i).norm(); + C.row(i) = C.row(i) / C_row_norm; + d(i) = d(i) / C_row_norm; + } + + CspaceFreePolytope::IgnoredCollisionPairs ignored_collision_pairs{}; + CspaceFreePolytope::FindSeparationCertificateGivenPolytopeOptions options; + options.verbose = false; + solvers::MosekSolver solver; + options.solver_id = solver.id(); + if (solver.available()) { + const auto certificates_result = + tester.FindSeparationCertificateGivenPolytope(ignored_collision_pairs, + C, d, options); + ASSERT_TRUE(std::all_of( + certificates_result.begin(), certificates_result.end(), + [](const std::optional& + certificate) { return certificate.has_value(); })); + + for (bool search_s_bounds_lagrangians : {false, true}) { + const int gram_total_size = tester.GetGramVarSizeForPolytopeSearchProgram( + ignored_collision_pairs, search_s_bounds_lagrangians); + MatrixX C_var(C.rows(), C.cols()); + VectorX d_var(d.rows()); + for (int i = 0; i < C.rows(); ++i) { + d_var(i) = symbolic::Variable("d" + std::to_string(i)); + for (int j = 0; j < C.cols(); ++j) { + C_var(i, j) = symbolic::Variable(fmt::format("C({},{})", i, j)); } - const double b_val = b.at(plane_index).Evaluate(env); - EXPECT_GE( - DistanceToHalfspace(*plane.positive_side_geometry, a_val, b_val, - plane.expressed_body, PlaneSide::kPositive, - plant, plant_context), - 0); - EXPECT_GE( - DistanceToHalfspace(*plane.negative_side_geometry, a_val, b_val, - plane.expressed_body, PlaneSide::kNegative, - plant, plant_context), - 0); + } + const VectorX d_minus_Cs = + tester.CalcDminusCs(C_var, d_var); + + std::unordered_map + new_certificates; + auto prog = tester.InitializePolytopeSearchProgram( + ignored_collision_pairs, C_var, d_var, d_minus_Cs, + certificates_result, search_s_bounds_lagrangians, gram_total_size, + &new_certificates); + solvers::SolverOptions solver_options; + solver_options.SetOption(solvers::CommonSolverOption::kPrintToConsole, 0); + const auto result = solver.Solve(*prog, std::nullopt, solver_options); + ASSERT_TRUE(result.is_success()); + const auto C_sol = result.GetSolution(C_var); + const auto d_sol = result.GetSolution(d_var); + CheckPolytopeSearchResult(tester, C_sol, d_sol, result, + certificates_result, new_certificates, + search_s_bounds_lagrangians, 2E-2); + } + } +} + +class CIrisToyRobotInitializePolytopeSearchProgramTest + : public CIrisToyRobotTest { + public: + void Test(bool with_cross_y, + const std::vector& search_s_bounds_lagrangians_options) { + const Eigen::Vector3d q_star(0, 0, 0); + CspaceFreePolytope::Options cspace_free_polytope_options; + cspace_free_polytope_options.with_cross_y = with_cross_y; + CspaceFreePolytopeTester tester(plant_, scene_graph_, + SeparatingPlaneOrder::kAffine, q_star, + cspace_free_polytope_options); + Eigen::Matrix C; + // clang-format off + C << 1, 1, 0, + -1, -1, 0, + -1, 0, 1, + 1, 0, -1, + 0, 1, 1, + 0, -1, -1, + 1, 0, 1, + 1, 1, -1, + 1, -1, 1; + // clang-format on + Eigen::Matrix d; + d << 0.1, 0.1, 0.1, 0.02, 0.02, 0.2, 0.1, 0.1, 0.2; + for (int i = 0; i < C.rows(); ++i) { + const double C_row_norm = C.row(i).norm(); + C.row(i) = C.row(i) / C_row_norm; + d(i) = d(i) / C_row_norm; + } + + CspaceFreePolytope::IgnoredCollisionPairs ignored_collision_pairs{ + SortedPair(world_box_, body2_sphere_), + SortedPair(world_box_, body2_capsule_), + SortedPair(body0_box_, body2_capsule_)}; + CspaceFreePolytope::FindSeparationCertificateGivenPolytopeOptions options; + options.verbose = false; + solvers::MosekSolver solver; + options.solver_id = solver.id(); + options.solver_options = solvers::SolverOptions(); + options.solver_options->SetOption( + solvers::CommonSolverOption::kPrintToConsole, 0); + if (solver.available()) { + const auto certificates_result = + tester.FindSeparationCertificateGivenPolytope(ignored_collision_pairs, + C, d, options); + ASSERT_TRUE(std::all_of( + certificates_result.begin(), certificates_result.end(), + [](const std::optional< + CspaceFreePolytope::SeparationCertificateResult>& certificate) { + return certificate.has_value(); + })); + + for (bool search_s_bounds_lagrangians : + search_s_bounds_lagrangians_options) { + const int gram_total_size = + tester.GetGramVarSizeForPolytopeSearchProgram( + ignored_collision_pairs, search_s_bounds_lagrangians); + MatrixX C_var(C.rows(), C.cols()); + VectorX d_var(d.rows()); + for (int i = 0; i < C.rows(); ++i) { + d_var(i) = symbolic::Variable("d" + std::to_string(i)); + for (int j = 0; j < C.cols(); ++j) { + C_var(i, j) = symbolic::Variable(fmt::format("C({},{})", i, j)); + } + } + const VectorX d_minus_Cs = + tester.CalcDminusCs(C_var, d_var); + + std::unordered_map + new_certificates; + auto prog = tester.InitializePolytopeSearchProgram( + ignored_collision_pairs, C_var, d_var, d_minus_Cs, + certificates_result, search_s_bounds_lagrangians, gram_total_size, + &new_certificates); + solvers::SolverOptions solver_options; + solver_options.SetOption(solvers::CommonSolverOption::kPrintToConsole, + 0); + const auto result = solver.Solve(*prog, std::nullopt, solver_options); + ASSERT_TRUE(result.is_success()); + const auto C_sol = result.GetSolution(C_var); + const auto d_sol = result.GetSolution(d_var); + CheckPolytopeSearchResult(tester, C_sol, d_sol, result, + certificates_result, new_certificates, + search_s_bounds_lagrangians, 1E-3); } } } +}; + +TEST_F(CIrisToyRobotInitializePolytopeSearchProgramTest, WithoutCrossY) { + Test(false /* with_cross_y */, + {true} /* search_s_bounds_lagrangians_options */); +} + +TEST_F(CIrisToyRobotInitializePolytopeSearchProgramTest, WithCrossY) { + Test(true /* with_cross_y */, + {true} /* search_s_bounds_lagrangians_options */); } TEST_F(CIrisToyRobotTest, FindPolytopeGivenLagrangian) { @@ -1242,21 +1373,26 @@ TEST_F(CIrisToyRobotTest, FindPolytopeGivenLagrangian) { } CspaceFreePolytope::IgnoredCollisionPairs ignored_collision_pairs{ - SortedPair(world_box_, body2_sphere_)}; + SortedPair(world_box_, body2_sphere_), + SortedPair(world_box_, body2_capsule_), + SortedPair(body0_box_, body2_capsule_), + }; CspaceFreePolytope::FindSeparationCertificateGivenPolytopeOptions options; - options.verbose = true; + options.verbose = false; solvers::MosekSolver solver; options.solver_id = solver.id(); - options.backoff_scale = 0.01; if (solver.available()) { - for (bool search_s_bounds_lagrangians : {false, true}) { - options.num_threads = -1; + options.num_threads = -1; - const auto certificates_result = - tester.FindSeparationCertificateGivenPolytope( - ignored_collision_pairs, C, d, - true /* search_separating_margin */, options); + const auto certificates_result = + tester.FindSeparationCertificateGivenPolytope(ignored_collision_pairs, + C, d, options); + ASSERT_TRUE(std::all_of( + certificates_result.begin(), certificates_result.end(), + [](const std::optional& + certificate) { return certificate.has_value(); })); + for (bool search_s_bounds_lagrangians : {true}) { const int gram_total_size = tester.GetGramVarSizeForPolytopeSearchProgram( ignored_collision_pairs, search_s_bounds_lagrangians); MatrixX C_var(C.rows(), C.cols()); @@ -1275,24 +1411,65 @@ TEST_F(CIrisToyRobotTest, FindPolytopeGivenLagrangian) { ellipsoid_margins(i) = symbolic::Variable("ellipsoid_margin" + std::to_string(i)); } - CspaceFreePolytope::FindPolytopeGivenLagrangianOptions polytope_options; polytope_options.solver_options.emplace(solvers::SolverOptions{}); polytope_options.solver_options->SetOption( solvers::CommonSolverOption::kPrintToConsole, 0); polytope_options.search_s_bounds_lagrangians = search_s_bounds_lagrangians; + polytope_options.backoff_scale = 0.05; + polytope_options.ellipsoid_margin_epsilon = 1E-4; + polytope_options.ellipsoid_margin_cost = + CspaceFreePolytope::EllipsoidMarginCost::kGeometricMean; const HPolyhedron cspace_h_polyhedron = tester.GetPolyhedronWithJointLimits(C, d); const auto ellipsoid = cspace_h_polyhedron.MaximumVolumeInscribedEllipsoid(); - const Eigen::MatrixXd ellipsoid_Q = ellipsoid.A().inverse(); + const Eigen::MatrixXd ellipsoid_Q = 0.98 * ellipsoid.A().inverse(); + std::unordered_map + new_certificates_result; const auto polytope_result = tester.FindPolytopeGivenLagrangian( ignored_collision_pairs, C_var, d_var, d_minus_Cs, certificates_result, ellipsoid_Q, ellipsoid.center(), - ellipsoid_margins, gram_total_size, polytope_options); + ellipsoid_margins, gram_total_size, polytope_options, + &new_certificates_result); ASSERT_TRUE(polytope_result.has_value()); + // Check the new separation certificates. + EXPECT_EQ(new_certificates_result.size(), + tester.cspace_free_polytope().separating_planes().size() - + ignored_collision_pairs.size()); + for (const auto& [plane_index, certificate_result] : + new_certificates_result) { + const auto& plane_geometries = tester.plane_geometries()[plane_index]; + const auto& plane = + tester.cspace_free_polytope().separating_planes()[plane_index]; + CheckRationalsPositiveInCspacePolytope( + plane_geometries.positive_side_rationals, + certificate_result.positive_side_rational_lagrangians, + plane.decision_variables, + certificate_result.plane_decision_var_vals, polytope_result->C, + polytope_result->d, tester, 0); + CheckRationalsPositiveInCspacePolytope( + plane_geometries.negative_side_rationals, + certificate_result.negative_side_rational_lagrangians, + plane.decision_variables, + certificate_result.plane_decision_var_vals, polytope_result->C, + polytope_result->d, tester, 0); + CheckRationalsPositiveInCspacePolytope( + plane_geometries.positive_side_psd_mat, + certificate_result.positive_side_psd_mat_lagrangians, + plane.decision_variables, + certificate_result.plane_decision_var_vals, polytope_result->C, + polytope_result->d, tester, 2E-4); + CheckRationalsPositiveInCspacePolytope( + plane_geometries.negative_side_psd_mat, + certificate_result.negative_side_psd_mat_lagrangians, + plane.decision_variables, + certificate_result.plane_decision_var_vals, polytope_result->C, + polytope_result->d, tester, 2E-4); + } + // Now check if the C-space polytope {s | C*s<=d, s_lower<=s<=s_upper} is // collision free. // First sample many s values, and project these values into the polytope @@ -1311,6 +1488,12 @@ TEST_F(CIrisToyRobotTest, FindPolytopeGivenLagrangian) { 0, 0, 0, 0.2, -1.5, 1; // clang-format on + EXPECT_EQ(polytope_result->a.size(), + tester.cspace_free_polytope().separating_planes().size() - + ignored_collision_pairs.size()); + EXPECT_EQ(polytope_result->a.size(), + tester.cspace_free_polytope().separating_planes().size() - + ignored_collision_pairs.size()); CheckSeparationBySamples(tester, *diagram_, s_samples, polytope_result->C, polytope_result->d, polytope_result->a, polytope_result->b, q_star, @@ -1327,14 +1510,27 @@ TEST_F(CIrisToyRobotTest, FindPolytopeGivenLagrangian) { polytope_result->C.row(i).norm(), polytope_result->ellipsoid_margins(i)); } - // Check that the inequality constraint |Qcᵢ|₂ ≤ dᵢ − δᵢ − cᵢᵀs₀ is active - // at the optimal solution. - EXPECT_TRUE(CompareMatrices( - (ellipsoid_Q * polytope_result->C.transpose()).colwise().norm(), - (polytope_result->d - polytope_result->ellipsoid_margins - - polytope_result->C * ellipsoid.center()) - .transpose(), - 1E-5)); + if (polytope_options.backoff_scale.has_value()) { + // Check the inequality constraint |Qcᵢ|₂ ≤ dᵢ − δᵢ − cᵢᵀs₀ is satisfied + EXPECT_TRUE(((ellipsoid_Q * polytope_result->C.transpose()) + .colwise() + .norm() + .transpose() + .array() <= + (polytope_result->d - polytope_result->ellipsoid_margins - + polytope_result->C * ellipsoid.center()) + .array()) + .all()); + } else { + // Check that the inequality constraint |Qcᵢ|₂ ≤ dᵢ − δᵢ − cᵢᵀs₀ is + // active at the optimal solution. + EXPECT_TRUE(CompareMatrices( + (ellipsoid_Q * polytope_result->C.transpose()).colwise().norm(), + (polytope_result->d - polytope_result->ellipsoid_margins - + polytope_result->C * ellipsoid.center()) + .transpose(), + 1E-5)); + } // Check that the norm of each row in C is <= 1. EXPECT_TRUE((polytope_result->C.rowwise().norm().array() <= 1).all()); } @@ -1367,14 +1563,23 @@ TEST_F(CIrisToyRobotTest, SearchWithBilinearAlternation) { CspaceFreePolytope::BilinearAlternationOptions bilinear_alternation_options; bilinear_alternation_options.max_iter = 5; bilinear_alternation_options.convergence_tol = 1E-5; + bilinear_alternation_options.find_polytope_options.solver_options = + solvers::SolverOptions(); + bilinear_alternation_options.find_polytope_options.solver_options->SetOption( + solvers::CommonSolverOption::kPrintToConsole, 0); + bilinear_alternation_options.find_polytope_options.backoff_scale = 0.04; + bilinear_alternation_options.find_polytope_options + .search_s_bounds_lagrangians = true; + bilinear_alternation_options.ellipsoid_scaling = 1; solvers::MosekSolver solver; if (solver.available()) { - auto bilinear_alternation_result = + auto bilinear_alternation_results = tester.cspace_free_polytope().SearchWithBilinearAlternation( - ignored_collision_pairs, C, d, true /* search_margin */, - bilinear_alternation_options); - ASSERT_TRUE(bilinear_alternation_result.has_value()); + ignored_collision_pairs, C, d, bilinear_alternation_options); + ASSERT_FALSE(bilinear_alternation_results.empty()); + ASSERT_EQ(bilinear_alternation_results.size(), + bilinear_alternation_results.back().num_iter + 1); // Sample many s_values, project to {s | C*s <= d}. And then make sure that // the corresponding configurations are collision free. Eigen::Matrix s_samples; @@ -1390,10 +1595,26 @@ TEST_F(CIrisToyRobotTest, SearchWithBilinearAlternation) { 0, 0, 0, 0.2, -1.5, 1; // clang-format on - CheckSeparationBySamples( - tester, *diagram_, s_samples, bilinear_alternation_result->C, - bilinear_alternation_result->d, bilinear_alternation_result->a, - bilinear_alternation_result->b, q_star, ignored_collision_pairs); + for (const auto& result : bilinear_alternation_results) { + EXPECT_EQ(result.a.size(), + tester.cspace_free_polytope().separating_planes().size() - + ignored_collision_pairs.size()); + EXPECT_EQ(result.a.size(), + tester.cspace_free_polytope().separating_planes().size() - + ignored_collision_pairs.size()); + CheckSeparationBySamples(tester, *diagram_, s_samples, result.C, result.d, + result.a, result.b, q_star, + ignored_collision_pairs); + } + + // Test the case that bilinear alternation fails. + // The initial C-space polytope is too huge. + auto bilinear_alternation_results_failure = + tester.cspace_free_polytope().SearchWithBilinearAlternation( + ignored_collision_pairs, C, + d + Eigen::VectorXd::Constant(d.rows(), 100), + bilinear_alternation_options); + EXPECT_TRUE(bilinear_alternation_results_failure.empty()); } } @@ -1422,7 +1643,8 @@ TEST_F(CIrisToyRobotTest, BinarySearch) { CspaceFreePolytope::BinarySearchOptions options; options.scale_min = 1; - options.scale_max = 3; + options.scale_max = 10; + options.convergence_tol = 1E-1; solvers::MosekSolver solver; if (solver.available()) { const Eigen::Vector3d s_center(0.01, 0, 0.01); @@ -1443,6 +1665,12 @@ TEST_F(CIrisToyRobotTest, BinarySearch) { 0, 0, 0, 0.2, -1.5, 1; // clang-format on + EXPECT_EQ(result->a.size(), + tester.cspace_free_polytope().separating_planes().size() - + ignored_collision_pairs.size()); + EXPECT_EQ(result->a.size(), + tester.cspace_free_polytope().separating_planes().size() - + ignored_collision_pairs.size()); CheckSeparationBySamples(tester, *diagram_, s_samples, result->C, result->d, result->a, result->b, q_star, ignored_collision_pairs); @@ -1458,13 +1686,19 @@ TEST_F(CIrisToyRobotTest, BinarySearch) { EXPECT_TRUE(CompareMatrices( result->d, options.scale_max * d + (1 - options.scale_max) * C * s_center, 1E-10)); + EXPECT_EQ(result->a.size(), + tester.cspace_free_polytope().separating_planes().size() - + ignored_collision_pairs.size()); + EXPECT_EQ(result->a.size(), + tester.cspace_free_polytope().separating_planes().size() - + ignored_collision_pairs.size()); CheckSeparationBySamples(tester, *diagram_, s_samples, result->C, result->d, result->a, result->b, q_star, ignored_collision_pairs); // Now check infeasible epsilon_min - options.scale_min = 10; - options.scale_max = 20; + options.scale_min = 20; + options.scale_max = 30; result = tester.cspace_free_polytope().BinarySearch( ignored_collision_pairs, C, d, s_center, options); ASSERT_FALSE(result.has_value());