From 33cbce4d308610786d5c185931b42dd43afe5c48 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Fri, 9 Oct 2020 07:08:48 -0700 Subject: [PATCH] Add python binding for static equlibrium problem. (#14140) Also use ComputeSignedDistancePairwiseClosestPoints() to avoid redundant computation. --- bindings/pydrake/multibody/BUILD.bazel | 30 +++ bindings/pydrake/multibody/all.py | 1 + bindings/pydrake/multibody/optimization_py.cc | 64 +++++++ .../multibody/test/optimization_test.py | 176 ++++++++++++++++++ multibody/optimization/contact_wrench.h | 16 +- .../static_equilibrium_constraint.cc | 29 +-- .../static_equilibrium_problem.cc | 57 +++--- ...riction_cone_complementarity_constraint.cc | 93 ++++----- .../test/static_equilibrium_problem_test.cc | 2 +- 9 files changed, 353 insertions(+), 115 deletions(-) create mode 100644 bindings/pydrake/multibody/optimization_py.cc create mode 100644 bindings/pydrake/multibody/test/optimization_test.py diff --git a/bindings/pydrake/multibody/BUILD.bazel b/bindings/pydrake/multibody/BUILD.bazel index 14d2d3d50f0b..f48dbab2eb04 100644 --- a/bindings/pydrake/multibody/BUILD.bazel +++ b/bindings/pydrake/multibody/BUILD.bazel @@ -148,6 +148,22 @@ drake_pybind_library( ], ) +drake_pybind_library( + name = "optimization_py", + cc_deps = [ + "//bindings/pydrake:documentation_pybind", + "//bindings/pydrake/common:default_scalars_pybind", + "//bindings/pydrake/common:value_pybind", + ], + cc_srcs = ["optimization_py.cc"], + package_info = PACKAGE_INFO, + py_deps = [ + ":module_py", + ":plant_py", + "//bindings/pydrake/solvers:mathematicalprogram_py", + ], +) + drake_jupyter_py_binary( name = "examples/door_hinge_inspector", add_test_rule = 1, @@ -161,6 +177,7 @@ PY_LIBRARIES_WITH_INSTALL = [ ":benchmarks_py", ":inverse_kinematics_py", ":math_py", + ":optimization_py", ":parsing_py", ":plant_py", ":tree_py", @@ -270,6 +287,19 @@ drake_py_unittest( ], ) +drake_py_unittest( + name = "optimization_test", + timeout = "moderate", + deps = [ + ":benchmarks_py", + ":inverse_kinematics_py", + ":optimization_py", + ":parsing_py", + "//bindings/pydrake/common/test_utilities", + "//bindings/pydrake/solvers:ipopt_py", + ], +) + add_pybind_coverage_data() add_lint_tests_pydrake() diff --git a/bindings/pydrake/multibody/all.py b/bindings/pydrake/multibody/all.py index b3ca962e92ec..86f586c747d7 100644 --- a/bindings/pydrake/multibody/all.py +++ b/bindings/pydrake/multibody/all.py @@ -3,6 +3,7 @@ # Normal symbols. from .inverse_kinematics import * # noqa from .math import * # noqa +from .optimization import * # noqa from .parsing import * # noqa from .plant import * # noqa from .tree import * # noqa diff --git a/bindings/pydrake/multibody/optimization_py.cc b/bindings/pydrake/multibody/optimization_py.cc new file mode 100644 index 000000000000..0b0bed2812d0 --- /dev/null +++ b/bindings/pydrake/multibody/optimization_py.cc @@ -0,0 +1,64 @@ +#include "pybind11/eigen.h" +#include "pybind11/pybind11.h" + +#include "drake/bindings/pydrake/common/default_scalars_pybind.h" +#include "drake/bindings/pydrake/common/value_pybind.h" +#include "drake/bindings/pydrake/documentation_pybind.h" +#include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/multibody/optimization/static_equilibrium_problem.h" + +namespace drake { +namespace pydrake { + +namespace { +PYBIND11_MODULE(optimization, m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::multibody; + constexpr auto& doc = pydrake_doc.drake.multibody; + + m.doc() = "Optimization module for MultibodyPlant motion planning"; + + py::module::import("pydrake.math"); + py::module::import("pydrake.multibody.plant"); + py::module::import("pydrake.solvers.mathematicalprogram"); + + { + py::class_(m, "ContactWrench", doc.ContactWrench.doc) + .def_readonly("bodyA_index", &ContactWrench::bodyA_index, + doc.ContactWrench.bodyA_index.doc) + .def_readonly("bodyB_index", &ContactWrench::bodyB_index, + doc.ContactWrench.bodyB_index.doc) + .def_readonly( + "p_WCb_W", &ContactWrench::p_WCb_W, doc.ContactWrench.p_WCb_W.doc) + .def_readonly( + "F_Cb_W", &ContactWrench::F_Cb_W, doc.ContactWrench.F_Cb_W.doc); + AddValueInstantiation(m); + } + + { + using Class = StaticEquilibriumProblem; + constexpr auto& cls_doc = doc.StaticEquilibriumProblem; + py::class_(m, "StaticEquilibriumProblem", cls_doc.doc) + .def(py::init*, + systems::Context*, + const std::set< + std::pair>&>(), + py::arg("plant"), py::arg("context"), + py::arg("ignored_collision_pairs"), + // Keep alive, reference: `self` keeps `plant` and `context` alive. + py::keep_alive<1, 2>(), py::keep_alive<1, 3>(), cls_doc.ctor.doc) + .def("get_mutable_prog", &Class::get_mutable_prog, + py_rvp::reference_internal, cls_doc.get_mutable_prog.doc) + .def("prog", &Class::prog, py_rvp::reference_internal, cls_doc.prog.doc) + .def("q_vars", &Class::q_vars, cls_doc.q_vars.doc) + .def("u_vars", &Class::u_vars, cls_doc.u_vars.doc) + .def("GetContactWrenchSolution", &Class::GetContactWrenchSolution, + py::arg("result"), cls_doc.GetContactWrenchSolution.doc) + .def("UpdateComplementarityTolerance", + &Class::UpdateComplementarityTolerance, py::arg("tol"), + cls_doc.UpdateComplementarityTolerance.doc); + } +} +} // namespace +} // namespace pydrake +} // namespace drake diff --git a/bindings/pydrake/multibody/test/optimization_test.py b/bindings/pydrake/multibody/test/optimization_test.py new file mode 100644 index 000000000000..2560906e1046 --- /dev/null +++ b/bindings/pydrake/multibody/test/optimization_test.py @@ -0,0 +1,176 @@ +import unittest +import typing + +import numpy as np + +from pydrake.multibody.optimization import StaticEquilibriumProblem +from pydrake.multibody.plant import ( + AddMultibodyPlantSceneGraph, + CoulombFriction +) +from pydrake.multibody.tree import ( + UnitInertia, + SpatialInertia, +) +import pydrake.multibody.inverse_kinematics as ik +import pydrake.solvers.mathematicalprogram as mp +from pydrake.solvers.ipopt import IpoptSolver +from pydrake.systems.framework import DiagramBuilder_ +from pydrake.geometry import ( + Box, + Sphere, +) +from pydrake.math import RigidTransform +from pydrake.autodiffutils import AutoDiffXd + + +def construct_environment(masses: typing.List, box_sizes: typing.List): + """ + Construct an environment with many free boxes. + @param masses masses[i] is the mass of box i. + @param box_sizes box_sizes[i] is the size of box i. + """ + builder = DiagramBuilder_[AutoDiffXd]() + plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) + # Add the ground as a big box. + ground_box = plant.AddRigidBody( + "ground", SpatialInertia(1, np.array([0, 0, 0]), UnitInertia(1, 1, 1))) + X_WG = RigidTransform([0, 0, -0.05]) + ground_geometry_id = plant.RegisterCollisionGeometry( + ground_box, RigidTransform(), Box(10, 10, 0.1), "ground", + CoulombFriction(0.9, 0.8)) + plant.RegisterVisualGeometry( + ground_box, RigidTransform(), Box(10, 10, 0.1), "ground", + [0.5, 0.5, 0.5, 1.]) + plant.WeldFrames(plant.world_frame(), ground_box.body_frame(), X_WG) + + # Add boxes + assert isinstance(masses, list) + assert isinstance(box_sizes, list) + assert len(masses) == len(box_sizes) + num_boxes = len(masses) + boxes = [] + boxes_geometry_id = [] + for i in range(num_boxes): + box_name = f"box{i}" + boxes.append(plant.AddRigidBody( + box_name, SpatialInertia( + masses[i], np.array([0, 0, 0]), UnitInertia(1, 1, 1)))) + box_shape = Box(box_sizes[i][0], box_sizes[i][1], box_sizes[i][2]) + boxes_geometry_id.append(plant.RegisterCollisionGeometry( + boxes[i], RigidTransform(), box_shape, f"{box_name}_box", + CoulombFriction(0.9, 0.8))) + plant.RegisterVisualGeometry( + ground_box, RigidTransform(), box_shape, f"{box_name}_box", + [0.5, 0.5, 0.5, 1.]) + # Add small spheres at the corners of the box. + vertices = np.array([ + [1, 1, 1], [1, 1, -1], [1, -1, 1], [1, -1, -1], [-1, 1, 1], + [-1, -1, 1], [-1, 1, -1], [-1, -1, -1]]) *\ + np.tile(box_sizes[i]/2, (8, 1)) + sphere_shape = Sphere(0.001) + for j in range(8): + plant.RegisterCollisionGeometry( + boxes[i], RigidTransform(vertices[j]), sphere_shape, + f"{box_name}_sphere{j}", CoulombFriction(0.9, 0.8)) + plant.RegisterVisualGeometry( + boxes[i], RigidTransform(vertices[j]), sphere_shape, + f"{box_name}_sphere{j}", [0.5, 0.5, 0.5, 1]) + + plant.Finalize() + diagram = builder.Build() + + return plant, scene_graph, diagram, boxes, ground_geometry_id,\ + boxes_geometry_id + + +def split_se3(q_se3): + """ + q_se3 is the quaternion/position of a floating body. + """ + assert q_se3.shape == (7,) + quat = q_se3[:4] + xyz = q_se3[4:] + return quat, xyz + + +class TestStaticEquilibriumProblem(unittest.TestCase): + + def test_one_box(self): + # Test with a single box. + masses = [1.] + box_sizes = [np.array([0.1, 0.1, 0.1])] + plant, scene_graph, diagram, boxes, ground_geometry_id,\ + boxes_geometry_id = construct_environment( + masses, box_sizes) + diagram_context = diagram.CreateDefaultContext() + plant_context = diagram.GetMutableSubsystemContext( + plant, diagram_context) + # Ignore the collision between box-box, since currently Drake doesn't + # support box-box collision with AutoDiffXd. + ignored_collision_pairs = {( + ground_geometry_id, boxes_geometry_id[0])} + dut = StaticEquilibriumProblem( + plant, plant_context, ignored_collision_pairs) + + # Add the constraint that the quaternion should have unit length. + box_quat, box_xyz = split_se3(dut.q_vars()) + ik.AddUnitQuaternionConstraintOnPlant( + plant, dut.q_vars(), dut.get_mutable_prog()) + + dut.get_mutable_prog().SetInitialGuess( + box_quat, np.array([0.5, 0.5, 0.5, 0.5])) + dut.get_mutable_prog().SetInitialGuess( + box_xyz, np.array([0, 0, 0.3])) + + # Now set the complementarity tolerance. + dut.UpdateComplementarityTolerance(0.002) + result = mp.Solve(dut.prog()) + self.assertTrue(result.is_success()) + q_sol = result.GetSolution(dut.q_vars()) + box_quat_sol, box_xyz_sol = split_se3(q_sol) + self.assertAlmostEqual(np.linalg.norm(box_quat_sol), 1.) + np.testing.assert_allclose(box_xyz_sol[2], 0.05, atol=0.002) + + contact_wrenches = dut.GetContactWrenchSolution(result) + self.assertEqual(len(contact_wrenches), 8) + + def test_two_boxes(self): + # test with 2 boxes. + masses = [1., 2] + box_sizes = [np.array([0.1, 0.1, 0.1]), np.array([0.1, 0.1, 0.2])] + plant, scene_graph, diagram, boxes, ground_geometry_id,\ + boxes_geometry_id = construct_environment( + masses, box_sizes) + diagram_context = diagram.CreateDefaultContext() + plant_context = diagram.GetMutableSubsystemContext( + plant, diagram_context) + # Ignore the collision between box-box, since currently Drake doesn't + # support box-box collision with AutoDiffXd. + ignored_collision_pairs = set() + for i in range(len(boxes_geometry_id)): + ignored_collision_pairs.add(tuple(( + ground_geometry_id, boxes_geometry_id[i]))) + for j in range(i+1, len(boxes_geometry_id)): + ignored_collision_pairs.add(tuple( + (boxes_geometry_id[i], boxes_geometry_id[j]))) + + dut = StaticEquilibriumProblem( + plant, plant_context, ignored_collision_pairs) + + # Add the constraint that the quaternion should have unit length. + ik.AddUnitQuaternionConstraintOnPlant( + plant, dut.q_vars(), dut.get_mutable_prog()) + for i in range(len(boxes)): + box_quaternion_start = 7 * i + box_quat, box_xyz = split_se3( + dut.q_vars()[box_quaternion_start:box_quaternion_start+7]) + dut.get_mutable_prog().SetInitialGuess( + box_quat, np.array([0.5, 0.5, 0.5, 0.5])) + dut.get_mutable_prog().SetInitialGuess( + box_xyz, np.array([0, 0, 0.3 + 0.2 * i])) + + dut.UpdateComplementarityTolerance(0.001) + ipopt_solver = IpoptSolver() + result = ipopt_solver.Solve(dut.prog()) + self.assertTrue(result.is_success()) diff --git a/multibody/optimization/contact_wrench.h b/multibody/optimization/contact_wrench.h index d4c9ab5250d0..0ebb45cccdb9 100644 --- a/multibody/optimization/contact_wrench.h +++ b/multibody/optimization/contact_wrench.h @@ -14,12 +14,7 @@ namespace multibody { */ struct ContactWrench { /** - * @param bodyA_index_in The index of Body A. - * @param bodyB_index_in The index of Body B. - * @param p_WCb_W_in The position of the point Cb (where the wrench is - * applied) expressed in the world frame W. - * @param F_Cb_W_in The wrench (spatial force) applied at point Cb from Body A - * to Body B, measured in the world frame. + * Refer to the documentation for each attribute. */ ContactWrench(BodyIndex bodyA_index_in, BodyIndex bodyB_index_in, Eigen::Vector3d p_WCb_W_in, SpatialForce F_Cb_W_in) @@ -27,9 +22,18 @@ struct ContactWrench { bodyB_index(bodyB_index_in), p_WCb_W(std::move(p_WCb_W_in)), F_Cb_W(std::move(F_Cb_W_in)) {} + + /** The index of Body A.*/ BodyIndex bodyA_index; + /** The index of Body B.*/ BodyIndex bodyB_index; + /** The position of the point Cb (where the wrench is applied) expressed in + * the world frame W. + */ Eigen::Vector3d p_WCb_W; + /** F_Cb_W_in The wrench (spatial force) applied at point Cb from Body A + * to Body B, measured in the world frame. + */ SpatialForce F_Cb_W; }; } // namespace multibody diff --git a/multibody/optimization/static_equilibrium_constraint.cc b/multibody/optimization/static_equilibrium_constraint.cc index 6b6409ad829e..a5b9705cef64 100644 --- a/multibody/optimization/static_equilibrium_constraint.cc +++ b/multibody/optimization/static_equilibrium_constraint.cc @@ -62,14 +62,15 @@ void StaticEquilibriumConstraint::DoEval( } const auto& query_object = query_port.Eval>(*context_); - const std::vector> - signed_distance_pairs = - query_object.ComputeSignedDistancePairwiseClosestPoints(); const geometry::SceneGraphInspector& inspector = query_object.inspector(); const int lambda_start_index_in_x = plant_->num_positions() + plant_->num_actuated_dofs(); - for (const auto& signed_distance_pair : signed_distance_pairs) { + for (const auto& [contact_pair, evaluator] : + contact_pair_to_wrench_evaluator_) { + const geometry::SignedDistancePair signed_distance_pair = + query_object.ComputeSignedDistancePairClosestPoints( + contact_pair.first(), contact_pair.second()); const geometry::FrameId frame_A_id = inspector.GetFrameId(signed_distance_pair.id_A); const geometry::FrameId frame_B_id = @@ -101,29 +102,19 @@ void StaticEquilibriumConstraint::DoEval( frameB, p_BCb, plant_->world_frame(), plant_->world_frame(), &Jv_V_WCb); - const SortedPair contact_pair( - signed_distance_pair.id_A, signed_distance_pair.id_B); // Find the lambda corresponding to the geometry pair (id_A, id_B). - const auto it = contact_pair_to_wrench_evaluator_.find(contact_pair); - if (it == contact_pair_to_wrench_evaluator_.end()) { - throw std::runtime_error( - "The input argument contact_pair_to_wrench_evaluator in the " - "StaticEquilibriumConstraint constructor doesn't include all " - "possible contact pairs."); - } - VectorX lambda( - it->second.contact_wrench_evaluator->num_lambda()); + evaluator.contact_wrench_evaluator->num_lambda()); for (int i = 0; i < lambda.rows(); ++i) { lambda(i) = x(lambda_start_index_in_x + - it->second.lambda_indices_in_all_lambda[i]); + evaluator.lambda_indices_in_all_lambda[i]); } AutoDiffVecXd F_AB_W; - it->second.contact_wrench_evaluator->Eval( - it->second.contact_wrench_evaluator->ComposeVariableValues(*context_, - lambda), + evaluator.contact_wrench_evaluator->Eval( + evaluator.contact_wrench_evaluator->ComposeVariableValues(*context_, + lambda), &F_AB_W); // By definition, F_AB_W is the contact wrench applied to id_B from id_A, diff --git a/multibody/optimization/static_equilibrium_problem.cc b/multibody/optimization/static_equilibrium_problem.cc index 6e13a612a74f..23f62efc3986 100644 --- a/multibody/optimization/static_equilibrium_problem.cc +++ b/multibody/optimization/static_equilibrium_problem.cc @@ -82,22 +82,14 @@ std::vector StaticEquilibriumProblem::GetContactWrenchSolution( } const auto& query_object = query_port.Eval>(*context_); - // TODO(hongkai.dai) compute the signed distance between a specific pair of - // contact, rather than all pairs of contact. - const std::vector> - signed_distance_pairs = - query_object.ComputeSignedDistancePairwiseClosestPoints(); const geometry::SceneGraphInspector& inspector = query_object.inspector(); std::vector contact_wrench_sol; contact_wrench_sol.reserve(contact_wrench_evaluators_and_lambda_.size()); - for (const auto& contact_wrench_evaluator_and_lambda : + for (const auto& [contact_wrench_evaluator, lambda] : contact_wrench_evaluators_and_lambda_) { - const std::shared_ptr& contact_wrench_evaluator = - contact_wrench_evaluator_and_lambda.first; // Compute the contact wrench - const auto lambda_sol = - result.GetSolution(contact_wrench_evaluator_and_lambda.second); + const auto lambda_sol = result.GetSolution(lambda); Eigen::VectorXd F_Cb_W; contact_wrench_evaluator->Eval( contact_wrench_evaluator->ComposeVariableValues(q_sol, lambda_sol), @@ -107,30 +99,27 @@ std::vector StaticEquilibriumProblem::GetContactWrenchSolution( const SortedPair& contact_pair = contact_wrench_evaluator->geometry_id_pair(); BodyIndex body_A_index, body_B_index; - for (const auto& signed_distance_pair : signed_distance_pairs) { - if (signed_distance_pair.id_A == contact_pair.first() && - signed_distance_pair.id_B == contact_pair.second()) { - const geometry::FrameId frame_Fa_id = - inspector.GetFrameId(signed_distance_pair.id_A); - const geometry::FrameId frame_Fb_id = - inspector.GetFrameId(signed_distance_pair.id_B); - const Frame& frame_Fa = - plant_.GetBodyFromFrameId(frame_Fa_id)->body_frame(); - const Frame& frame_Fb = - plant_.GetBodyFromFrameId(frame_Fb_id)->body_frame(); - body_A_index = frame_Fa.body().index(); - body_B_index = frame_Fb.body().index(); - // Define Body B's frame as Fb, the geometry attached to body B has - // frame Gb, and the witness point on geometry Gb is Cb. - const auto& X_FbGb = inspector.GetPoseInFrame(signed_distance_pair.id_B) - .template cast(); - const auto& p_GbCb = signed_distance_pair.p_BCb; - const Vector3 p_FbCb = X_FbGb * p_GbCb; - plant_.CalcPointsPositions(*context_, frame_Fb, p_FbCb, - plant_.world_frame(), &p_WCb); - break; - } - } + const geometry::SignedDistancePair signed_distance_pair = + query_object.ComputeSignedDistancePairClosestPoints( + contact_pair.first(), contact_pair.second()); + const geometry::FrameId frame_Fa_id = + inspector.GetFrameId(signed_distance_pair.id_A); + const geometry::FrameId frame_Fb_id = + inspector.GetFrameId(signed_distance_pair.id_B); + const Frame& frame_Fa = + plant_.GetBodyFromFrameId(frame_Fa_id)->body_frame(); + const Frame& frame_Fb = + plant_.GetBodyFromFrameId(frame_Fb_id)->body_frame(); + body_A_index = frame_Fa.body().index(); + body_B_index = frame_Fb.body().index(); + // Define Body B's frame as Fb, the geometry attached to body B has + // frame Gb, and the witness point on geometry Gb is Cb. + const auto& X_FbGb = inspector.GetPoseInFrame(signed_distance_pair.id_B) + .template cast(); + const auto& p_GbCb = signed_distance_pair.p_BCb; + const Vector3 p_FbCb = X_FbGb * p_GbCb; + plant_.CalcPointsPositions(*context_, frame_Fb, p_FbCb, + plant_.world_frame(), &p_WCb); contact_wrench_sol.emplace_back( body_A_index, body_B_index, math::autoDiffToValueMatrix(p_WCb), SpatialForce(Vector6(F_Cb_W))); diff --git a/multibody/optimization/static_friction_cone_complementarity_constraint.cc b/multibody/optimization/static_friction_cone_complementarity_constraint.cc index 65eda34f45e7..5944de2183b7 100644 --- a/multibody/optimization/static_friction_cone_complementarity_constraint.cc +++ b/multibody/optimization/static_friction_cone_complementarity_constraint.cc @@ -96,61 +96,44 @@ void StaticFrictionConeComplementarityNonlinearConstraint::DoEval( } const auto& query_object = query_port.Eval>(context); - const std::vector> - signed_distance_pairs = - query_object.ComputeSignedDistancePairwiseClosestPoints(); - bool found_geometry_pair = false; - for (const auto& signed_distance_pair : signed_distance_pairs) { - if (signed_distance_pair.id_A == - contact_wrench_evaluator_->geometry_id_pair().first() && - signed_distance_pair.id_B == - contact_wrench_evaluator_->geometry_id_pair().second()) { - found_geometry_pair = true; - // Compute the friction. - const geometry::ProximityProperties& geometryA_props = - *query_object.inspector().GetProximityProperties( - signed_distance_pair.id_A); - const geometry::ProximityProperties& geometryB_props = - *query_object.inspector().GetProximityProperties( - signed_distance_pair.id_B); - - const CoulombFriction& geometryA_friction = - geometryA_props.GetProperty>( - "material", "coulomb_friction"); - const CoulombFriction& geometryB_friction = - geometryB_props.GetProperty>( - "material", "coulomb_friction"); - - CoulombFriction combined_friction = - CalcContactFrictionFromSurfaceProperties(geometryA_friction, - geometryB_friction); - - const auto& nhat_BA_W = signed_distance_pair.nhat_BA_W; - // The constraint f_Wᵀ * ((μ² + 1)* n_W * n_Wᵀ - I) * f_W >= 0 - (*y)(0) = - f_AB_W.dot(((std::pow(combined_friction.static_friction(), 2) + 1) * - nhat_BA_W * nhat_BA_W.transpose() - - Eigen::Matrix3d::Identity()) * - f_AB_W); - - // The constraint n̂_AB_W.dot(f_AB_W) - α = 0 - (*y)(1) = -nhat_BA_W.dot(f_AB_W) - alpha; - - // The constraint sdf - β = 0 - (*y)(2) = signed_distance_pair.distance - beta; - - // The constraint α * β <= ε - (*y)(3) = alpha * beta; - - break; - } - } - if (!found_geometry_pair) { - throw std::runtime_error( - "StaticFrictionConeComplementarityNonlinearConstraint: the input " - "contact_wrench_evaluator contains a pair of geometry, that has not " - "been registered to the SceneGraph for distance computation."); - } + const geometry::SignedDistancePair signed_distance_pair = + query_object.ComputeSignedDistancePairClosestPoints( + contact_wrench_evaluator_->geometry_id_pair().first(), + contact_wrench_evaluator_->geometry_id_pair().second()); + // Compute the friction. + const geometry::ProximityProperties& geometryA_props = + *query_object.inspector().GetProximityProperties( + signed_distance_pair.id_A); + const geometry::ProximityProperties& geometryB_props = + *query_object.inspector().GetProximityProperties( + signed_distance_pair.id_B); + + const CoulombFriction& geometryA_friction = + geometryA_props.GetProperty>("material", + "coulomb_friction"); + const CoulombFriction& geometryB_friction = + geometryB_props.GetProperty>("material", + "coulomb_friction"); + + CoulombFriction combined_friction = + CalcContactFrictionFromSurfaceProperties(geometryA_friction, + geometryB_friction); + + const auto& nhat_BA_W = signed_distance_pair.nhat_BA_W; + // The constraint f_Wᵀ * ((μ² + 1)* n_W * n_Wᵀ - I) * f_W >= 0 + (*y)(0) = f_AB_W.dot(((std::pow(combined_friction.static_friction(), 2) + 1) * + nhat_BA_W * nhat_BA_W.transpose() - + Eigen::Matrix3d::Identity()) * + f_AB_W); + + // The constraint n̂_AB_W.dot(f_AB_W) - α = 0 + (*y)(1) = -nhat_BA_W.dot(f_AB_W) - alpha; + + // The constraint sdf - β = 0 + (*y)(2) = signed_distance_pair.distance - beta; + + // The constraint α * β <= ε + (*y)(3) = alpha * beta; } void StaticFrictionConeComplementarityNonlinearConstraint::DoEval( diff --git a/multibody/optimization/test/static_equilibrium_problem_test.cc b/multibody/optimization/test/static_equilibrium_problem_test.cc index 5e98974db4c2..7bf7ba546d2b 100644 --- a/multibody/optimization/test/static_equilibrium_problem_test.cc +++ b/multibody/optimization/test/static_equilibrium_problem_test.cc @@ -212,6 +212,6 @@ GTEST_TEST(TestStaticEquilibriumProblem, TwoSpheresWithinBin) { q_init.tail<3>() << -0.1, 0, radii[1] + 0.01; dut.prog().SetDecisionVariableValueInVector(dut.q_vars(), q_init, &x_init); check_static_equilibrium_problem_solve(x_init); -} // namespace multibody +} } // namespace multibody } // namespace drake