chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 4 Packages Projects Releases Wiki Activity

- Added passive forces plugins

Browse Source 
- 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
2022-10-10 02:45:59 -07:00
committed by Copybara-Service
parent 794ef0b771
commit e250ff0d5a
34 changed files with 1564 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+20
View File
@@ -23,6 +23,7 @@
#include "engine/engine_core_constraint.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_plugin.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
@@ -1422,6 +1423,25 @@ void mj_passive(const mjModel* m, mjData* d) {
if (mjcb_passive) {
mjcb_passive(m, d);
}
// plugin
if (m->nplugin) {
const int nslot = mjp_pluginCount();
// iterate over plugins, call compute if type is mjPLUGIN_PASSIVE
for (int i=0; i<m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
mju_error_i("invalid plugin slot: %d", slot);
}
if (plugin->type & mjPLUGIN_PASSIVE) {
if (!plugin->compute) {
mju_error_i("`compute` is a null function pointer for plugin at slot %d", slot);
}
plugin->compute(m, d, i, mjPLUGIN_PASSIVE);
}
}
}
}
src/engine/engine_io.c
+11 -7
View File
@@ -875,10 +875,14 @@ static mjData* _makeData(const mjModel* m) {
for (int i = 0; i < m->nplugin; ++i) {
d->plugin[i] = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]);
if (!plugin->init) {
mju_error_i("`init` is a null function pointer for plugin at slot %d", m->plugin[i]);
if (plugin->init) {
if (plugin->init(m, d, i) < 0) {
mju_free(d->buffer);
mju_free(d->arena);
mju_free(d);
mju_error_i("plugin->init failed for plugin id %d", i);
}
}
plugin->init(m, d, i);
}
return d;
@@ -1142,10 +1146,9 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
for (int i = 0; i < m->nplugin; ++i) {
d->plugin[i] = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]);
if (!plugin->reset) {
mju_error_i("`reset` is a null function pointer for plugin at slot %d", m->plugin[i]);
if (plugin->reset) {
plugin->reset(m, d, i);
}
plugin->reset(m, d, i);
}
}
@@ -1333,6 +1336,7 @@ const char* mj_validateReferences(const mjModel* m) {
X(body_jntadr, nbody, njnt , m->body_jntnum ) \
X(body_dofadr, nbody, nv , m->body_dofnum ) \
X(body_geomadr, nbody, ngeom , m->body_geomnum ) \
X(body_plugin, nbody, nplugin , 0 ) \
X(jnt_qposadr, njnt, nq , 0 ) \
X(jnt_dofadr, njnt, nv , 0 ) \
X(jnt_bodyid, njnt, nbody , 0 ) \
@@ -1364,7 +1368,7 @@ const char* mj_validateReferences(const mjModel* m) {
X(pair_geom2, npair, ngeom , 0 ) \
X(actuator_plugin, nu, nplugin , 0 ) \
X(sensor_plugin, nsensor, nplugin , 0 ) \
X(plugin_stateadr, nplugin, npluginstate , 0 ) \
X(plugin_stateadr, nplugin, npluginstate , m->plugin_statenum ) \
X(plugin_attradr, nplugin, npluginattr , 0 ) \
X(tendon_adr, ntendon, nwrap , m->tendon_num ) \
X(tendon_matid, ntendon, nmat , 0 ) \
src/engine/engine_util_blas.h
+1 -1
View File
@@ -91,7 +91,7 @@ MJAPI void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl);
// res = vec1 + vec2*scl
MJAPI void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtNum scl);
// normalize vector, return length before normalization
// normalize vector, return length before normalization, set to [1, 0, 0] if norm is tiny
MJAPI mjtNum mju_normalize3(mjtNum vec[3]);
// compute vector length (without normalizing)
src/user/user_composite.cc
+373 -36
View File
@@ -60,6 +60,11 @@ mjCComposite::mjCComposite(void) {
flatinertia = 0;
mj_defaultSolRefImp(solrefsmooth, solimpsmooth);
// cable
curve[0] = curve[1] = curve[2] = mjCOMPSHAPE_ZERO;
mjuu_setvec(size, 1, 0, 0);
initial = "ball";
// skin
skin = false;
skintexcoord = false;
@@ -122,6 +127,7 @@ void mjCComposite::SetDefault(void) {
type==mjCOMPTYPE_PARTICLE ||
type==mjCOMPTYPE_ROPE ||
type==mjCOMPTYPE_LOOP ||
type==mjCOMPTYPE_CABLE ||
(type==mjCOMPTYPE_GRID && tmpdim==1)) {
for (i=0; i<mjNCOMPKINDS; i++) {
def[i].geom.group = 0;
@@ -146,6 +152,7 @@ void mjCComposite::SetDefault(void) {
break;
case mjCOMPTYPE_CABLE: // cable
case mjCOMPTYPE_ROPE: // rope
break;
@@ -194,9 +201,10 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
}
// check geom type
if (def[0].geom.type!=mjGEOM_SPHERE &&
def[0].geom.type!=mjGEOM_CAPSULE &&
def[0].geom.type!=mjGEOM_ELLIPSOID) {
if ((def[0].geom.type!=mjGEOM_SPHERE &&
def[0].geom.type!=mjGEOM_CAPSULE &&
def[0].geom.type!=mjGEOM_ELLIPSOID) &&
type!=mjCOMPTYPE_CABLE) {
return comperr(error, "Composite geom type must be sphere, capsule or ellipsoid", error_sz);
}
@@ -213,8 +221,22 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
}
// check spacing
if (spacing<mjMINVAL) {
return comperr(error, "Positive spacing expected in composite", error_sz);
if (mjuu_dot3(size, size)<mjMINVAL && uservert.empty()) {
return comperr(error, "Positive spacing or length expected in composite", error_sz);
}
// check either spacing or length
if (spacing && type==mjCOMPTYPE_CABLE) {
return comperr(error, "Spacing is not supported by cable composite", error_sz);
}
// check either uservert or count but not both
if (!uservert.empty()) {
if (count[0]>1) {
return comperr(error, "Either vertex or count can be specified, not both", error_sz);
}
count[0] = uservert.size()/3;
count[1] = 1;
}
// determine dimensionality, check singleton order
@@ -231,7 +253,7 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
}
// require 3x3 for subgrid
if (skin && skinsubgrid>0) {
if (skin && skinsubgrid>0 && type!=mjCOMPTYPE_CABLE) {
if (count[0]<3 || count[1]<3) {
return comperr(error, "At least 3x3 required for skin subgrid", error_sz);
}
@@ -249,6 +271,9 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
case mjCOMPTYPE_LOOP:
return MakeRope(model, body, error, error_sz);
case mjCOMPTYPE_CABLE:
return MakeCable(model, body, error, error_sz);
case mjCOMPTYPE_CLOTH:
return MakeCloth(model, body, error, error_sz);
@@ -407,9 +432,9 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjCBody* body, char* error, int err
// skin
if (skin) {
if (skinsubgrid>0) {
MakeSkin2Subgrid(model);
MakeSkin2Subgrid(model, skininflate);
} else {
MakeSkin2(model);
MakeSkin2(model, skininflate);
}
}
@@ -418,6 +443,203 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjCBody* body, char* error, int err
bool mjCComposite::MakeCable(mjCModel* model, mjCBody* body, char* error, int error_sz) {
// check dim
if (dim!=1) {
return comperr(error, "Cable must be one-dimensional", error_sz);
}
// check geom type
if (def[0].geom.type!=mjGEOM_CYLINDER &&
def[0].geom.type!=mjGEOM_CAPSULE &&
def[0].geom.type!=mjGEOM_BOX) {
return comperr(error, "Cable geom type must be sphere, capsule or box", error_sz);
}
// add name to model
mjCText* pte = model->AddText();
pte->name = "composite_" + prefix;
pte->data = "rope_" + prefix;
// populate uservert if not specified
if (uservert.empty()) {
for (int ix=0; ix<count[0]; ix++) {
for (int k=0; k<3; k++) {
switch (curve[k]) {
case mjCOMPSHAPE_LINE:
uservert.push_back(ix*size[0]/(count[0]-1));
break;
case mjCOMPSHAPE_COS:
uservert.push_back(size[1]*cos(mjPI*ix*size[2]/(count[0]-1)));
break;
case mjCOMPSHAPE_SIN:
uservert.push_back(size[1]*sin(mjPI*ix*size[2]/(count[0]-1)));
break;
case mjCOMPSHAPE_ZERO:
uservert.push_back(0);
break;
default:
// SHOULD NOT OCCUR
mju_error_i("Invalid composite shape: %d", curve[k]);
break;
}
}
}
}
// create frame
mjtNum normal[3], prev_quat[4];
mjuu_setvec(normal, 0, 1, 0);
mjuu_setvec(prev_quat, 1, 0, 0, 0);
// add one body after the other
for (int ix=0; ix<count[0]-1; ix++) {
body = AddCableBody(model, body, ix, normal, prev_quat);
}
// add skin
if (def[0].geom.type==mjGEOM_BOX) {
if (skinsubgrid>0) {
count[1]+=2;
MakeSkin2Subgrid(model, 2*def[0].geom.size[2]);
count[1]-=2;
} else {
count[1]++;
MakeSkin2(model, 2*def[0].geom.size[2]);
count[1]--;
}
}
return true;
}
mjCBody* mjCComposite::AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]) {
char txt_geom[100], txt_site[100], txt_slide[100];
char this_body[100], next_body[100], this_joint[100];
mjtNum dquat[4], this_quat[4];
// set flags
int lastidx = count[0]-2;
bool first = ix==0;
bool last = ix==lastidx;
bool secondlast = ix==lastidx-1;
// compute edge and tangent vectors
mjtNum edge[3], tprev[3], tnext[3];
mjuu_setvec(edge, uservert[3*(ix+1)+0]-uservert[3*ix+0],
uservert[3*(ix+1)+1]-uservert[3*ix+1],
uservert[3*(ix+1)+2]-uservert[3*ix+2]);
if (!first) {
mjuu_setvec(tprev, uservert[3*ix+0]-uservert[3*(ix-1)+0],
uservert[3*ix+1]-uservert[3*(ix-1)+1],
uservert[3*ix+2]-uservert[3*(ix-1)+2]);
mjuu_normvec(tprev, 3);
}
if (!last) {
mjuu_setvec(tnext, uservert[3*(ix+2)+0]-uservert[3*(ix+1)+0],
uservert[3*(ix+2)+1]-uservert[3*(ix+1)+1],
uservert[3*(ix+2)+2]-uservert[3*(ix+1)+2]);
mjuu_normvec(tnext, 3);
}
// update moving frame
mjtNum length = mju_updateFrame(this_quat, normal, edge, tprev, tnext, first);
// create body, joint, and geom names
if (first) {
mju::sprintf_arr(this_body, "%sB_first", prefix.c_str());
mju::sprintf_arr(next_body, "%sB_%d", prefix.c_str(), ix+1);
mju::sprintf_arr(this_joint, "%sJ_first", prefix.c_str());
mju::sprintf_arr(txt_site, "%sS_first", prefix.c_str());
} else if (last) {
mju::sprintf_arr(this_body, "%sB_last", prefix.c_str());
mju::sprintf_arr(next_body, "%sB_first", prefix.c_str());
mju::sprintf_arr(this_joint, "%sJ_last", prefix.c_str());
mju::sprintf_arr(txt_site, "%sS_last", prefix.c_str());
} else if (secondlast){
mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix);
mju::sprintf_arr(next_body, "%sB_last", prefix.c_str());
mju::sprintf_arr(this_joint, "%sJ_%d", prefix.c_str(), ix);
} else {
mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix);
mju::sprintf_arr(next_body, "%sB_%d", prefix.c_str(), ix+1);
mju::sprintf_arr(this_joint, "%sJ_%d", prefix.c_str(), ix);
}
mju::sprintf_arr(txt_geom, "%sG%d", prefix.c_str(), ix);
mju::sprintf_arr(txt_slide, "%sJs%d", prefix.c_str(), ix);
// add body
body = body->AddBody();
body->name = this_body;
if (first) {
mjuu_setvec(body->pos, offset[0]+uservert[3*ix],
offset[1]+uservert[3*ix+1],
offset[2]+uservert[3*ix+2]);
mjuu_copyvec(body->quat, this_quat, 4);
} else {
mjuu_setvec(body->pos, length, 0, 0);
mjtNum negquat[4] = {prev_quat[0], -prev_quat[1], -prev_quat[2], -prev_quat[3]};
mjuu_mulquat(dquat, negquat, this_quat);
mjuu_copyvec(body->quat, dquat, 4);
}
// add geom
mjCGeom* geom = body->AddGeom(def);
geom->def = body->def;
geom->name = txt_geom;
if (def[0].geom.type==mjGEOM_CYLINDER ||
def[0].geom.type==mjGEOM_CAPSULE) {
mjuu_zerovec(geom->fromto, 6);
geom->fromto[3] = length;
} else if (def[0].geom.type==mjGEOM_BOX) {
mjuu_zerovec(geom->pos, 3);
geom->pos[0] = length/2;
geom->size[0] = length/2;
}
// add plugin
if (plugin_instance) {
body->is_plugin = true;
body->plugin_name = plugin_name;
body->plugin_instance = plugin_instance;
body->plugin_instance_name = plugin_instance_name;
}
// update orientation
mjuu_copyvec(prev_quat, this_quat, 4);
// add curvature joint
if (!first || strcmp(initial.c_str(), "none")) {
mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_JOINT);
jnt->def = body->def;
jnt->type = (first && strcmp(initial.c_str(), "free")==0) ? mjJNT_FREE : mjJNT_BALL;
jnt->damping = jnt->type==mjJNT_FREE ? 0 : jnt->damping;
jnt->armature = jnt->type==mjJNT_FREE ? 0 : jnt->armature;
jnt->frictionloss = jnt->type==mjJNT_FREE ? 0 : jnt->frictionloss;
jnt->name = this_joint;
}
// exclude contact pair
if (!last) {
mjCBodyPair* exclude = model->AddExclude();
exclude->bodyname1 = this_body;
exclude->bodyname2 = next_body;
}
// add site at the boundary
if (last || first) {
mjCSite* site = body->AddSite(def);
site->def = body->def;
site->name = txt_site;
mjuu_setvec(site->pos, last ? length : 0, 0, 0);
mjuu_setvec(site->quat, 1, 0, 0, 0);
}
return body;
}
// make rope
bool mjCComposite::MakeRope(mjCModel* model, mjCBody* body, char* error, int error_sz) {
// check dim
@@ -709,9 +931,9 @@ bool mjCComposite::MakeCloth(mjCModel* model, mjCBody* body, char* error, int er
// skin
if (skin) {
if (skinsubgrid>0) {
MakeSkin2Subgrid(model);
MakeSkin2Subgrid(model, skininflate);
} else {
MakeSkin2(model);
MakeSkin2(model, skininflate);
}
}
@@ -1043,7 +1265,7 @@ void mjCComposite::MakeShear(mjCModel* model) {
// add skin to 2D
void mjCComposite::MakeSkin2(mjCModel* model) {
void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) {
char txt[100];
int N = count[0]*count[1];
@@ -1053,7 +1275,7 @@ void mjCComposite::MakeSkin2(mjCModel* model) {
skin->name = txt;
skin->material = skinmaterial;
mjuu_copyvec(skin->rgba, skinrgba, 4);
skin->inflate = skininflate;
skin->inflate = inflate;
skin->group = skingroup;
// populate mesh: two sides
@@ -1129,6 +1351,21 @@ void mjCComposite::MakeSkin2(mjCModel* model) {
skin->face.push_back(N + iy+1 + (count[0]-1)*count[1]);
}
// couple with bones
if (type==mjCOMPTYPE_CLOTH || type==mjCOMPTYPE_GRID) {
MakeClothBones(model, skin);
} else if (type==mjCOMPTYPE_CABLE) {
MakeCableBones(model, skin);
}
}
// add bones in 2D
void mjCComposite::MakeClothBones(mjCModel* model, mjCSkin* skin) {
char txt[100];
int N = count[0]*count[1];
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
@@ -1159,6 +1396,124 @@ void mjCComposite::MakeSkin2(mjCModel* model) {
}
void mjCComposite::MakeClothBonesSubgrid(mjCModel* model, mjCSkin* skin) {
char txt[100];
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
// body name
mju::sprintf_arr(txt, "%sB%d_%d", prefix.c_str(), ix, iy);
// bind pose
skin->bodyname.push_back(txt);
skin->bindpos.push_back(ix*spacing);
skin->bindpos.push_back(iy*spacing);
skin->bindpos.push_back(0);
skin->bindquat.push_back(1);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
// empty vertid and vertweight
vector<int> vertid;
vector<float> vertweight;
skin->vertid.push_back(vertid);
skin->vertweight.push_back(vertweight);
}
}
}
// add bones to 1D
void mjCComposite::MakeCableBones(mjCModel* model, mjCSkin* skin) {
char this_body[100];
int N = count[0]*count[1];
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
// body name
if (ix==0) {
mju::sprintf_arr(this_body, "%sB_first", prefix.c_str());
} else if (ix>=count[0]-2) {
mju::sprintf_arr(this_body, "%sB_last", prefix.c_str());
} else {
mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix);
}
// bind pose
if (iy==0) {
skin->bodyname.push_back(this_body);
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(-def[0].geom.size[1]);
skin->bindpos.push_back(0);
skin->bindquat.push_back(1); skin->bindquat.push_back(0);
skin->bindquat.push_back(0); skin->bindquat.push_back(0);
} else {
skin->bodyname.push_back(this_body);
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(def[0].geom.size[1]);
skin->bindpos.push_back(0);
skin->bindquat.push_back(1); skin->bindquat.push_back(0);
skin->bindquat.push_back(0); skin->bindquat.push_back(0);
}
// create vertid and vertweight
skin->vertid.push_back({ix*count[1]+iy, N + ix*count[1]+iy});
skin->vertweight.push_back({1, 1});
}
}
}
void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjCSkin* skin) {
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
char txt[100];
// body name
if (ix==0) {
mju::sprintf_arr(txt, "%sB_first", prefix.c_str());
} else if (ix>=count[0]-2) {
mju::sprintf_arr(txt, "%sB_last", prefix.c_str());
} else {
mju::sprintf_arr(txt, "%sB_%d", prefix.c_str(), ix);
}
// bind pose
if (iy==0) {
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(-def[0].geom.size[1]);
skin->bindpos.push_back(0);
} else if (iy==2) {
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(def[0].geom.size[1]);
skin->bindpos.push_back(0);
} else {
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(0);
skin->bindpos.push_back(0);
}
skin->bodyname.push_back(txt);
skin->bindquat.push_back(1);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
// empty vertid and vertweight
skin->vertid.push_back({});
skin->vertweight.push_back({});
}
}
}
//------------------------------------- subgrid matrices
// C = W * [f; f_x; f_y; f_xy]
@@ -1390,7 +1745,7 @@ static const mjtNum subD22[] = {
// add skin to 2D, with subgrid
void mjCComposite::MakeSkin2Subgrid(mjCModel* model) {
void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
// assemble pointers to Dxx matrices
const mjtNum* Dp[3][3] = {
{subD00, subD01, subD02},
@@ -1474,7 +1829,7 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model) {
skin->name = txt;
skin->material = skinmaterial;
mjuu_copyvec(skin->rgba, skinrgba, 4);
skin->inflate = skininflate;
skin->inflate = inflate;
skin->group = skingroup;
// populate mesh: two sides
@@ -1554,28 +1909,10 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model) {
skin->face.push_back(NN + iy+1 + (C0-1)*C1);
}
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
// body name
mju::sprintf_arr(txt, "%sB%d_%d", prefix.c_str(), ix, iy);
// bind pose
skin->bodyname.push_back(txt);
skin->bindpos.push_back(ix*spacing);
skin->bindpos.push_back(iy*spacing);
skin->bindpos.push_back(0);
skin->bindquat.push_back(1);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
// empty vertid and vertweight
vector<int> vertid;
vector<float> vertweight;
skin->vertid.push_back(vertid);
skin->vertweight.push_back(vertweight);
}
if (type==mjCOMPTYPE_CLOTH || type==mjCOMPTYPE_GRID) {
MakeClothBonesSubgrid(model, skin);
} else if (type==mjCOMPTYPE_CABLE) {
MakeCableBonesSubgrid(model, skin);
}
// bind vertices to bones: one big square at a time
src/user/user_composite.h
+36 -5
View File
@@ -26,6 +26,7 @@
typedef enum _mjtCompType {
mjCOMPTYPE_PARTICLE = 0,
mjCOMPTYPE_GRID,
mjCOMPTYPE_CABLE,
mjCOMPTYPE_ROPE,
mjCOMPTYPE_LOOP,
mjCOMPTYPE_CLOTH,
@@ -48,6 +49,16 @@ typedef enum _mjtCompKind {
} mjtCompKind;
typedef enum _mjtCompShape {
mjCOMPSHAPE_LINE = 0,
mjCOMPSHAPE_COS,
mjCOMPSHAPE_SIN,
mjCOMPSHAPE_ZERO,
mjNCOMPSHAPES
} mjtCompShape;
class mjCComposite {
public:
mjCComposite(void);
@@ -60,19 +71,22 @@ class mjCComposite {
bool MakeParticle(mjCModel* model, mjCBody* body, char* error, int error_sz);
bool MakeGrid(mjCModel* model, mjCBody* body, char* error, int error_sz);
bool MakeRope(mjCModel* model, mjCBody* body, char* error, int error_sz);
bool MakeCable(mjCModel* model, mjCBody* body, char* error, int error_sz);
bool MakeCloth(mjCModel* model, mjCBody* body, char* error, int error_sz);
bool MakeBox(mjCModel* model, mjCBody* body, char* error, int error_sz);
void MakeShear(mjCModel* model);
void MakeSkin2(mjCModel* model);
void MakeSkin2Subgrid(mjCModel* model);
void MakeSkin2(mjCModel* model, mjtNum inflate);
void MakeSkin2Subgrid(mjCModel* model, mjtNum inflate);
void MakeClothBones(mjCModel* model, mjCSkin* skin);
void MakeClothBonesSubgrid(mjCModel* model, mjCSkin* skin);
void MakeCableBones(mjCModel* model, mjCSkin* skin);
void MakeCableBonesSubgrid(mjCModel* model, mjCSkin* skin);
void MakeSkin3(mjCModel* model);
void MakeSkin3Box(mjCSkin* skin, int c0, int c1, int side, int& vcnt, const char* format);
void MakeSkin3Smooth(mjCSkin* skin, int c0, int c1, int side,
const std::map<std::string, int>& vmap, const char* format);
mjCBody* AddClothBody(mjCModel* model, mjCBody* body, int ix, int iy, int ix1, int iy1);
mjCBody* AddRopeBody(mjCModel* model, mjCBody* body, int ix, int ix1);
void BoxProject(double* pos);
@@ -87,6 +101,18 @@ class mjCComposite {
mjtNum solrefsmooth[mjNREF]; // solref for smoothing equality
mjtNum solimpsmooth[mjNIMP]; // solimp for smoothing equality
// currently used only for cable
std::string initial; // root boundary type
std::vector<float> uservert; // user-specified vertex positions
mjtNum size[3]; // rope size (meaning depends on the shape)
mjtCompShape curve[3]; // geometric shape
// plugin support
bool is_plugin;
std::string plugin_name;
std::string plugin_instance_name;
mjCPlugin* plugin_instance;
// skin
bool skin; // generate skin
bool skintexcoord; // generate texture coordinates
@@ -102,6 +128,11 @@ class mjCComposite {
// computed internally
int dim; // dimensionality
private:
mjCBody* AddRopeBody(mjCModel* model, mjCBody* body, int ix, int ix1);
mjCBody* AddClothBody(mjCModel* model, mjCBody* body, int ix, int iy, int ix1, int iy1);
mjCBody* AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]);
};
#endif // MUJOCO_SRC_USER_USER_COMPOSITE_H_
src/user/user_model.cc
+9 -1
View File
@@ -2601,7 +2601,14 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
}
}
// set sensor_plugin to the plugin instance ID
for (int i = 0; i < nbody; ++i) {
if (bodies[i]->is_plugin) {
m->body_plugin[i] = bodies[i]->plugin_instance->id;
} else {
m->body_plugin[i] = -1;
}
}
std::vector<std::vector<int>> plugin_to_sensors(nplugin);
for (int i = 0; i < nsensor; ++i) {
if (sensors[i]->type == mjSENS_PLUGIN) {
@@ -2623,6 +2630,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
}
int nstate = plugin->nstate(m, i);
m->plugin_stateadr[i] = stateadr;
m->plugin_statenum[i] = nstate;
stateadr += nstate;
if (plugin->type & mjPLUGIN_SENSOR) {
for (int sensor_id : plugin_to_sensors[i]) {
src/user/user_objects.cc
+21
View File
@@ -328,6 +328,12 @@ mjCBody::mjCBody(mjCModel* _model) {
lastdof = -1;
userdata.clear();
// plugin variables
is_plugin = false;
plugin_instance = nullptr;
plugin_name = "";
plugin_instance_name = "";
// clear object lists
bodies.clear();
geoms.clear();
@@ -799,6 +805,21 @@ void mjCBody::Compile(void) {
// compile all lights
for (i=0; i<lights.size(); i++) lights[i]->Compile();
// plugin
if (is_plugin) {
if (plugin_name.empty() && plugin_instance_name.empty()) {
throw mjCError(
this, "neither 'plugin' nor 'instance' is specified for body '%s', (id = %d)",
name.c_str(), id);
}
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
if (!(plugin->type & mjPLUGIN_PASSIVE)) {
throw mjCError(this, "plugin '%s' does not support passive forces", plugin->name);
}
}
}
src/user/user_objects.h
+8 -1
View File
@@ -118,6 +118,7 @@ class mjCAlternative {
//------------------------- class mjCBase ----------------------------------------------------------
// Generic functionality for all derived classes
class mjCPlugin;
class mjCBase {
friend class mjCDef;
@@ -127,10 +128,16 @@ class mjCBase {
int id; // object id
int xmlpos[2]; // row and column in xml file
mjCDef* def; // defaults class used to init this object
mjCModel* model; // pointer to model that created object
// plugin support
bool is_plugin;
std::string plugin_name;
std::string plugin_instance_name;
mjCPlugin* plugin_instance;
protected:
mjCBase(); // constructor
mjCModel* model; // pointer to model that created object
};
src/user/user_util.cc
+50
View File
@@ -23,6 +23,7 @@
#include <mujoco/mjtnum.h>
#include "engine/engine_macro.h"
#include "engine/engine_util_spatial.h"
using std::isnan;
using std::string;
@@ -461,6 +462,55 @@ void mjuu_visccoef(double* visccoef, double mass, const double* inertia, double
}
// update moving frame along a curve or initialize it, returns edge length
// inputs:
// normal - normal vector computed by a previous call to the function
// edge - edge vector (non-unit tangent vector)
// tprv - unit tangent vector of previous body
// tnxt - unit tangent vector of next body
// first - 1 if the frame requires initialization
// outputs:
// quat - frame orientation
// normal - unit normal vector
mjtNum mju_updateFrame(mjtNum quat[4], mjtNum normal[3], const mjtNum edge[3],
const mjtNum tprv[3], const mjtNum tnxt[3], int first) {
mjtNum tangent[3], binormal[3];
// normalize tangent
mjuu_copyvec(tangent, edge, 3);
mjuu_normvec(tangent, 3);
// compute moving frame
if (first) {
// use the first vertex binormal for the first edge
mjuu_crossvec(binormal, tangent, tnxt);
mjuu_normvec(binormal, 3);
// compute edge normal given tangent and binormal
mjuu_crossvec(normal, binormal, tangent);
mjuu_normvec(normal, 3);
} else {
mjtNum darboux[4];
// rotate edge normal about the vertex binormal
mjuu_crossvec(binormal, tprv, tangent);
mjtNum angle = atan2(mjuu_normvec(binormal, 3), mjuu_dot3(tprv, tangent));
mju_axisAngle2Quat(darboux, binormal, angle);
mju_rotVecQuat(normal, normal, darboux);
mjuu_normvec(normal, 3);
// compute edge binormal given tangent and normal
mjuu_crossvec(binormal, tangent, normal);
mjuu_normvec(binormal, 3);
}
// global orientation of the frame
mjuu_frame2quat(quat, tangent, normal, binormal);
// return edge length
return sqrt(mjuu_dot3(edge, edge));
}
// strip directory from filename
string mjuu_strippath(string filename) {
// find last pathsymbol
src/user/user_util.h
+13
View File
@@ -124,6 +124,19 @@ void mjuu_offcenter(double* res, const double mass, const double* vec);
// compute viscosity coefficients from mass and inertia
void mjuu_visccoef(double* visccoef, double mass, const double* inertia, double scl=1);
// update moving frame along a discrete curve or initialize it, returns edge length
// inputs:
// normal - normal vector computed by a previous call to the function
// edge - edge vector (non-unit tangent vector)
// tprv - unit tangent vector of previous body
// tnxt - unit tangent vector of next body
// first - 1 if the frame requires initialization
// outputs:
// quat - frame orientation
// normal - unit normal vector
double mju_updateFrame(double quat[4], double normal[3], const double edge[3],
const double tprv[3], const double tnxt[3], int first);
// strip path from filename
std::string mjuu_strippath(std::string filename);
src/xml/xml_native_reader.cc
+72 -5
View File
@@ -79,7 +79,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) {
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 183;
static const int nMJCF = 191;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -230,6 +230,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"body", "R", "10", "name", "childclass", "pos", "quat", "mocap",
"axisangle", "xyaxes", "zaxis", "euler", "user"},
{"<"},
{"plugin", "*", "3", "name", "plugin", "instance"},
{"<"},
{"config", "*", "2", "key", "value"},
{">"},
{"inertial", "?", "9", "pos", "quat", "mass", "diaginertia",
"axisangle", "xyaxes", "zaxis", "euler", "fullinertia"},
{"joint", "*", "21", "name", "class", "type", "group", "pos", "axis",
@@ -251,9 +255,14 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"light", "*", "15", "name", "class", "directional", "castshadow", "active",
"pos", "dir", "attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular",
"mode", "target"},
{"composite", "*", "8", "prefix", "type", "count", "spacing", "offset",
"flatinertia", "solrefsmooth", "solimpsmooth"},
{"composite", "*", "13", "prefix", "type", "count", "spacing", "offset",
"flatinertia", "solrefsmooth", "solimpsmooth", "vertex",
"flat", "initial", "curve", "size"},
{"<"},
{"plugin", "*", "3", "name", "plugin", "instance"},
{"<"},
{"config", "*", "2", "key", "value"},
{">"},
{"joint", "*", "15", "kind", "group", "stiffness", "damping", "armature",
"solreffix", "solimpfix",
"limited", "range", "margin", "solreflimit", "solimplimit",
@@ -652,6 +661,7 @@ const mjMap comp_map[mjNCOMPTYPES] = {
{"grid", mjCOMPTYPE_GRID},
{"rope", mjCOMPTYPE_ROPE},
{"loop", mjCOMPTYPE_LOOP},
{"cable", mjCOMPTYPE_CABLE},
{"cloth", mjCOMPTYPE_CLOTH},
{"box", mjCOMPTYPE_BOX},
{"cylinder", mjCOMPTYPE_CYLINDER},
@@ -667,6 +677,15 @@ const mjMap jkind_map[3] = {
};
// composite rope shape
const mjMap shape_map[mjNCOMPSHAPES] = {
{"s", mjCOMPSHAPE_LINE},
{"cos(s)", mjCOMPSHAPE_COS},
{"sin(s)", mjCOMPSHAPE_SIN},
{"0", mjCOMPSHAPE_ZERO}
};
// composite tendon kind
const mjMap tkind_map[2] = {
{"main", mjCOMPKIND_TENDON},
@@ -1859,11 +1878,46 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) {
if (MapValue(elem, "type", &n, comp_map, mjNCOMPTYPES, true)) {
comp.type = (mjtCompType)n;
}
ReadAttr(elem, "count", 3, comp.count, text, true, false);
ReadAttr(elem, "spacing", 1, &comp.spacing, text, true);
ReadAttr(elem, "count", 3, comp.count, text, false, false);
ReadAttr(elem, "spacing", 1, &comp.spacing, text, false);
ReadAttr(elem, "offset", 3, comp.offset, text);
ReadAttr(elem, "flatinertia", 1, &comp.flatinertia, text);
// plugin
XMLElement* eplugin = elem->FirstChildElement("plugin");
if (eplugin) {
ReadAttrTxt(eplugin, "plugin", comp.plugin_name);
ReadAttrTxt(eplugin, "instance", comp.plugin_instance_name);
if (comp.plugin_instance_name.empty()) {
comp.plugin_instance = model->AddPlugin();
comp.plugin_instance->name = "composite"+comp.prefix;
comp.plugin_instance_name = comp.plugin_instance->name;
} else {
model->hasImplicitPluginElem = true;
}
ReadPluginConfigs(eplugin, comp.plugin_instance);
}
// cable
std::string curves;
ReadAttrTxt(elem, "curve", curves);
ReadAttrTxt(elem, "initial", comp.initial);
ReadAttr(elem, "size", 3, comp.size, text, false, false);
if (ReadAttrTxt(elem, "vertex", text)){
String2Vector(text, comp.uservert);
}
// process curve string
std::istringstream iss(curves);
int i = 0;
while (iss) {
iss >> text;
comp.curve[i++] = (mjtCompShape)FindKey(shape_map, mjNCOMPSHAPES, text);
if (iss.eof()){
break;
}
};
// skin
XMLElement* eskin = elem->FirstChildElement("skin");
if (eskin) {
@@ -2619,6 +2673,19 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
OneLight(elem, plight);
}
// plugin sub-element
else if (name == "plugin") {
pbody->is_plugin = true;
ReadAttrTxt(elem, "plugin", pbody->plugin_name);
ReadAttrTxt(elem, "instance", pbody->plugin_instance_name);
if (pbody->plugin_instance_name.empty()) {
pbody->plugin_instance = model->AddPlugin();
} else {
model->hasImplicitPluginElem = true;
}
ReadPluginConfigs(elem, pbody->plugin_instance);
}
// composite sub-element
else if (name=="composite") {
// create composite and parse
src/xml/xml_native_writer.cc
+22
View File
@@ -1373,6 +1373,28 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
OneLight(InsertEnd(elem, "light"), body->lights[i], body->lights[i]->def);
}
// write plugin
if (body->is_plugin) {
XMLElement *child = InsertEnd(elem, "plugin");
if (!body->plugin_instance_name.empty()) {
WriteAttrTxt(child, "instance", body->plugin_instance_name);
} else {
WriteAttrTxt(child, "plugin", body->plugin_name);
const mjpPlugin* plugin = mjp_getPluginAtSlot(
body->plugin_instance->plugin_slot);
const char* c = &body->plugin_instance->flattened_attributes[0];
for (int i = 0; i < plugin->nattribute; ++i) {
std::string value(c);
if (!value.empty()) {
WriteAttrTxt(child, std::string("plugin:") + plugin->attributes[i],
value);
c += value.size();
}
++c;
}
}
}
// write child bodies recursively
for (i=0; i<body->bodies.size(); i++) {
Body(InsertEnd(elem, "body"), body->bodies[i]);