diff --git a/examples/multibody/deformable/deformable_torus.cc b/examples/multibody/deformable/deformable_torus.cc index 2c6ed06a06f3..5d4437b73183 100644 --- a/examples/multibody/deformable/deformable_torus.cc +++ b/examples/multibody/deformable/deformable_torus.cc @@ -90,18 +90,18 @@ ModelInstanceIndex AddSuctionGripper( Vector4d(0.9, 0.1, 0.1, 0.8)); plant->RegisterVisualGeometry(body, RigidTransformd::Identity(), capsule, "cup_visual", cup_illustration_props); - /* Add a visual hint for the center of the suction force source. */ - const Sphere sphere{0.0075}; + /* Add a visual hint for the support of the suction force source. */ + const Sphere sphere{0.01}; IllustrationProperties source_illustration_props; source_illustration_props.AddProperty("phong", "diffuse", Vector4d(0.1, 0.9, 0.1, 0.8)); - plant->RegisterVisualGeometry(body, RigidTransformd(Vector3d(0, 0, -0.07)), + plant->RegisterVisualGeometry(body, RigidTransformd(Vector3d(0, 0, -0.065)), sphere, "source_visual", source_illustration_props); plant->RegisterCollisionGeometry(body, RigidTransformd::Identity(), capsule, "cup_collision", proximity_props); /* Adds an actuated joint between the suction cup body and the world. */ - const RigidTransformd X_WF(Vector3d(0.04, 0, -0.05)); + const RigidTransformd X_WF(Vector3d(0.05, 0, -0.05)); const auto& prismatic_joint = plant->AddJoint( "translate_z_joint", plant->world_body(), X_WF, body, std::nullopt, Vector3d::UnitZ()); @@ -188,6 +188,9 @@ int do_main() { deformable_config.set_poissons_ratio(FLAGS_nu); deformable_config.set_mass_density(FLAGS_density); deformable_config.set_stiffness_damping_coefficient(FLAGS_beta); + /* Subdivide the element for suction force integration; otherwise, the mesh is + too coarse for the support of the suction force field (1cm radius). */ + deformable_config.set_element_subdivision(2); /* Load the geometry and scale it down to 65% (to showcase the scaling capability and to make the torus suitable for grasping by the gripper). */ @@ -206,7 +209,7 @@ int do_main() { const PointSourceForceField* suction_force_ptr{nullptr}; if (use_suction) { auto suction_force = std::make_unique( - plant, plant.GetBodyByName("cup_body"), Vector3d(0, 0, -0.07), 0.1); + plant, plant.GetBodyByName("cup_body"), Vector3d(0, 0, -0.065), 0.01); suction_force_ptr = suction_force.get(); deformable_model.AddExternalForce(std::move(suction_force)); } diff --git a/examples/multibody/deformable/suction_cup_controller.cc b/examples/multibody/deformable/suction_cup_controller.cc index d94806cf5cf7..e95f89c8dbbc 100644 --- a/examples/multibody/deformable/suction_cup_controller.cc +++ b/examples/multibody/deformable/suction_cup_controller.cc @@ -60,9 +60,9 @@ void SuctionCupController::CalcMaxForceDensity( BasicVector* max_force_density) const { const double time = context.get_time(); if (time >= start_suction_time_ && time <= release_suction_time_) { - // An arbitrary value that's reasonable for picking up the deformable torus - // in the example with density comparable to water. - (*max_force_density)[0] = 2.0e5; + /* An arbitrary value that's reasonable for picking up the deformable torus + in the example with density comparable to water. */ + (*max_force_density)[0] = 2.0e8; } else { (*max_force_density)[0] = 0.0; } diff --git a/multibody/fem/deformable_body_config.h b/multibody/fem/deformable_body_config.h index a17c251f20fc..14e2517759c1 100644 --- a/multibody/fem/deformable_body_config.h +++ b/multibody/fem/deformable_body_config.h @@ -2,8 +2,8 @@ #include -#include "drake/common/drake_assert.h" #include "drake/common/drake_copyable.h" +#include "drake/common/drake_throw.h" namespace drake { namespace multibody { @@ -63,31 +63,31 @@ class DeformableBodyConfig { /** @pre youngs_modulus > 0. */ void set_youngs_modulus(T youngs_modulus) { - DRAKE_DEMAND(youngs_modulus > 0); + DRAKE_THROW_UNLESS(youngs_modulus > 0); youngs_modulus_ = std::move(youngs_modulus); } /** @pre -1 < poissons_ratio < 0.5. */ void set_poissons_ratio(T poissons_ratio) { - DRAKE_DEMAND(-1 < poissons_ratio && poissons_ratio < 0.5); + DRAKE_THROW_UNLESS(-1 < poissons_ratio && poissons_ratio < 0.5); poissons_ratio_ = std::move(poissons_ratio); } /** @pre mass_damping_coefficient >= 0. */ void set_mass_damping_coefficient(T mass_damping_coefficient) { - DRAKE_DEMAND(mass_damping_coefficient_ >= 0); + DRAKE_THROW_UNLESS(mass_damping_coefficient_ >= 0); mass_damping_coefficient_ = std::move(mass_damping_coefficient); } /** @pre stiffness_damping_coefficient >= 0. */ void set_stiffness_damping_coefficient(T stiffness_damping_coefficient) { - DRAKE_DEMAND(stiffness_damping_coefficient_ >= 0); + DRAKE_THROW_UNLESS(stiffness_damping_coefficient_ >= 0); stiffness_damping_coefficient_ = std::move(stiffness_damping_coefficient); } /** @pre mass_density > 0. */ void set_mass_density(T mass_density) { - DRAKE_DEMAND(mass_density > 0); + DRAKE_THROW_UNLESS(mass_density > 0); mass_density_ = std::move(mass_density); } @@ -95,6 +95,13 @@ class DeformableBodyConfig { material_model_ = material_model; } + /** @pre 0 <= element_subdivision <= 4. */ + void set_element_subdivision(int element_subdivision) { + DRAKE_THROW_UNLESS(element_subdivision >= 0); + DRAKE_THROW_UNLESS(element_subdivision <= 4); + element_subdivision_ = element_subdivision; + } + /** Returns the Young's modulus, with unit of N/m². */ const T& youngs_modulus() const { return youngs_modulus_; } /** Returns the Poisson's ratio, unitless. */ @@ -111,6 +118,10 @@ class DeformableBodyConfig { const T& mass_density() const { return mass_density_; } /** Returns the constitutive model of the material. */ MaterialModel material_model() const { return material_model_; } + /** Returns the number of times each element is subdivided when evaluating the + external forces. Useful when elements are too coarse to resolve external + force fields. */ + int element_subdivision() const { return element_subdivision_; } private: T youngs_modulus_{1e8}; @@ -119,6 +130,7 @@ class DeformableBodyConfig { T stiffness_damping_coefficient_{0}; T mass_density_{1.5e3}; MaterialModel material_model_{MaterialModel::kLinearCorotated}; + int element_subdivision_{0}; }; } // namespace fem diff --git a/multibody/fem/linear_simplex_element.cc b/multibody/fem/linear_simplex_element.cc index 0be71514097e..c2098fd85590 100644 --- a/multibody/fem/linear_simplex_element.cc +++ b/multibody/fem/linear_simplex_element.cc @@ -144,7 +144,7 @@ template class LinearSimplexElement; template class LinearSimplexElement; template class LinearSimplexElement; template class LinearSimplexElement; -template class LinearSimplexElement; +template class LinearSimplexElement; template class LinearSimplexElement; template class LinearSimplexElement; diff --git a/multibody/fem/volumetric_model.h b/multibody/fem/volumetric_model.h index ee0c47515341..cde915024170 100644 --- a/multibody/fem/volumetric_model.h +++ b/multibody/fem/volumetric_model.h @@ -33,7 +33,9 @@ class VolumetricModel : public FemModelImpl { static_assert( std::is_same_v< VolumetricElement, + typename Element::Quadrature, ConstitutiveModel, + typename Element::SubdIsoparametricElement, + typename Element::SubdQuadrature>, Element>, "The template parameter `Element` must be of type VolumetricElement."); diff --git a/multibody/plant/deformable_model.cc b/multibody/plant/deformable_model.cc index c37a34f354de..b683d6291297 100644 --- a/multibody/plant/deformable_model.cc +++ b/multibody/plant/deformable_model.cc @@ -10,6 +10,7 @@ #include "drake/multibody/fem/linear_corotated_model.h" #include "drake/multibody/fem/linear_simplex_element.h" #include "drake/multibody/fem/simplex_gaussian_quadrature.h" +#include "drake/multibody/fem/tet_subdivision_quadrature.h" #include "drake/multibody/fem/volumetric_model.h" #include "drake/multibody/plant/multibody_plant.h" @@ -352,16 +353,15 @@ DeformableModel::BuildLinearVolumetricModel( } switch (config.material_model()) { case MaterialModel::kLinear: - BuildLinearVolumetricModelHelper( - id, mesh, config); + ConstitutiveModelHelper(id, mesh, + config); break; case MaterialModel::kCorotated: - BuildLinearVolumetricModelHelper(id, mesh, - config); + ConstitutiveModelHelper(id, mesh, config); break; case MaterialModel::kLinearCorotated: - BuildLinearVolumetricModelHelper( - id, mesh, config); + ConstitutiveModelHelper(id, mesh, + config); break; } } @@ -369,7 +369,32 @@ DeformableModel::BuildLinearVolumetricModel( template template