forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Implement DynamicElasticityModel and NewmarkScheme (RobotLocomotion#1…
…4619) Implement DynamicElasticityModel and NewmarkScheme - DynamicElasticityModel implements ElasticityModel. - NewmarkScheme implements StateUpdater. - Add a new method to StateUpdater, IntegrateTime. - Add a new helper method to FemElement, ExtractElementDofs. - Fix a bug in DynamicElasticityElement that the entire state (instead of the state dofs corresponding to the element) multiplies the mass/damping matrix when calculating the element residual.
- Loading branch information
1 parent
ed84c9f
commit db7995b
Showing
13 changed files
with
611 additions
and
40 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,96 @@ | ||
| #pragma once | ||
|
|
||
| #include <array> | ||
| #include <utility> | ||
|
|
||
| #include "drake/common/eigen_types.h" | ||
| #include "drake/geometry/proximity/volume_mesh.h" | ||
| #include "drake/multibody/fixed_fem/dev/damping_model.h" | ||
| #include "drake/multibody/fixed_fem/dev/elasticity_model.h" | ||
|
|
||
| namespace drake { | ||
| namespace multibody { | ||
| namespace fixed_fem { | ||
| /** The FEM model for dynamic 3D elasticity problems. Implements the interface | ||
| in FemModel. It is assumed that elements are only added to, but never deleted | ||
| from, the model. | ||
| @tparam Element The type of DynamicElasticityElement used in this | ||
| %DynamicElasticityModel, must be an instantiation of DynamicElasticityElement. | ||
| */ | ||
| template <class Element> | ||
| class DynamicElasticityModel : public ElasticityModel<Element> { | ||
| public: | ||
| DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(DynamicElasticityModel); | ||
|
|
||
| using T = typename Element::Traits::T; | ||
| using ConstitutiveModel = typename Element::Traits::ConstitutiveModel; | ||
|
|
||
| DynamicElasticityModel() = default; | ||
| ~DynamicElasticityModel() = default; | ||
|
|
||
| /** Add tetrahedral DynamicElasticityElements to the %DynamicElasticityModel | ||
| from a mesh. The positions of the vertices in the mesh are used as reference | ||
| positions for the volume as well as the generalized positions for the model | ||
| in MakeFemState(). | ||
| @param mesh The input tetrahedral mesh that describes the connectivity and | ||
| the positions of the vertices. Each geometry::VolumeElement in the input | ||
| `mesh` will generate a DynamicElasticityElement in this | ||
| %DynamicElasticityModel. | ||
| @param constitutive_model The ConstitutiveModel to be used for all the | ||
| DynamicElasticityElements created from the input `mesh`. | ||
| @param density The mass density of the new elements, in unit kg/m³. | ||
| @param damping_model The DampingModel to be used for all the | ||
| DynamicElasticityElements created from the input `mesh`. | ||
| @throw std::exception if Element::Traits::kNumNodes != 4. */ | ||
| void AddDynamicElasticityElementsFromTetMesh( | ||
| const geometry::VolumeMesh<T>& mesh, | ||
| const ConstitutiveModel& constitutive_model, const T& density, | ||
| const DampingModel<T>& damping_model) { | ||
| /* Alias for more readability. */ | ||
| constexpr int kDim = Element::Traits::kSolutionDimension; | ||
| constexpr int kNumNodes = Element::Traits::kNumNodes; | ||
| constexpr int kNumDofs = kDim * kNumNodes; | ||
| DRAKE_THROW_UNLESS(kNumNodes == 4); | ||
|
|
||
| using geometry::VolumeElementIndex; | ||
| /* Record the reference positions of the input mesh. The returned offset is | ||
| from before the new tets are added. */ | ||
| const NodeIndex node_index_offset(this->ParseTetMesh(mesh)); | ||
|
|
||
| /* Builds and adds new elements. */ | ||
| const VectorX<T>& X = this->reference_positions(); | ||
| std::array<NodeIndex, kNumNodes> element_node_indices; | ||
| for (VolumeElementIndex i(0); i < mesh.num_elements(); ++i) { | ||
| for (int j = 0; j < kNumNodes; ++j) { | ||
| /* To obtain the global node index, offset the local index of the nodes | ||
| in the mesh (starting from 0) by the existing number of nodes | ||
| before the current mesh is added. */ | ||
| const int node_id = mesh.element(i).vertex(j) + node_index_offset; | ||
| element_node_indices[j] = NodeIndex(node_id); | ||
| } | ||
| const Vector<T, kNumDofs> element_reference_positions = | ||
| Element::ExtractElementDofs(element_node_indices, X); | ||
| const ElementIndex next_element_index = | ||
| ElementIndex(this->num_elements()); | ||
| const auto& element_reference_positions_reshaped = | ||
| Eigen::Map<const Eigen::Matrix<T, kDim, kNumNodes>>( | ||
| element_reference_positions.data(), kDim, kNumNodes); | ||
| this->AddElement(next_element_index, element_node_indices, | ||
| constitutive_model, element_reference_positions_reshaped, | ||
| density, this->gravity(), damping_model); | ||
| } | ||
| } | ||
|
|
||
| private: | ||
| /* Implements FemModel::DoMakeFemState(). Generalized positions are | ||
| initialized to be reference positions of the input mesh vertices. Velocities | ||
| and accelerations are initialized to 0. */ | ||
| FemState<Element> DoMakeFemState() const final { | ||
| const VectorX<T>& X = this->reference_positions(); | ||
| return FemState<Element>(X, VectorX<T>::Zero(X.size()), | ||
| VectorX<T>::Zero(X.size())); | ||
| } | ||
| }; | ||
| } // namespace fixed_fem | ||
| } // namespace multibody | ||
| } // namespace drake |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,77 @@ | ||
| #pragma once | ||
|
|
||
| #include "drake/multibody/fixed_fem/dev/state_updater.h" | ||
|
|
||
| namespace drake { | ||
| namespace multibody { | ||
| namespace fixed_fem { | ||
| /** Implements the interface StateUpdater with Newmark-beta time integration | ||
| scheme. Given the value for the current time step accleration `a`, the states | ||
| are calculated from states from the previous time step according to the | ||
| following equations: | ||
| v = vₙ + dt ⋅ (γ ⋅ a + (1−γ) ⋅ aₙ) | ||
| x = xₙ + dt ⋅ vₙ + dt² ⋅ [β ⋅ a + (0.5−β) ⋅ aₙ]. | ||
| See [Newmark, 1959] for the original reference for the method. | ||
| [Newmark, 1959] Newmark, Nathan M. "A method of computation for structural | ||
| dynamics." Journal of the engineering mechanics division 85.3 (1959): 67-94. | ||
| @tparam State The type of FemState to be updated by the %NewmarkScheme. The | ||
| template parameter State must be an instantiation of FemState. | ||
| @pre State::ode_order() == 2. */ | ||
| template <class State> | ||
| class NewmarkScheme final : public StateUpdater<State> { | ||
| public: | ||
| DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(NewmarkScheme); | ||
|
|
||
| static_assert(State::ode_order() == 2); | ||
| using T = typename State::T; | ||
|
|
||
| /** Construct a Newmark scheme with the provided timestep, `gamma` and `beta`. | ||
| @pre dt > 0. | ||
| @pre 0 <= gamma <= 1. | ||
| @pre 0 <= beta <= 0.5. */ | ||
| NewmarkScheme(double dt, double gamma, double beta) | ||
| : dt_(dt), gamma_(gamma), beta_(beta) { | ||
| DRAKE_DEMAND(dt > 0); | ||
| DRAKE_DEMAND(0 <= gamma && gamma <= 1); | ||
| DRAKE_DEMAND(0 <= beta && beta <= 0.5); | ||
| } | ||
|
|
||
| ~NewmarkScheme() = default; | ||
|
|
||
| private: | ||
| /* Implements StateUpdater::weights(). */ | ||
| Vector3<T> do_get_weights() const final { | ||
| return {beta_ * dt_ * dt_, gamma_ * dt_, 1.0}; | ||
| } | ||
|
|
||
| /* Implements StateUpdater::DoUpdateState(). */ | ||
| void DoUpdateState(const VectorX<T>& dz, State* state) const final { | ||
| const VectorX<T>& a = state->qddot(); | ||
| const VectorX<T>& v = state->qdot(); | ||
| const VectorX<T>& x = state->q(); | ||
| state->set_qddot(a + dz); | ||
| state->set_qdot(v + dt_ * gamma_ * dz); | ||
| state->set_q(x + dt_ * dt_ * beta_ * dz); | ||
| } | ||
|
|
||
| /* Implements StateUpdater::DoAdvanceOneTimeStep(). */ | ||
| void DoAdvanceOneTimeStep(const State& prev_state, State* state) const final { | ||
| const VectorX<T>& an = prev_state.qddot(); | ||
| const VectorX<T>& vn = prev_state.qdot(); | ||
| const VectorX<T>& xn = prev_state.q(); | ||
| const VectorX<T>& a = state->qddot(); | ||
| state->set_qdot(vn + dt_ * (gamma_ * a + (1.0 - gamma_) * an)); | ||
| state->set_q(xn + dt_ * vn + dt_ * dt_ * (beta_ * a + (0.5 - beta_) * an)); | ||
| } | ||
|
|
||
| double dt_{0}; | ||
| double gamma_{0.5}; | ||
| double beta_{0.25}; | ||
| }; | ||
| } // namespace fixed_fem | ||
| } // namespace multibody | ||
| } // namespace drake |
Oops, something went wrong.