From 132426a9875baab2ef555d933b88d4ecb5fdc62f Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 27 Apr 2023 09:40:32 +0100 Subject: [PATCH 1/2] Add corotational plugin model --- model/plugin/cube.xml | 91 ++++++++++++ plugin/elasticity/CMakeLists.txt | 2 + plugin/elasticity/corotational.cc | 237 ++++++++++++++++++++++++++++++ plugin/elasticity/corotational.h | 46 ++++++ plugin/elasticity/elasticity.cc | 2 + 5 files changed, 378 insertions(+) create mode 100644 model/plugin/cube.xml create mode 100644 plugin/elasticity/corotational.cc create mode 100644 plugin/elasticity/corotational.h diff --git a/model/plugin/cube.xml b/model/plugin/cube.xml new file mode 100644 index 0000000000..fcd1633b87 --- /dev/null +++ b/model/plugin/cube.xml @@ -0,0 +1,91 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/plugin/elasticity/CMakeLists.txt b/plugin/elasticity/CMakeLists.txt index f35cf2d3b8..39a8d78c05 100644 --- a/plugin/elasticity/CMakeLists.txt +++ b/plugin/elasticity/CMakeLists.txt @@ -19,6 +19,8 @@ set(MUJOCO_ELASTICITY_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/../.. set(MUJOCO_ELASTICITY_SRCS cable.cc cable.h + corotational.cc + corotational.h elasticity.cc solid.cc solid.h diff --git a/plugin/elasticity/corotational.cc b/plugin/elasticity/corotational.cc new file mode 100644 index 0000000000..7c74046dac --- /dev/null +++ b/plugin/elasticity/corotational.cc @@ -0,0 +1,237 @@ + +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include +#include +#include +#include +#include +#include +#include "corotational.h" + +namespace mujoco::plugin::elasticity { +namespace { + +// Gauss Legendre quadrature points in 1 dimension on the interval [a, b] +void quadratureGaussLegendre(mjtNum* points, mjtNum* weights, + const int order, const mjtNum a, const mjtNum b) { + if (order > 2) + mju_error("Integration order > 2 not yet supported."); + // x is on [-1, 1], p on [a, b] + mjtNum p0 = (a+b)/2.; + mjtNum dpdx = (b-a)/2; + points[0] = -dpdx/sqrt(3) + p0; + points[1] = dpdx/sqrt(3) + p0; + weights[0] = dpdx; + weights[1] = dpdx; +} + +// evaluate 1-dimensional basis function +mjtNum phi(const mjtNum s, const mjtNum component) { + if (component == 0) { + return 1-s; + } else { + return s; + } +} + +// evaluate gradient fo 1-dimensional basis function +mjtNum dphi(const mjtNum s, const mjtNum component) { + if (component == 0) { + return -1; + } else { + return 1; + } +} + +// convert from joint to global coordinates +void evalFlexibleKinematics(mjtNum* jac, const int ncols) { + // col 0 + jac[0*ncols] = 1/sqrt(3); + jac[1*ncols] = 1/sqrt(3); + jac[2*ncols] = 1/sqrt(3); + // col 1 + jac[3*ncols+1] = -1/sqrt(3); + jac[4*ncols+1] = 1/sqrt(3); + jac[5*ncols+1] = 1/sqrt(3); + // col 14 + jac[18*ncols+14] = 1/sqrt(3); + jac[19*ncols+14] = 1/sqrt(3); + jac[20*ncols+14] = 1/sqrt(3); + // col 15 + jac[21*ncols+15] = -1/sqrt(3); + jac[22*ncols+15] = 1/sqrt(3); + jac[23*ncols+15] = 1/sqrt(3); + // identity + for (int k=0; k < 12; k++) { + jac[(k+6)*ncols + (k+2)] = 1; + } +} + +// reads numeric attributes +bool CheckAttr(const char* name, const mjModel* m, int instance) { + char *end; + std::string value = mj_getPluginConfig(m, instance, name); + value.erase(std::remove_if(value.begin(), value.end(), isspace), value.end()); + strtod(value.c_str(), &end); + return end == value.data() + value.size(); +} +} // namespace +// factory function +std::optional Corotational::Create( + const mjModel* m, mjData* d, int instance) { + if (CheckAttr("modulus", m, instance)) { + return Corotational(m, d, instance); + } else { + mju_warning("Invalid parameter specification in solid plugin"); + return std::nullopt; + } +} + +// plugin constructor +Corotational::Corotational(const mjModel* m, mjData* d, int instance) { + // parameters were validated by the factor function + mjtNum E = strtod(mj_getPluginConfig(m, instance, "modulus"), nullptr); + // count plugin bodies + n = 0; + for (int i = 1; i < m->nbody; i++) { + if (m->body_plugin[i] == instance) { + if (!n++) { + i0 = i; + } + } + } + // degrees of freedom + nrows = 3*n; + ncols = 16; + // quadrature order + int order = 2; + if (pow(order, 3) != n) { + mju_error("Order is not supported"); + } + // allocate arrays + std::vector stiffness(n); // material parameters + K.assign(nrows*nrows, 0); // stiffness matrix + jac.assign(nrows*ncols, 0); // jacobian matrix + // compute quadrature points + std::vector points(order); // quadrature points + std::vector weight(order); // quadrature weights + quadratureGaussLegendre(points.data(), weight.data(), order, 0, 1); + // compute stiffness + for (int b = 0; b < n; b++) { + int i = i0 + b; + if (m->body_plugin[i] != instance) { + mju_error("This body does not have the requested plugin instance"); + } + // compute physical parameters + stiffness[b] = E; + } + // compute stiffness matrix + mjtNum s, t, u, dvol; + std::vector F(nrows); + mju_zero(K.data(), nrows * nrows); + // loop over quadrature points + for (int ps=0; ps < order; ps++) { + for (int pt=0; pt < order; pt++) { + for (int pu=0; pu < order; pu++) { + s = points[ps]; + t = points[pt]; + u = points[pu]; + dvol = weight[ps]*weight[pt]*weight[pu]; + // cartesian product of basis functions + for (int bx=0; bx < order; bx++) { + for (int by=0; by < order; by++) { + for (int bz=0; bz < order; bz++) { + int basis_idx = (order*order*bx + order*by + bz); + F[3*basis_idx+0] = dphi(s, bx) * phi(t, by) * phi(u, bz); + F[3*basis_idx+1] = phi(s, bx) * dphi(t, by) * phi(u, bz); + F[3*basis_idx+2] = phi(s, bx) * phi(t, by) * dphi(u, bz); + } + } + } + // tensor contraction of the gradients of elastic strains + // (d(F+F')/dx : d(F+F')/dx) + for (int i=0; i < n; i++) { + for (int j=0; j < n; j++) { + for (int d=0; d < 3; d++) { + K[nrows*(3*i+d) + 3*j+d] -= + 1e2 * ( mju_dot(F.data()+3*i, F.data()+3*j, 3) + + mju_dot(F.data()+3*j, F.data()+3*i, 3) ) * dvol; + } + for (int k=0; k < 3; k++) { + for (int l=0; l < 3; l++) { + K[nrows*(3*i+k) + 3*j+l] -= + 1e2 * ( F[3*i+k]*F[3*j+l] + + F[3*j+k]*F[3*i+l] ) * dvol; + } + } + } + } + } + } + } + // compute kinematics + mju_zero(jac.data(), nrows * ncols); + evalFlexibleKinematics(jac.data(), ncols); +} + +void Corotational::Compute(const mjModel* m, mjData* d, int instance) { + int offset = m->nq - ncols; + mjtNum* xfrc_local = (mjtNum*) mju_malloc(sizeof(mjtNum)*nrows); + mjtNum* xpos_local = (mjtNum*) mju_malloc(sizeof(mjtNum)*nrows); + // elastic force as matrix-vector product + mju_mulMatVec(xpos_local, jac.data(), d->qpos + offset, nrows, ncols); + mju_mulMatVec(xfrc_local, K.data(), xpos_local, nrows, nrows); + mju_mulMatTVec( + d->qfrc_passive+offset - 1, jac.data(), xfrc_local, nrows, ncols); + mju_free(xfrc_local); + mju_free(xpos_local); +} + +void Corotational::RegisterPlugin() { + mjpPlugin plugin; + mjp_defaultPlugin(&plugin); + + plugin.name = "mujoco.elasticity.corotational"; + plugin.capabilityflags |= mjPLUGIN_PASSIVE; + + const char* attributes[] = {"modulus"}; + plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); + plugin.attributes = attributes; + plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; + + plugin.init = +[](const mjModel* m, mjData* d, int instance) { + auto elasticity_or_null = Corotational::Create(m, d, instance); + if (!elasticity_or_null.has_value()) { + return -1; + } + d->plugin_data[instance] = reinterpret_cast( + new Corotational(std::move(*elasticity_or_null))); + return 0; + }; + plugin.destroy = +[](mjData* d, int instance) { + delete reinterpret_cast(d->plugin_data[instance]); + d->plugin_data[instance] = 0; + }; + plugin.compute = + +[](const mjModel* m, mjData* d, int instance, int capability_bit) { + auto* elasticity = reinterpret_cast(d->plugin_data[instance]); + elasticity->Compute(m, d, instance); + }; + + mjp_registerPlugin(&plugin); +} + +} // namespace mujoco::plugin::elasticity diff --git a/plugin/elasticity/corotational.h b/plugin/elasticity/corotational.h new file mode 100644 index 0000000000..ed0c18a793 --- /dev/null +++ b/plugin/elasticity/corotational.h @@ -0,0 +1,46 @@ + +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef THIRD_PARTY_MUJOCO_PLUGIN_ELASTICITY_SOLID_H_ +#define THIRD_PARTY_MUJOCO_PLUGIN_ELASTICITY_SOLID_H_ +#include +#include +#include +#include +#include +namespace mujoco::plugin::elasticity { +class Corotational { + public: + // Creates a new Corotational instance (allocated with `new`) or + // returns null on failure. + static std::optional Create(const mjModel* m, mjData* d, + int instance); + Corotational(Corotational&&) = default; + ~Corotational() = default; + void Compute(const mjModel* m, mjData* d, int instance); + + static void RegisterPlugin(); + + int i0; // index of first body + int n; // number of bodies (vertices) + int nrows; // degrees of freedom + int ncols; // deformation modes + std::vector K; // stiffness matrix (nrows x nrows) + std::vector jac; // kinematic jacobian (nrows x ncols) + private: + Corotational(const mjModel* m, mjData* d, int instance); +}; +} // namespace mujoco::plugin::elasticity +#endif // THIRD_PARTY_MUJOCO_PLUGIN_ELASTICITY_SOLID_H_ diff --git a/plugin/elasticity/elasticity.cc b/plugin/elasticity/elasticity.cc index e349379fe6..863f07bbd1 100644 --- a/plugin/elasticity/elasticity.cc +++ b/plugin/elasticity/elasticity.cc @@ -14,12 +14,14 @@ #include #include "cable.h" +#include "corotational.h" #include "solid.h" namespace mujoco::plugin::elasticity { mjPLUGIN_LIB_INIT { Cable::RegisterPlugin(); + Corotational::RegisterPlugin(); Solid::RegisterPlugin(); } From 509caec9a630e992e7599b6c9615bc3eb84a9ffa Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 27 Apr 2023 09:59:03 +0100 Subject: [PATCH 2/2] increase plugin count in test --- test/plugin/elasticity/elasticity_test.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/plugin/elasticity/elasticity_test.cc b/test/plugin/elasticity/elasticity_test.cc index 2c72dc069a..5708ddbccb 100644 --- a/test/plugin/elasticity/elasticity_test.cc +++ b/test/plugin/elasticity/elasticity_test.cc @@ -79,7 +79,7 @@ TEST_F(PluginTest, ElasticEnergy) { ASSERT_THAT(m, testing::NotNull()) << error; mjData* d = mj_makeData(m); - EXPECT_THAT(mjp_pluginCount(), 2); + EXPECT_THAT(mjp_pluginCount(), 3); auto* solid = reinterpret_cast(d->plugin_data[0]); // check that if the entire geometry is rescaled by a factor "scale", then