- Added passive forces plugins
- Added new `cable` composite type: * The `initial` parameter specifies the joint at the starting boundary: `free`, `ball`, or `none`. * The boundary bodies are exposed with the names:`B_left` and `B_right`. * The vertex initial positions can be specified directly in the XML with the parameter `vertex`. * The orientation of the body frame **is** the orientation of the material frame of the curve. - Added new `cable` passive force plugin: * Twist and bending stiffness can be set separately with the parameters `twist` and `bend`. * The stress-free configuration can be set to be the initial one or flat with the flag `flat`. * New cable example showing the formation of plectoneme. * New coil example. * New belt example showing interaction between twist and anisotropy. * Added test using cantilever exact solution. PiperOrigin-RevId: 480033694 Change-Id: I491271bce8fccb185961477e903e5a72d172c8a3
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
794ef0b771
commit
e250ff0d5a
@@ -0,0 +1,29 @@
|
||||
# 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
|
||||
#
|
||||
# https://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.
|
||||
|
||||
set(MUJOCO_ELASTICITY_INCLUDE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../..
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../../src
|
||||
)
|
||||
|
||||
set(MUJOCO_ELASTICITY_SRCS
|
||||
cable.cc
|
||||
cable.h
|
||||
)
|
||||
|
||||
add_library(cable SHARED)
|
||||
target_sources(cable PRIVATE ${MUJOCO_ELASTICITY_SRCS})
|
||||
target_include_directories(cable PRIVATE ${MUJOCO_ELASTICITY_INCLUDE})
|
||||
target_link_libraries(cable PRIVATE mujoco)
|
||||
|
||||
@@ -0,0 +1,257 @@
|
||||
// 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 <algorithm>
|
||||
#include <cstddef>
|
||||
#include <sstream>
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "cable.h"
|
||||
|
||||
|
||||
namespace mujoco::plugin::elasticity {
|
||||
namespace {
|
||||
|
||||
// compute quaternion difference between two frames in joint coordinates
|
||||
void QuatDiff(mjtNum* quat, const mjtNum body_quat[4],
|
||||
const mjtNum joint_quat[4], bool pullback) {
|
||||
if (pullback == 0) {
|
||||
// contribution in local coordinates
|
||||
mju_mulQuat(quat, body_quat, joint_quat);
|
||||
} else {
|
||||
// contribution pulled-back in local coordinates of the other body
|
||||
mjtNum invquat[4];
|
||||
mju_mulQuat(invquat, body_quat, joint_quat);
|
||||
mju_negQuat(quat, invquat);
|
||||
}
|
||||
}
|
||||
|
||||
// compute local force given material properties, orientation,
|
||||
// and reference curvature
|
||||
// inputs:
|
||||
// stiffness - material parameters
|
||||
// quat - orientation of the body in local coordinates
|
||||
// omega0 - initial curvature
|
||||
// xquat - cartesian orientation of the body (optional)
|
||||
// scl - scaling of the force
|
||||
// outputs:
|
||||
// qfrc - local torque contribution
|
||||
void LocalForce(mjtNum qfrc[3], const mjtNum stiffness[4],
|
||||
const mjtNum quat[4], const mjtNum omega0[3],
|
||||
const mjtNum xquat[4], mjtNum scl) {
|
||||
mjtNum omega[3], lfrc[3];
|
||||
|
||||
// compute curvature
|
||||
mju_quat2Vel(omega, quat, scl);
|
||||
|
||||
// subtract omega0 in reference configuration
|
||||
mjtNum tmp[] = {
|
||||
- stiffness[0]*(omega[0] - omega0[0]) / stiffness[3],
|
||||
- stiffness[1]*(omega[1] - omega0[1]) / stiffness[3],
|
||||
- stiffness[2]*(omega[2] - omega0[2]) / stiffness[3],
|
||||
};
|
||||
|
||||
// rotate into global frame
|
||||
if (xquat) {
|
||||
mju_rotVecQuat(lfrc, tmp, xquat);
|
||||
} else {
|
||||
mju_copy3(lfrc, tmp);
|
||||
}
|
||||
|
||||
// add to total qfrc
|
||||
mju_addToScl3(qfrc, lfrc, scl);
|
||||
}
|
||||
|
||||
// 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<Elasticity> Elasticity::Create(
|
||||
const mjModel* m, mjData* d, int instance) {
|
||||
if (CheckAttr("twist", m, instance) && CheckAttr("bend", m, instance)) {
|
||||
return Elasticity(m, d, instance);
|
||||
} else {
|
||||
mju_warning("Invalid parameter specification in cable plugin");
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
|
||||
// plugin constructor
|
||||
Elasticity::Elasticity(const mjModel* m, mjData* d, int instance) {
|
||||
// parameters were validated by the factor function
|
||||
std::string flat = mj_getPluginConfig(m, instance, "flat");
|
||||
mjtNum G = strtod(mj_getPluginConfig(m, instance, "twist"), nullptr);
|
||||
mjtNum E = strtod(mj_getPluginConfig(m, instance, "bend"), nullptr);
|
||||
|
||||
// count plugin bodies
|
||||
n = 0;
|
||||
for (int i = 1; i < m->nbody; i++) {
|
||||
if (m->body_plugin[i] == instance) {
|
||||
if (!n++) {
|
||||
i0 = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// allocate arrays
|
||||
prev.assign(n, 0); // index of previous body
|
||||
next.assign(n, 0); // index of next body
|
||||
omega0.assign(3*n, 0); // reference curvature
|
||||
stiffness.assign(4*n, 0); // material parameters
|
||||
|
||||
// run forward kinematics to populate xquat (mjData not yet initialized)
|
||||
mju_zero(d->mocap_quat, 4*m->nmocap);
|
||||
mju_copy(d->qpos, m->qpos0, m->nq);
|
||||
mj_kinematics(m, d);
|
||||
|
||||
// compute initial curvature
|
||||
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");
|
||||
}
|
||||
bool first = (b == 0), last = (b == n-1);
|
||||
prev[b] = first ? 0 : -1;
|
||||
next[b] = last ? 0 : +1;
|
||||
|
||||
// compute omega0: curvature at equilibrium
|
||||
if (prev[b] && flat != "true") {
|
||||
int qadr = m->jnt_qposadr[m->body_jntadr[i]] + m->body_dofnum[i]-3;
|
||||
mju_subQuat(omega0.data()+3*b, m->body_quat+4*i, d->qpos+qadr);
|
||||
} else {
|
||||
mju_zero3(omega0.data()+3*b);
|
||||
}
|
||||
|
||||
// compute physical parameters
|
||||
int geom_i = m->body_geomadr[i];
|
||||
mjtNum J = 0, Iy = 0, Iz = 0;
|
||||
if (m->geom_type[geom_i] == mjGEOM_CYLINDER ||
|
||||
m->geom_type[geom_i] == mjGEOM_CAPSULE) {
|
||||
// https://en.wikipedia.org/wiki/Torsion_constant#Circle
|
||||
// https://en.wikipedia.org/wiki/List_of_second_moments_of_area
|
||||
J = mjPI * pow(m->geom_size[3*geom_i+0], 4) / 2;
|
||||
Iy = Iz = mjPI * pow(m->geom_size[3*geom_i+0], 4) / 4.;
|
||||
} else if (m->geom_type[geom_i] == mjGEOM_BOX) {
|
||||
// https://en.wikipedia.org/wiki/Torsion_constant#Rectangle
|
||||
// https://en.wikipedia.org/wiki/List_of_second_moments_of_area
|
||||
mjtNum h = m->geom_size[3*geom_i+1];
|
||||
mjtNum w = m->geom_size[3*geom_i+2];
|
||||
mjtNum a = std::max(h, w);
|
||||
mjtNum b = std::min(h, w);
|
||||
J = a*pow(b, 3)*(16./3.-3.36*b/a*(1-pow(b, 4)/pow(a, 4)/12));
|
||||
Iy = pow(2 * w, 3) * 2 * h / 12.;
|
||||
Iz = pow(2 * h, 3) * 2 * w / 12.;
|
||||
}
|
||||
stiffness[4*b+0] = J * G;
|
||||
stiffness[4*b+1] = Iy * E;
|
||||
stiffness[4*b+2] = Iz * E;
|
||||
stiffness[4*b+3] =
|
||||
prev[b] ? mju_dist3(d->xpos+3*i, d->xpos+3*(i+prev[b])) : 0;
|
||||
}
|
||||
}
|
||||
|
||||
void Elasticity::Compute(const mjModel* m, mjData* d, int instance) {
|
||||
for (int b = 0; b < n; b++) {
|
||||
// index into body array
|
||||
int i = i0 + b;
|
||||
if (m->body_plugin[i] != instance) {
|
||||
mju_error(
|
||||
"This body is not associated with the requested plugin instance");
|
||||
}
|
||||
|
||||
// if no stiffness, skip body
|
||||
if (!stiffness[b*4+0] && !stiffness[b*4+1] && !stiffness[b*4+2]) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// elastic forces
|
||||
mjtNum quat[4] = {0};
|
||||
mjtNum xfrc[3] = {0};
|
||||
|
||||
// local orientation
|
||||
if (prev[b]) {
|
||||
int qadr = m->jnt_qposadr[m->body_jntadr[i]] + m->body_dofnum[i]-3;
|
||||
QuatDiff(quat, m->body_quat+4*i, d->qpos+qadr, 0);
|
||||
|
||||
// contribution of orientation i-1 to xfrc i
|
||||
LocalForce(xfrc, stiffness.data()+4*b, quat, omega0.data()+3*b,
|
||||
d->xquat+4*(i+prev[b]), 1);
|
||||
}
|
||||
|
||||
if (next[b]) {
|
||||
int bn = b + next[b];
|
||||
int in = i + next[b];
|
||||
|
||||
// local orientation
|
||||
int qadr = m->jnt_qposadr[m->body_jntadr[in]] + m->body_dofnum[in]-3;
|
||||
QuatDiff(quat, m->body_quat+4*in, d->qpos+qadr, 1);
|
||||
|
||||
// contribution of orientation i+1 to xfrc i
|
||||
LocalForce(xfrc, stiffness.data()+4*bn, quat, omega0.data()+3*bn,
|
||||
d->xquat+4*i, -1);
|
||||
}
|
||||
|
||||
// convert from global coordinates and apply torque to com
|
||||
mj_applyFT(m, d, 0, xfrc, d->xpos+3*i, i, d->qfrc_passive);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
mjPLUGIN_DYNAMIC_LIBRARY_INIT {
|
||||
mjpPlugin plugin;
|
||||
mjp_defaultPlugin(&plugin);
|
||||
|
||||
plugin.name = "mujoco.elasticity.cable";
|
||||
plugin.type |= mjPLUGIN_PASSIVE;
|
||||
|
||||
const char* attributes[] = {"twist", "bend", "flat"};
|
||||
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 = Elasticity::Create(m, d, instance);
|
||||
if (!elasticity_or_null.has_value()) {
|
||||
return -1;
|
||||
}
|
||||
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
|
||||
new Elasticity(std::move(*elasticity_or_null)));
|
||||
return 0;
|
||||
};
|
||||
plugin.destroy = +[](mjData* d, int instance) {
|
||||
delete reinterpret_cast<Elasticity*>(d->plugin_data[instance]);
|
||||
d->plugin_data[instance] = 0;
|
||||
};
|
||||
plugin.compute = +[](const mjModel* m, mjData* d, int instance, int type) {
|
||||
auto* elasticity = reinterpret_cast<Elasticity*>(d->plugin_data[instance]);
|
||||
elasticity->Compute(m, d, instance);
|
||||
};
|
||||
|
||||
mjp_registerPlugin(&plugin);
|
||||
}
|
||||
|
||||
} // namespace mujoco::plugin::elasticity
|
||||
@@ -0,0 +1,52 @@
|
||||
// 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 MUJOCO_SRC_PLUGIN_ELASTICITY_CABLE_H_
|
||||
#define MUJOCO_SRC_PLUGIN_ELASTICITY_CABLE_H_
|
||||
|
||||
#include <optional>
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
|
||||
|
||||
namespace mujoco::plugin::elasticity {
|
||||
|
||||
class Elasticity {
|
||||
public:
|
||||
// Creates a new Elasticity instance (allocated with `new`) or
|
||||
// returns null on failure.
|
||||
static std::optional<Elasticity> Create(const mjModel* m, mjData* d,
|
||||
int instance);
|
||||
Elasticity(Elasticity&&) = default;
|
||||
~Elasticity() = default;
|
||||
|
||||
void Compute(const mjModel* m, mjData* d, int instance);
|
||||
|
||||
int i0; // index of first body
|
||||
int n; // number of bodies in the cable
|
||||
std::vector<int> prev; // indices of previous bodies (n x 1)
|
||||
std::vector<int> next; // indices of next bodies (n x 1)
|
||||
std::vector<mjtNum> stiffness; // stiffness parameters (n x 4)
|
||||
std::vector<mjtNum> omega0; // reference curvature (n x 3)
|
||||
|
||||
private:
|
||||
Elasticity(const mjModel* m, mjData* d, int instance);
|
||||
};
|
||||
|
||||
} // namespace mujoco::plugin::elasticity
|
||||
|
||||
#endif // MUJOCO_SRC_PLUGIN_ELASTICITY_CABLE_H_
|
||||
Reference in New Issue
Block a user