Skip to content

Commit

Permalink
Remove default_coulomb_friction from MBP (RobotLocomotion#13371)
Browse files Browse the repository at this point in the history
removes default coulomb friction
  • Loading branch information
joemasterjohn authored Jun 4, 2020
1 parent 76956e1 commit 9acf636
Show file tree
Hide file tree
Showing 13 changed files with 276 additions and 123 deletions.
11 changes: 8 additions & 3 deletions bindings/pydrake/multibody/plant_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -664,11 +664,16 @@ void DoScalarDependentDefinitions(py::module m, T) {
py_reference_internal, cls_doc.GetCollisionGeometriesForBody.doc)
.def("num_collision_geometries", &Class::num_collision_geometries,
cls_doc.num_collision_geometries.doc)
.def("default_coulomb_friction", &Class::default_coulomb_friction,
py::arg("geometry_id"), py_reference_internal,
cls_doc.default_coulomb_friction.doc)
.def("CollectRegisteredGeometries", &Class::CollectRegisteredGeometries,
py::arg("bodies"), cls_doc.CollectRegisteredGeometries.doc);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
cls.def("default_coulomb_friction", &Class::default_coulomb_friction,
py::arg("geometry_id"), py_reference_internal,
cls_doc.default_coulomb_friction.doc_deprecated);
#pragma GCC diagnostic pop
DeprecateAttribute(cls, "default_coulomb_friction",
cls_doc.default_coulomb_friction.doc_deprecated);
// Port accessors.
cls // BR
.def("get_actuation_input_port",
Expand Down
24 changes: 12 additions & 12 deletions bindings/pydrake/multibody/test/plant_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -195,25 +195,25 @@ def test_multibody_plant_construction_api(self, T):
body=body, X_BG=body_X_BG, shape=box,
name="new_body_collision", coulomb_friction=body_friction)
self.assertGreater(plant.num_collision_geometries(), 0)
self.assertEqual(plant.default_coulomb_friction(
plant.GetCollisionGeometriesForBody(body)[0]
).static_friction(), 0.6)
self.assertEqual(plant.default_coulomb_friction(
plant.GetCollisionGeometriesForBody(body)[0]
).dynamic_friction(), 0.5)
body0_props = scene_graph.model_inspector().GetProximityProperties(
plant.GetCollisionGeometriesForBody(body)[0])
body0_friction = body0_props.GetProperty(
"material", "coulomb_friction")
self.assertEqual(body0_friction.static_friction(), 0.6)
self.assertEqual(body0_friction.dynamic_friction(), 0.5)
explicit_props = ProximityProperties()
explicit_props.AddProperty("material", "coulomb_friction",
CoulombFriction(1.1, 0.8))
plant.RegisterCollisionGeometry(
body=body, X_BG=body_X_BG, shape=box,
name="new_body_collision2", properties=explicit_props)
body1_props = scene_graph.model_inspector().GetProximityProperties(
plant.GetCollisionGeometriesForBody(body)[1])
body1_friction = body1_props.GetProperty(
"material", "coulomb_friction")
self.assertGreater(plant.num_collision_geometries(), 1)
self.assertEqual(plant.default_coulomb_friction(
plant.GetCollisionGeometriesForBody(body)[1]
).static_friction(), 1.1)
self.assertEqual(plant.default_coulomb_friction(
plant.GetCollisionGeometriesForBody(body)[1]
).dynamic_friction(), 0.8)
self.assertEqual(body1_friction.static_friction(), 1.1)
self.assertEqual(body1_friction.dynamic_friction(), 0.8)

@numpy_compare.check_all_types
def test_multibody_plant_api_via_parsing(self, T):
Expand Down
19 changes: 15 additions & 4 deletions examples/planar_gripper/gripper_brick.cc
Original file line number Diff line number Diff line change
Expand Up @@ -199,11 +199,22 @@ geometry::GeometryId GripperBrickHelper<T>::finger_tip_sphere_geometry_id(
template <typename T>
multibody::CoulombFriction<T>
GripperBrickHelper<T>::GetFingerTipBrickCoulombFriction(Finger finger) const {
const multibody::CoulombFriction<T>& brick_friction =
plant_->default_coulomb_friction(
const geometry::ProximityProperties& brick_props =
*scene_graph_->model_inspector().GetProximityProperties(
plant_->GetCollisionGeometriesForBody(brick_frame().body())[0]);
const multibody::CoulombFriction<double>& finger_tip_friction =
plant_->default_coulomb_friction(finger_tip_sphere_geometry_id(finger));

const geometry::ProximityProperties& finger_props =
*scene_graph_->model_inspector().GetProximityProperties(
finger_tip_sphere_geometry_id(finger));

const multibody::CoulombFriction<T>& brick_friction =
brick_props.GetProperty<multibody::CoulombFriction<T>>(
"material", "coulomb_friction");

const multibody::CoulombFriction<T>& finger_tip_friction =
finger_props.GetProperty<multibody::CoulombFriction<T>>(
"material", "coulomb_friction");

return multibody::CalcContactFrictionFromSurfaceProperties(
brick_friction, finger_tip_friction);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,16 +16,8 @@ void AddFrictionConeConstraint(
const BrickFace brick_face,
const Eigen::Ref<const Vector2<symbolic::Variable>>& f_Cb_B,
double friction_cone_shrink_factor, solvers::MathematicalProgram* prog) {
const auto& plant = gripper_brick_system.plant();
const multibody::CoulombFriction<double>& brick_friction =
plant.default_coulomb_friction(plant.GetCollisionGeometriesForBody(
gripper_brick_system.brick_frame().body())[0]);
const multibody::CoulombFriction<double>& finger_tip_friction =
plant.default_coulomb_friction(
gripper_brick_system.finger_tip_sphere_geometry_id(finger));
const multibody::CoulombFriction<double> combined_friction =
multibody::CalcContactFrictionFromSurfaceProperties(brick_friction,
finger_tip_friction);
gripper_brick_system.GetFingerTipBrickCoulombFriction(finger);
const double mu =
combined_friction.static_friction() * friction_cone_shrink_factor;
switch (brick_face) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,16 +35,9 @@ GTEST_TEST(AddFrictionConeConstraintTest, Test) {
EXPECT_EQ(is_satisfied, is_in_cone);
};

const auto& plant = gripper_brick.plant();
const multibody::CoulombFriction<double>& brick_friction =
plant.default_coulomb_friction(plant.GetCollisionGeometriesForBody(
gripper_brick.brick_frame().body())[0]);
const multibody::CoulombFriction<double>& finger_tip_friction =
plant.default_coulomb_friction(
gripper_brick.finger_tip_sphere_geometry_id(Finger::kFinger2));
const multibody::CoulombFriction<double> combined_friction =
multibody::CalcContactFrictionFromSurfaceProperties(brick_friction,
finger_tip_friction);
gripper_brick.GetFingerTipBrickCoulombFriction(Finger::kFinger2);

const double mu = combined_friction.static_friction();
// Test a force that points in the neg Y direction. This force is not in the
// friction cone.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -175,17 +175,27 @@ void SlidingFrictionComplementarityNonlinearConstraint::DoEval(
signed_distance_pair.id_B ==
contact_wrench_evaluator_->geometry_id_pair().second()) {
found_geometry_pair = true;

const geometry::SceneGraphInspector<AutoDiffXd>& inspector =
query_object.inspector();

// Compute the friction.
const geometry::ProximityProperties& geometryA_props =
*inspector.GetProximityProperties(signed_distance_pair.id_A);
const geometry::ProximityProperties& geometryB_props =
*inspector.GetProximityProperties(signed_distance_pair.id_B);

const CoulombFriction<double>& geometryA_friction =
plant.default_coulomb_friction(signed_distance_pair.id_A);
geometryA_props.GetProperty<CoulombFriction<double>>(
"material", "coulomb_friction");
const CoulombFriction<double>& geometryB_friction =
plant.default_coulomb_friction(signed_distance_pair.id_B);
geometryB_props.GetProperty<CoulombFriction<double>>(
"material", "coulomb_friction");

CoulombFriction<double> combined_friction =
CalcContactFrictionFromSurfaceProperties(geometryA_friction,
geometryB_friction);

const geometry::SceneGraphInspector<AutoDiffXd>& inspector =
query_object.inspector();
const geometry::FrameId frame_A_id =
inspector.GetFrameId(signed_distance_pair.id_A);
const geometry::FrameId frame_B_id =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -107,10 +107,20 @@ void StaticFrictionConeComplementarityNonlinearConstraint::DoEval(
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<double>& geometryA_friction =
plant.default_coulomb_friction(signed_distance_pair.id_A);
geometryA_props.GetProperty<CoulombFriction<double>>(
"material", "coulomb_friction");
const CoulombFriction<double>& geometryB_friction =
plant.default_coulomb_friction(signed_distance_pair.id_B);
geometryB_props.GetProperty<CoulombFriction<double>>(
"material", "coulomb_friction");

CoulombFriction<double> combined_friction =
CalcContactFrictionFromSurfaceProperties(geometryA_friction,
geometryB_friction);
Expand Down
14 changes: 12 additions & 2 deletions multibody/optimization/static_friction_cone_constraint.cc
Original file line number Diff line number Diff line change
Expand Up @@ -74,10 +74,20 @@ void StaticFrictionConeConstraint::DoEval(
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<double>& geometryA_friction =
plant.default_coulomb_friction(signed_distance_pair.id_A);
geometryA_props.GetProperty<CoulombFriction<double>>(
"material", "coulomb_friction");
const CoulombFriction<double>& geometryB_friction =
plant.default_coulomb_friction(signed_distance_pair.id_B);
geometryB_props.GetProperty<CoulombFriction<double>>(
"material", "coulomb_friction");

CoulombFriction<double> combined_friction =
CalcContactFrictionFromSurfaceProperties(geometryA_friction,
geometryB_friction);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -61,10 +61,10 @@ void ComputeRelativeMotion(test::FreeSpheresAndBoxes<T>* spheres,
}

GTEST_TEST(SlidingFrictionComplementarityNonlinearConstraintTest, Constructor) {
const test::SphereSpecification sphere1_spec(
0.1, 1e3, CoulombFriction<double>(1.1, 0.9));
const test::SphereSpecification sphere2_spec(
0.2, 1.2e3, CoulombFriction<double>(1.2, 0.8));
const CoulombFriction<double> sphere1_friction(1.1, 0.9);
const test::SphereSpecification sphere1_spec(0.1, 1e3, sphere1_friction);
const CoulombFriction<double> sphere2_friction(1.2, 0.8);
const test::SphereSpecification sphere2_spec(0.2, 1.2e3, sphere2_friction);
const CoulombFriction<double> ground_friction(0.6, 0.5);
test::FreeSpheresAndBoxes<AutoDiffXd> spheres(
{sphere1_spec, sphere2_spec}, {} /* no box. */, ground_friction);
Expand Down Expand Up @@ -149,9 +149,8 @@ GTEST_TEST(SlidingFrictionComplementarityNonlinearConstraintTest, Constructor) {

// Evaluate constraint (3)
const CoulombFriction<double> combined_friction =
CalcContactFrictionFromSurfaceProperties(
plant.default_coulomb_friction(spheres.sphere_geometry_ids()[0]),
plant.default_coulomb_friction(spheres.sphere_geometry_ids()[1]));
CalcContactFrictionFromSurfaceProperties(sphere1_friction,
sphere2_friction);
CompareAutoDiff(y_autodiff(6), f_sliding.dot(n_SaSb_W), tol);

const Vector3<AutoDiffXd> f_sliding_tangential =
Expand Down Expand Up @@ -194,10 +193,10 @@ GTEST_TEST(SlidingFrictionComplementarityNonlinearConstraintTest, Constructor) {

namespace {
GTEST_TEST(SlidingFrictionComplementarityConstraintTest, AddConstraint) {
const test::SphereSpecification sphere1_spec(
0.1, 1e3, CoulombFriction<double>(1.1, 0.9));
const test::SphereSpecification sphere2_spec(
0.2, 1.2e3, CoulombFriction<double>(1.2, 0.8));
const CoulombFriction<double> sphere1_friction(1.1, 0.9);
const test::SphereSpecification sphere1_spec(0.1, 1e3, sphere1_friction);
const CoulombFriction<double> sphere2_friction(1.2, 0.8);
const test::SphereSpecification sphere2_spec(0.2, 1.2e3, sphere2_friction);
const CoulombFriction<double> ground_friction(0.6, 0.5);
test::FreeSpheresAndBoxes<AutoDiffXd> spheres(
{sphere1_spec, sphere2_spec}, {} /* no box. */, ground_friction);
Expand Down Expand Up @@ -245,9 +244,8 @@ GTEST_TEST(SlidingFrictionComplementarityConstraintTest, AddConstraint) {
double c_val1 = 0.1;
Eigen::Vector3d f_sliding_tangential = -c_val1 * v_tangential_SaCb_W;
const CoulombFriction<double> combined_friction =
CalcContactFrictionFromSurfaceProperties(
plant.default_coulomb_friction(spheres.sphere_geometry_ids()[0]),
plant.default_coulomb_friction(spheres.sphere_geometry_ids()[1]));
CalcContactFrictionFromSurfaceProperties(sphere1_friction,
sphere2_friction);
Eigen::Vector3d f_sliding_normal = nhat_SaSb_W * f_sliding_tangential.norm() /
combined_friction.dynamic_friction();
Eigen::Vector3d f_sliding = f_sliding_normal + f_sliding_tangential;
Expand Down
24 changes: 15 additions & 9 deletions multibody/parsing/test/common_parser_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -124,12 +124,16 @@ TEST_P(MultibodyPlantLinkTests, LinksWithCollisions) {
plant_.GetCollisionGeometriesForBody(plant_.GetBodyByName("link1"));
ASSERT_EQ(link1_collision_geometry_ids.size(), 2);

EXPECT_TRUE(
plant_.default_coulomb_friction(link1_collision_geometry_ids[0]) ==
CoulombFriction<double>(0.8, 0.3));
EXPECT_TRUE(
plant_.default_coulomb_friction(link1_collision_geometry_ids[1]) ==
CoulombFriction<double>(1.5, 0.6));
EXPECT_TRUE(scene_graph_.model_inspector()
.GetProximityProperties(link1_collision_geometry_ids[0])
->GetProperty<CoulombFriction<double>>("material",
"coulomb_friction") ==
CoulombFriction<double>(0.8, 0.3));
EXPECT_TRUE(scene_graph_.model_inspector()
.GetProximityProperties(link1_collision_geometry_ids[1])
->GetProperty<CoulombFriction<double>>("material",
"coulomb_friction") ==
CoulombFriction<double>(1.5, 0.6));

const std::vector<GeometryId>& link2_collision_geometry_ids =
plant_.GetCollisionGeometriesForBody(plant_.GetBodyByName("link2"));
Expand All @@ -140,9 +144,11 @@ TEST_P(MultibodyPlantLinkTests, LinksWithCollisions) {
ASSERT_EQ(link3_collision_geometry_ids.size(), 1);
// Verifies the default value of the friction coefficients when the user does
// not specify them in the SDF file.
EXPECT_TRUE(
plant_.default_coulomb_friction(link3_collision_geometry_ids[0]) ==
default_friction());
EXPECT_EQ(scene_graph_.model_inspector()
.GetProximityProperties(link3_collision_geometry_ids[0])
->GetProperty<CoulombFriction<double>>("material",
"coulomb_friction"),
default_friction());
}


Expand Down
Loading

0 comments on commit 9acf636

Please sign in to comment.