From 5f1433e751153fdfe7341aef233d5b1eb6df9fe7 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Tue, 1 Aug 2023 16:36:07 -0700 Subject: [PATCH] Construct MinimumDistanceConstraint with a CollisionChecker. (#19760) We can now compute the distance through CollisionChecker, and then impose bounds on the minimum distance. --- multibody/inverse_kinematics/BUILD.bazel | 3 + .../minimum_distance_constraint.cc | 118 ++++++++++++++-- .../minimum_distance_constraint.h | 59 +++++++- .../test/inverse_kinematics_test_utilities.cc | 60 ++++++-- .../test/inverse_kinematics_test_utilities.h | 39 ++++-- .../test/minimum_distance_constraint_test.cc | 128 ++++++++++++++++++ 6 files changed, 378 insertions(+), 29 deletions(-) diff --git a/multibody/inverse_kinematics/BUILD.bazel b/multibody/inverse_kinematics/BUILD.bazel index 18ca2e8512a5..b1a8ebd9e94f 100644 --- a/multibody/inverse_kinematics/BUILD.bazel +++ b/multibody/inverse_kinematics/BUILD.bazel @@ -104,6 +104,7 @@ drake_cc_library( "//math:geometric_transform", "//math:gradient", "//multibody/plant", + "//planning:collision_checker", "//solvers:constraint", "//solvers:mathematical_program", "//solvers:minimum_value_constraint", @@ -206,6 +207,7 @@ drake_cc_library( "//multibody/parsing", "//multibody/plant", "//multibody/tree", + "//planning:robot_diagram_builder", "@gtest//:without_main", ], ) @@ -290,6 +292,7 @@ drake_cc_googletest( ":inverse_kinematics_test_utilities", ":kinematic_evaluators", "//common/test_utilities:expect_throws_message", + "//planning:scene_graph_collision_checker", "//solvers/test_utilities", ], ) diff --git a/multibody/inverse_kinematics/minimum_distance_constraint.cc b/multibody/inverse_kinematics/minimum_distance_constraint.cc index 69f430fb9b9f..22b94fb5c081 100644 --- a/multibody/inverse_kinematics/minimum_distance_constraint.cc +++ b/multibody/inverse_kinematics/minimum_distance_constraint.cc @@ -67,13 +67,32 @@ VectorX Distances(const MultibodyPlant& plant, return distances; } -template -void MinimumDistanceConstraint::Initialize( - const MultibodyPlant& plant, systems::Context* plant_context, +Eigen::VectorXd Distances( + const planning::CollisionChecker& collision_checker, + planning::CollisionCheckerContext* collision_checker_context, + const Eigen::Ref& x, double influence_distance_val) { + return collision_checker + .CalcContextRobotClearance(collision_checker_context, x, + influence_distance_val) + .distances(); +} + +AutoDiffVecXd Distances( + const planning::CollisionChecker& collision_checker, + planning::CollisionCheckerContext* collision_checker_context, + const Eigen::Ref& x, double influence_distance_val) { + const planning::RobotClearance robot_clearance = + collision_checker.CalcContextRobotClearance(collision_checker_context, + math::ExtractValue(x), + influence_distance_val); + return math::InitializeAutoDiff( + robot_clearance.distances(), + robot_clearance.jacobians() * math::ExtractGradient(x)); +} + +void MinimumDistanceConstraint::CheckMinimumDistanceBounds( double minimum_distance_lower, double minimum_distance_upper, - double influence_distance, - MinimumDistancePenaltyFunction penalty_function) { - CheckPlantIsConnectedToSceneGraph(plant, *plant_context); + double influence_distance) const { if (!std::isfinite(influence_distance)) { throw std::invalid_argument( "MinimumDistanceConstraint: influence_distance must be finite."); @@ -94,6 +113,17 @@ void MinimumDistanceConstraint::Initialize( "minimum_distance_upper={}.", influence_distance, minimum_distance_upper)); } +} + +template +void MinimumDistanceConstraint::Initialize( + const MultibodyPlant& plant, systems::Context* plant_context, + double minimum_distance_lower, double minimum_distance_upper, + double influence_distance, + MinimumDistancePenaltyFunction penalty_function) { + CheckPlantIsConnectedToSceneGraph(plant, *plant_context); + CheckMinimumDistanceBounds(minimum_distance_lower, minimum_distance_upper, + influence_distance); const auto& query_port = plant.get_geometry_query_input_port(); // Maximum number of SignedDistancePairs returned by calls to // ComputeSignedDistancePairwiseClosestPoints(). @@ -120,6 +150,37 @@ void MinimumDistanceConstraint::Initialize( } } +void MinimumDistanceConstraint::Initialize( + const planning::CollisionChecker& collision_checker, + planning::CollisionCheckerContext* collision_checker_context, + double minimum_distance_lower, double minimum_distance_upper, + double influence_distance, + MinimumDistancePenaltyFunction penalty_function) { + CheckMinimumDistanceBounds(minimum_distance_lower, minimum_distance_upper, + influence_distance); + minimum_value_constraint_ = std::make_unique( + collision_checker.plant().num_positions(), minimum_distance_lower, + minimum_distance_upper, influence_distance, + collision_checker.MaxContextNumDistances(*collision_checker_context), + [this](const Eigen::Ref& x, + double influence_distance_val) { + return Distances(*(this->collision_checker_), + this->collision_checker_context_, x, + influence_distance_val); + }, + [this](const Eigen::Ref& x, + double influence_distance_val) { + return Distances(*(this->collision_checker_), + this->collision_checker_context_, x, + influence_distance_val); + }); + this->set_bounds(minimum_value_constraint_->lower_bound(), + minimum_value_constraint_->upper_bound()); + if (penalty_function) { + minimum_value_constraint_->set_penalty_function(penalty_function); + } +} + MinimumDistanceConstraint::MinimumDistanceConstraint( const multibody::MultibodyPlant* const plant, double minimum_distance, systems::Context* plant_context, @@ -148,7 +209,8 @@ MinimumDistanceConstraint::MinimumDistanceConstraint( plant_double_{plant}, plant_context_double_{plant_context}, plant_autodiff_{nullptr}, - plant_context_autodiff_{nullptr} { + plant_context_autodiff_{nullptr}, + collision_checker_{nullptr} { Initialize(*plant_double_, plant_context_double_, minimum_distance_lower, minimum_distance_upper, influence_distance_offset, penalty_function); @@ -179,11 +241,51 @@ MinimumDistanceConstraint::MinimumDistanceConstraint( plant_double_{nullptr}, plant_context_double_{nullptr}, plant_autodiff_{plant}, - plant_context_autodiff_{plant_context} { + plant_context_autodiff_{plant_context}, + collision_checker_{nullptr} { Initialize(*plant_autodiff_, plant_context_autodiff_, minimum_distance_lower, minimum_distance_upper, influence_distance, penalty_function); } +MinimumDistanceConstraint::MinimumDistanceConstraint( + const planning::CollisionChecker* collision_checker, + double minimum_distance_lower, + planning::CollisionCheckerContext* collision_checker_context, + MinimumDistancePenaltyFunction penalty_function, + double influence_distance_offset) + : MinimumDistanceConstraint( + collision_checker, minimum_distance_lower, + kInf /* minimum_distance_upper */, collision_checker_context, + penalty_function, + minimum_distance_lower + influence_distance_offset) {} + +MinimumDistanceConstraint::MinimumDistanceConstraint( + const planning::CollisionChecker* collision_checker, + double minimum_distance_lower, double minimum_distance_upper, + planning::CollisionCheckerContext* collision_checker_context, + MinimumDistancePenaltyFunction penalty_function, double influence_distance) + : solvers::Constraint( + NumConstraints(minimum_distance_lower, minimum_distance_upper), + internal::PtrOrThrow( + collision_checker, + "MinimumDistanceConstraint: collision_checker is nullptr") + ->plant() + .num_positions(), + Eigen::VectorXd::Zero( + NumConstraints(minimum_distance_lower, minimum_distance_upper)), + Eigen::VectorXd::Zero( + NumConstraints(minimum_distance_lower, minimum_distance_upper))), + plant_double_{nullptr}, + plant_context_double_{nullptr}, + plant_autodiff_{nullptr}, + plant_context_autodiff_{nullptr}, + collision_checker_{collision_checker}, + collision_checker_context_{collision_checker_context} { + Initialize(*collision_checker_, collision_checker_context_, + minimum_distance_lower, minimum_distance_upper, influence_distance, + penalty_function); +} + template void MinimumDistanceConstraint::DoEvalGeneric( const Eigen::Ref>& x, VectorX* y) const { diff --git a/multibody/inverse_kinematics/minimum_distance_constraint.h b/multibody/inverse_kinematics/minimum_distance_constraint.h index 2543bab34c30..3ebf5483fb7a 100644 --- a/multibody/inverse_kinematics/minimum_distance_constraint.h +++ b/multibody/inverse_kinematics/minimum_distance_constraint.h @@ -4,6 +4,7 @@ #include #include "drake/multibody/plant/multibody_plant.h" +#include "drake/planning/collision_checker.h" #include "drake/solvers/constraint.h" #include "drake/solvers/minimum_value_constraint.h" #include "drake/systems/framework/context.h" @@ -56,8 +57,8 @@ configuration vector, q, of the associated MultibodyPlant. The formulation of the constraint is -SmoothOverMax( φ((dᵢ(q) - d_influence)/(d_influence - lb)) / φ(-1) ) ≤ 1 -SmoothUnderMax( φ((dᵢ(q) - d_influence)/(d_influence - ub)) / φ(-1) ) ≥ 1 + SmoothOverMax( φ((dᵢ(q) - d_influence)/(d_influence - lb)) / φ(-1) ) ≤ 1 + SmoothUnderMax( φ((dᵢ(q) - d_influence)/(d_influence - ub)) / φ(-1) ) ≥ 1 where dᵢ(q) is the signed distance of the i-th pair, lb is the minimum allowable distance, d_influence is the "influence distance" (the distance below @@ -87,7 +88,14 @@ class MinimumDistanceConstraint final : public solvers::Constraint { influence_distance_offset. This value must be finite and strictly positive, as it is used to scale the signed distances between pairs of geometries. Smaller values may improve performance, as fewer pairs of geometries need to be - considered in each constraint evaluation. @default 1 meter + considered in each constraint evaluation. @default 1 meter. + The chosen influence_distance_offset can significantly affect the runtime and + optimization performance of using this constraint. Larger values result in + more expensive collision checks (since more potential collision candidates + must be considered) and may result in worse optimization performance (the + optimizer may not be able to find a configuration that satisfies the + constraint). In work at TRI, we have used much lower values (e.g. 1e-6) for + influence_distance_offset with good results. @throws std::exception if `plant` has not registered its geometry with a SceneGraph object. @throws std::exception if influence_distance_offset = ∞. @@ -136,6 +144,7 @@ class MinimumDistanceConstraint final : public solvers::Constraint { @param minimum_distance_upper The upper bound of the minimum distance. min(distance) <= upper. If minimum_distance_upper is finite, then it must be smaller than influence_distance. + @param collision_checker_context The context for the collision checker. @pydrake_mkdoc_identifier{autodiff_with_upper_bound} */ MinimumDistanceConstraint( @@ -145,6 +154,34 @@ class MinimumDistanceConstraint final : public solvers::Constraint { MinimumDistancePenaltyFunction penalty_function, double influence_distance); + /** Overloaded constructor. + Constructs the constraint with CollisionChecker instead of MultibodyPlant. + @param collision_checker collision_checker must outlive this constraint. + @param collision_checker_context The context for the collision checker. See + CollisionChecker class for more details. + @pydrake_mkdoc_identifier{collision_checker_no_upper_bound} + */ + MinimumDistanceConstraint( + const planning::CollisionChecker* collision_checker, + double minimum_distance, + planning::CollisionCheckerContext* collision_checker_context, + MinimumDistancePenaltyFunction penalty_function = {}, + double influence_distance_offset = 1); + + /** Overloaded constructor. + Constructs the constraint with CollisionChecker instead of MultibodyPlant. + @param collision_checker collision_checker must outlive this constraint. + @param collision_checker_context The context for the collision checker. See + CollisionChecker class for more details. + @pydrake_mkdoc_identifier{collision_checker_with_upper_bound} + */ + MinimumDistanceConstraint( + const planning::CollisionChecker* collision_checker, + double minimum_distance_lower, double minimum_distance_upper, + planning::CollisionCheckerContext* collision_checker_context, + MinimumDistancePenaltyFunction penalty_function, + double influence_distance); + ~MinimumDistanceConstraint() override {} /** Getter for the minimum distance. */ @@ -190,14 +227,28 @@ class MinimumDistanceConstraint final : public solvers::Constraint { void Initialize(const MultibodyPlant& plant, systems::Context* plant_context, double minimum_distance_lower, double minimum_distance_upper, - double influence_distance_offset, + double influence_distance, + MinimumDistancePenaltyFunction penalty_function); + + // Overload Initialize with CollisionChecker instead of MultibodyPlant. + void Initialize(const planning::CollisionChecker& collision_checker, + planning::CollisionCheckerContext* collision_checker_context, + double minimum_distance_lower, double minimum_distance_upper, + double influence_distance, MinimumDistancePenaltyFunction penalty_function); + void CheckMinimumDistanceBounds(double minimum_distance_lower, + double minimum_distance_upper, + double influence_distance) const; + const multibody::MultibodyPlant* const plant_double_; systems::Context* const plant_context_double_; std::unique_ptr minimum_value_constraint_; const multibody::MultibodyPlant* const plant_autodiff_; systems::Context* const plant_context_autodiff_; + + const planning::CollisionChecker* collision_checker_; + planning::CollisionCheckerContext* collision_checker_context_; }; } // namespace multibody } // namespace drake diff --git a/multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.cc b/multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.cc index bd0a09a0600e..4d264566e054 100644 --- a/multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.cc +++ b/multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.cc @@ -3,8 +3,12 @@ #include #include +#include + #include "drake/common/default_scalars.h" #include "drake/common/find_resource.h" +#include "drake/geometry/geometry_roles.h" +#include "drake/geometry/proximity_properties.h" #include "drake/multibody/parsing/parser.h" #include "drake/multibody/plant/coulomb_friction.h" #include "drake/systems/framework/diagram_builder.h" @@ -140,6 +144,48 @@ TwoFreeSpheresTest::TwoFreeSpheresTest() { *plant_autodiff_, diagram_context_autodiff_.get())); } +SpheresAndWallsTest::SpheresAndWallsTest() : builder_{} { + builder_.plant().set_name("spheres_and_walls"); + geometry::ProximityProperties proximity_properties{}; + // Kinematics doesn't care about robot dynamics. Use arbitrary material + // properties. + AddContactMaterial(0.1, 250, multibody::CoulombFriction{0.9, 0.5}, + &proximity_properties); + left_wall_ = builder_.plant().RegisterCollisionGeometry( + builder_.plant().world_body(), + math::RigidTransformd(Eigen::Vector3d(-(wall_length_ / 2 + 0.05), 0, 0)), + geometry::Box(0.1, wall_length_, 1), "left_wall", proximity_properties); + right_wall_ = builder_.plant().RegisterCollisionGeometry( + builder_.plant().world_body(), + math::RigidTransformd(Eigen::Vector3d(wall_length_ / 2 + 0.05, 0, 0)), + geometry::Box(0.1, wall_length_, 1), "right_wall", proximity_properties); + top_wall_ = builder_.plant().RegisterCollisionGeometry( + builder_.plant().world_body(), + math::RigidTransformd(Eigen::Vector3d(0, wall_length_ / 2 + 0.05, 0)), + geometry::Box(wall_length_, 0.1, 1), "top_wall", proximity_properties); + bottom_wall_ = builder_.plant().RegisterCollisionGeometry( + builder_.plant().world_body(), + math::RigidTransformd(Eigen::Vector3d(0, -(wall_length_ / 2 + 0.05), 0)), + geometry::Box(wall_length_, 0.1, 1), "bottom_wall", proximity_properties); + // Use arbitrary inertia. + const multibody::SpatialInertia spatial_inertia( + 1, Eigen::Vector3d::Zero(), + multibody::UnitInertia(0.001, 0.001, 0.001)); + + for (int i = 0; i < static_cast(body_indices_.size()); ++i) { + body_indices_[i] = + builder_.plant() + .AddRigidBody(fmt::format("body{}", i), spatial_inertia) + .index(); + spheres_[i] = builder_.plant().RegisterCollisionGeometry( + builder_.plant().get_body(body_indices_[i]), + math::RigidTransformd(Eigen::Vector3d::Zero()), + geometry::Sphere(radius_), fmt::format("sphere{}", i), + proximity_properties); + } + builder_.plant().Finalize(); +} + template std::unique_ptr> ConstructBoxSphereDiagram( const Eigen::Vector3d& box_size, double radius, @@ -158,9 +204,9 @@ std::unique_ptr> ConstructBoxSphereDiagram( const auto& sphere = (*plant)->AddRigidBody( "sphere", SpatialInertia(1, Eigen::Vector3d::Zero(), UnitInertia(1, 1, 1))); - (*plant)->RegisterCollisionGeometry( - sphere, RigidTransformd::Identity(), geometry::Sphere(radius), "sphere", - CoulombFriction(0.9, 0.8)); + (*plant)->RegisterCollisionGeometry(sphere, RigidTransformd::Identity(), + geometry::Sphere(radius), "sphere", + CoulombFriction(0.9, 0.8)); *box_frame_index = box.body_frame().index(); *sphere_frame_index = sphere.body_frame().index(); @@ -213,8 +259,8 @@ T BoxSphereSignedDistance(const Eigen::Ref& box_size, return -(half_size - p_BS.array().abs()).minCoeff() - radius; } else { T signed_distance = 0; - using std::pow; using std::abs; + using std::pow; for (int i = 0; i < 3; ++i) { // Compute the distance from the sphere center box face along the i'th // dimension. @@ -227,10 +273,8 @@ T BoxSphereSignedDistance(const Eigen::Ref& box_size, } } -DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(( - &ConstructTwoFreeBodiesPlant, - &BoxSphereSignedDistance -)) +DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + (&ConstructTwoFreeBodiesPlant, &BoxSphereSignedDistance)) } // namespace multibody } // namespace drake diff --git a/multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.h b/multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.h index 2a3052e81c1a..72de5d7c88be 100644 --- a/multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.h +++ b/multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -14,6 +15,7 @@ #include "drake/math/rigid_transform.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/tree/multibody_tree.h" +#include "drake/planning/robot_diagram_builder.h" #include "drake/systems/framework/diagram.h" namespace drake { @@ -48,10 +50,10 @@ typename std::enable_if_t< std::is_same_v> CompareAutoDiffVectors(const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, double tol) { - EXPECT_TRUE(CompareMatrices(math::ExtractValue(a), - math::ExtractValue(b), tol)); - EXPECT_TRUE(CompareMatrices(math::ExtractGradient(a), - math::ExtractGradient(b), tol)); + EXPECT_TRUE( + CompareMatrices(math::ExtractValue(a), math::ExtractValue(b), tol)); + EXPECT_TRUE( + CompareMatrices(math::ExtractGradient(a), math::ExtractGradient(b), tol)); } /** @@ -137,6 +139,24 @@ class TwoFreeSpheresTest : public ::testing::Test { systems::Context* plant_context_autodiff_{nullptr}; }; +// We put 4 walls (each with length 1 meter) to form a square around the origin. +// We put spheres in this environment. +class SpheresAndWallsTest : public ::testing::Test { + public: + SpheresAndWallsTest(); + + protected: + double radius_{0.05}; + double wall_length_{1}; + planning::RobotDiagramBuilder builder_; + std::array body_indices_; + geometry::GeometryId left_wall_; + geometry::GeometryId right_wall_; + geometry::GeometryId top_wall_; + geometry::GeometryId bottom_wall_; + std::array spheres_; +}; + /** * Compute the signed distance between a box and a sphere. * @param box_size The size of the box. @@ -214,8 +234,7 @@ void TestKinematicConstraintEval( EXPECT_TRUE(CompareMatrices(y1_left, y1_right, tol)); // condition 2 - const auto x_autodiff = - math::InitializeAutoDiff(x_double, dx); + const auto x_autodiff = math::InitializeAutoDiff(x_double, dx); AutoDiffVecXd y2_left, y2_right; constraint_from_double.Eval(x_autodiff, &y2_left); constraint_from_autodiff.Eval(x_autodiff, &y2_right); @@ -234,9 +253,11 @@ void TestKinematicConstraintEval( // condition 4 std::function&, Eigen::VectorXd*)> - eval_fun = [&constraint_from_double]( - const Eigen::Ref& x, - Eigen::VectorXd* y) { constraint_from_double.Eval(x, y); }; + eval_fun = + [&constraint_from_double](const Eigen::Ref& x, + Eigen::VectorXd* y) { + constraint_from_double.Eval(x, y); + }; const auto dy_dx_numeric = math::ComputeNumericalGradient(eval_fun, x_double); const Eigen::MatrixXd y_grad_numeric = dy_dx_numeric * dx; EXPECT_TRUE(CompareMatrices(y_grad_numeric, math::ExtractGradient(y2_right), diff --git a/multibody/inverse_kinematics/test/minimum_distance_constraint_test.cc b/multibody/inverse_kinematics/test/minimum_distance_constraint_test.cc index 7dbaf8497c3f..4a82294a3fc0 100644 --- a/multibody/inverse_kinematics/test/minimum_distance_constraint_test.cc +++ b/multibody/inverse_kinematics/test/minimum_distance_constraint_test.cc @@ -6,6 +6,8 @@ #include "drake/math/compute_numerical_gradient.h" #include "drake/math/rigid_transform.h" #include "drake/multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.h" +#include "drake/planning/scene_graph_collision_checker.h" +#include "drake/solvers/minimum_value_constraint.h" #include "drake/solvers/test_utilities/check_constraint_eval_nonsymbolic.h" namespace drake { @@ -476,5 +478,131 @@ GTEST_TEST(ThreeSpheresTest, SomeLargerThanInfluenceSomeSmallerThanMinimum) { (y_val.array() > dut.upper_bound().array()).any()); } } + +TEST_F(SpheresAndWallsTest, TestWithCollisionChecker) { + // Test MinimumDistanceConstraint constructed with CollisionChecker. + planning::CollisionCheckerParams params; + params.model = builder_.Build(); + params.robot_model_instances.push_back(multibody::default_model_instance()); + auto quaternion_difference = [](const Eigen::Ref& q1, + const Eigen::Ref& q2) { + // Compute 1-cos(theta/2) where theta is the angle between the two + // quaternions. + return 1 - (Eigen::Quaterniond(q1(0), q1(1), q1(2), q1(3)).conjugate() * + Eigen::Quaterniond(q2(0), q2(1), q2(2), q2(3))) + .w(); + }; + params.configuration_distance_function = [quaternion_difference]( + const Eigen::VectorXd& q1, + const Eigen::VectorXd& q2) { + return quaternion_difference(q1.head<4>(), q2.head<4>()) + + quaternion_difference(q1.segment<4>(7), q2.segment<4>(7)) + + (q1.segment<3>(4) - q2.segment<3>(4)).norm() + + (q1.segment<3>(11) - q2.segment<3>(11)).norm(); + }; + params.edge_step_size = 0.05; + planning::SceneGraphCollisionChecker collision_checker(std::move(params)); + auto collision_checker_context = + collision_checker.MakeStandaloneModelContext(); + double influence_distance = 0.2; + auto check_constraint = [&collision_checker]( + const MinimumDistanceConstraint& dut, + const Eigen::Vector3d& p_WS1, + const Eigen::Vector3d& p_WS2, bool is_satisfied) { + Eigen::VectorXd q(collision_checker.plant().num_positions()); + // sphere 1 quaternion. + q.head<4>() << 1, 0, 0, 0; + q.segment<3>(4) = p_WS1; + // sphere 2 quaternion. + q.segment<4>(7) << 1, 0, 0, 0; + q.tail<3>() = p_WS2; + EXPECT_EQ(dut.CheckSatisfied(q), is_satisfied); + // Make sure the gradient is correct. + const AutoDiffVecXd q_ad = math::InitializeAutoDiff(q); + AutoDiffVecXd y_ad; + dut.Eval(q_ad, &y_ad); + + math::NumericalGradientOption options( + math::NumericalGradientMethod::kCentral); + const Eigen::MatrixXd grad_numeric = + math::ComputeNumericalGradient( + [&dut](const Eigen::VectorXd& x, Eigen::VectorXd* y) { + dut.Eval(x, y); + }, + q, options); + EXPECT_TRUE( + CompareMatrices(math::ExtractGradient(y_ad), grad_numeric, 1E-7)); + }; + + { + // We provide both minimum_distance_lower and minimum_distance_upper. + double minimum_distance_lower = 0.05; + double minimum_distance_upper = 0.15; + MinimumDistanceConstraint dut( + &collision_checker, minimum_distance_lower, minimum_distance_upper, + collision_checker_context.get(), + solvers::QuadraticallySmoothedHingeLoss, influence_distance); + EXPECT_EQ(dut.num_constraints(), 2); + EXPECT_EQ(dut.num_vars(), collision_checker.plant().num_positions()); + + // The two spheres almost coincide (do not set the two spheres to exactly + // coincide as the distance jacobian is ill defined at that point). + check_constraint(dut, Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(1E-4, 0, 0), + false); + // The two spheres are separated, but the distance is smaller than + // minimum_distance_lower + check_constraint( + dut, Eigen::Vector3d(0, 0, 0), + Eigen::Vector3d(0.9 * minimum_distance_lower + 2 * radius_, 0, 0), + false); + // The distance between the two spheres is between minimum_distance_lower + // and minimum_distance_upper. + check_constraint( + dut, Eigen::Vector3d(0, 0, 0), + Eigen::Vector3d( + (minimum_distance_lower + minimum_distance_upper) / 2 + 2 * radius_, + 0, 0), + true); + // The distance between the two spheres is above minimum_distance_upper. + check_constraint( + dut, Eigen::Vector3d(0, 0, 0), + Eigen::Vector3d(minimum_distance_upper + 2 * radius_ + 1E-3, 0, 0), + false); + // The distance between the two spheres is above minimum_distance_upper, but + // the distance between sphere 2 and the right wall is between + // minimum_distance_lower and minimum_distance_upper. + check_constraint( + dut, Eigen::Vector3d(0, 0, 0), + Eigen::Vector3d( + wall_length_ / 2 - + (minimum_distance_lower + minimum_distance_upper) / 2 - radius_, + 0, 0), + true); + } + + { + // Only specify minimum_distance_lower. + double minimum_distance = 0.05; + MinimumDistanceConstraint dut(&collision_checker, minimum_distance, + collision_checker_context.get()); + EXPECT_EQ(dut.num_constraints(), 1); + EXPECT_EQ(dut.num_vars(), collision_checker.plant().num_positions()); + + // The two spheres almost coincide (do not set the two spheres to exactly + // coincide as the distance jacobian is ill defined at that point). + check_constraint(dut, Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(1E-4, 0, 0), + false); + // The two spheres are separated, but the distance is smaller than + // minimum_distance_lower. + check_constraint( + dut, Eigen::Vector3d(0, 0, 0), + Eigen::Vector3d(0.9 * minimum_distance + 2 * radius_, 0, 0), false); + // The distance between the two spheres is above minimum_distance. + check_constraint( + dut, Eigen::Vector3d(0, 0, 0), + Eigen::Vector3d(1.1 * minimum_distance + 2 * radius_, 0, 0), true); + } +} } // namespace multibody } // namespace drake