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

Particles with arbitrary geoms and joints.

Browse Source 
Fixes #553

PiperOrigin-RevId: 486614769
Change-Id: I9a09bf73da97b6b9746800a72ac6c97cce2efa92
This commit is contained in:
Alessio Quaglino
2022-11-07 03:21:36 -08:00
committed by Copybara-Service
parent fd4d47e4a5
commit 3b89b0fd30
10 changed files with 283 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_composite.cc
+48 -20
View File
@@ -102,6 +102,23 @@ void mjCComposite::AdjustSoft(mjtNum* solref, mjtNum* solimp, int level) {
// create the array of default joint options, append new elements only for particles type
bool mjCComposite::AddDefaultJoint(char* error, int error_sz) {
for (int i=0; i<mjNCOMPKINDS; i++) {
if (!defjoint[(mjtCompKind)i].empty() && type!=mjCOMPTYPE_PARTICLE) {
comperr(error, "Only particles are allowed to have multiple joints", error_sz);
return false;
} else {
mjCDef jnt;
jnt.joint.group = 3;
defjoint[(mjtCompKind)i].push_back(jnt);
}
}
return true;
}
// set defaults, after reading top-level info and skin
void mjCComposite::SetDefault(void) {
int i;
@@ -118,10 +135,12 @@ void mjCComposite::SetDefault(void) {
for (int i=0; i<mjNCOMPKINDS; i++) {
def[i].geom.group = 3;
def[i].site.group = 3;
def[i].joint.group = 3;
def[i].tendon.group = 3;
}
// set default joint
AddDefaultJoint();
// set default geom and tendon group to 0 if needed to be visible
if (!skin ||
type==mjCOMPTYPE_PARTICLE ||
@@ -204,7 +223,7 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
if ((def[0].geom.type!=mjGEOM_SPHERE &&
def[0].geom.type!=mjGEOM_CAPSULE &&
def[0].geom.type!=mjGEOM_ELLIPSOID) &&
type!=mjCOMPTYPE_CABLE) {
type!=mjCOMPTYPE_PARTICLE && type!=mjCOMPTYPE_CABLE) {
return comperr(error, "Composite geom type must be sphere, capsule or ellipsoid", error_sz);
}
@@ -303,20 +322,29 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int
b->pos[1] = offset[1] + spacing*(iy - 0.5*count[1]);
b->pos[2] = offset[2] + spacing*(iz - 0.5*count[2]);
// add slider joints
for (int i=0; i<3; i++) {
mjCJoint* jnt = b->AddJoint(def + mjCOMPKIND_JOINT, false);
jnt->def = body->def;
jnt->type = mjJNT_SLIDE;
mjuu_setvec(jnt->pos, 0, 0, 0);
mjuu_setvec(jnt->axis, 0, 0, 0);
jnt->axis[i] = 1;
// add slider joints if none defined
if (!add[mjCOMPKIND_PARTICLE]) {
for (int i=0; i<3; i++) {
mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0], false);
jnt->def = body->def;
jnt->type = mjJNT_SLIDE;
mjuu_setvec(jnt->pos, 0, 0, 0);
mjuu_setvec(jnt->axis, 0, 0, 0);
jnt->axis[i] = 1;
}
}
// add user-specified joints
else {
for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) {
mjCJoint* jnt = b->AddJoint(&defjnt, false);
jnt->def = body->def;
}
}
// add geom
mjCGeom* g = b->AddGeom(def);
g->def = body->def;
g->type = mjGEOM_SPHERE;
}
}
}
@@ -387,7 +415,7 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjCBody* body, char* error, int err
// add slider joint
mjCJoint* jnt[3];
for (int i=0; i<3; i++) {
jnt[i] = b->AddJoint(def + mjCOMPKIND_JOINT);
jnt[i] = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]);
jnt[i]->def = body->def;
mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), i, ix, iy);
jnt[i]->name = txt;
@@ -611,7 +639,7 @@ mjCBody* mjCComposite::AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtN
// add curvature joint
if (!first || strcmp(initial.c_str(), "none")) {
mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_JOINT);
mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]);
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;
@@ -754,7 +782,7 @@ mjCBody* mjCComposite::AddRopeBody(mjCModel* model, mjCBody* body, int ix, int i
// add main joint
for (int i=0; i<2; i++) {
// add joint
mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_JOINT);
mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]);
jnt->def = body->def;
mju::sprintf_arr(txt, "%sJ%d_%d", prefix.c_str(), i, ix1);
jnt->name = txt;
@@ -767,7 +795,7 @@ mjCBody* mjCComposite::AddRopeBody(mjCModel* model, mjCBody* body, int ix, int i
// add twist joint
if (add[mjCOMPKIND_TWIST]) {
// add joint
mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_TWIST);
mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_TWIST][0]);
jnt->def = body->def;
mju::sprintf_arr(txt, "%sJT%d", prefix.c_str(), ix1);
jnt->name = txt;
@@ -785,7 +813,7 @@ mjCBody* mjCComposite::AddRopeBody(mjCModel* model, mjCBody* body, int ix, int i
// add stretch joint
if (add[mjCOMPKIND_STRETCH]) {
// add joint
mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_STRETCH);
mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_STRETCH][0]);
jnt->def = body->def;
mju::sprintf_arr(txt, "%sJS%d", prefix.c_str(), ix1);
jnt->name = txt;
@@ -1008,7 +1036,7 @@ mjCBody* mjCComposite::AddClothBody(mjCModel* model, mjCBody* body,
// add main joint
for (int i=0; i<2; i++) {
// add joint
mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_JOINT);
mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]);
jnt->def = body->def;
mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), i, ix1, iy1);
jnt->name = txt;
@@ -1026,7 +1054,7 @@ mjCBody* mjCComposite::AddClothBody(mjCModel* model, mjCBody* body,
// add twist joint
if (add[mjCOMPKIND_TWIST]) {
mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_TWIST);
mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_TWIST][0]);
jnt->def = body->def;
mju::sprintf_arr(txt, "%sJT%d_%d", prefix.c_str(), ix1, iy1);
jnt->name = txt;
@@ -1048,7 +1076,7 @@ mjCBody* mjCComposite::AddClothBody(mjCModel* model, mjCBody* body,
// add stretch joint
if (add[mjCOMPKIND_STRETCH]) {
// add joint
mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_STRETCH);
mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_STRETCH][0]);
jnt->def = body->def;
mju::sprintf_arr(txt, "%sJS%d_%d", prefix.c_str(), ix1, iy1);
jnt->name = txt;
@@ -1174,7 +1202,7 @@ bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int erro
}
// add slider joint
mjCJoint* jnt = b->AddJoint(def + mjCOMPKIND_JOINT);
mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]);
jnt->def = body->def;
mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), ix, iy, iz);
jnt->name = txt;
src/user/user_composite.h
+6 -3
View File
@@ -15,8 +15,8 @@
#ifndef MUJOCO_SRC_USER_USER_COMPOSITE_H_
#define MUJOCO_SRC_USER_USER_COMPOSITE_H_
#include <map>
#include <string>
#include <unordered_map>
#include <vector>
#include <mujoco/mjmodel.h>
@@ -44,6 +44,7 @@ typedef enum _mjtCompKind {
mjCOMPKIND_STRETCH,
mjCOMPKIND_TENDON,
mjCOMPKIND_SHEAR,
mjCOMPKIND_PARTICLE,
mjNCOMPKINDS
} mjtCompKind;
@@ -64,6 +65,7 @@ class mjCComposite {
mjCComposite(void);
void SetDefault(void);
bool AddDefaultJoint(char* error = NULL, int error_sz = 0);
void AdjustSoft(mjtNum* solref, mjtNum* solimp, int level);
bool Make(mjCModel* model, mjCBody* body, char* error, int error_sz);
@@ -123,8 +125,9 @@ class mjCComposite {
int skingroup; // skin group of the composite object
// element options
bool add[mjNCOMPKINDS]; // add element
mjCDef def[mjNCOMPKINDS]; // defaults
bool add[mjNCOMPKINDS]; // add element
mjCDef def[mjNCOMPKINDS]; // default geom, site, tendon
std::unordered_map<mjtCompKind, std::vector<mjCDef> > defjoint; // default joints
// computed internally
int dim; // dimensionality
src/xml/xml_native_reader.cc
+37 -19
View File
@@ -263,8 +263,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"<"},
{"config", "*", "2", "key", "value"},
{">"},
{"joint", "*", "15", "kind", "group", "stiffness", "damping", "armature",
"solreffix", "solimpfix",
{"joint", "*", "17", "kind", "group", "stiffness", "damping", "armature",
"solreffix", "solimpfix", "type", "axis",
"limited", "range", "margin", "solreflimit", "solimplimit",
"frictionloss", "solreffriction", "solimpfriction"},
{"tendon", "*", "17", "kind", "group", "stiffness", "damping",
@@ -670,10 +670,11 @@ const mjMap comp_map[mjNCOMPTYPES] = {
// composite joint kind
const mjMap jkind_map[3] = {
const mjMap jkind_map[4] = {
{"main", mjCOMPKIND_JOINT},
{"twist", mjCOMPKIND_TWIST},
{"stretch", mjCOMPKIND_STRETCH}
{"stretch", mjCOMPKIND_STRETCH},
{"particle", mjCOMPKIND_PARTICLE}
};
@@ -1978,28 +1979,45 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) {
while (ejnt) {
// kind
int kind;
MapValue(ejnt, "kind", &kind, jkind_map, 3, true);
MapValue(ejnt, "kind", &kind, jkind_map, 4, true);
// create a new element if this kind already exists
if (comp.add[kind]) {
char error[200];
if (!comp.AddDefaultJoint(error, 200)) {
throw mjXError(elem, error);
}
}
comp.add[kind] = true;
// get element
mjCDef *el = &comp.defjoint[(mjtCompKind)kind].back();
// particle joint
if (MapValue(ejnt, "type", &n, joint_map, joint_sz)) {
el->joint.type = (mjtJoint)n;
}
ReadAttr(ejnt, "axis", 3, el->joint.axis, text);
// solreffix, solimpfix
ReadAttr(ejnt, "solreffix", mjNREF, comp.def[kind].equality.solref, text, false, false);
ReadAttr(ejnt, "solimpfix", mjNIMP, comp.def[kind].equality.solimp, text, false, false);
ReadAttr(ejnt, "solreffix", mjNREF, el->equality.solref, text, false, false);
ReadAttr(ejnt, "solimpfix", mjNIMP, el->equality.solimp, text, false, false);
// joint attributes
MapValue(elem, "limited", &comp.def[kind].joint.limited, TFAuto_map, 3);
ReadAttrInt(ejnt, "group", &comp.def[kind].joint.group);
ReadAttr(ejnt, "solreflimit", mjNREF, comp.def[kind].joint.solref_limit, text, false, false);
ReadAttr(ejnt, "solimplimit", mjNIMP, comp.def[kind].joint.solimp_limit, text, false, false);
MapValue(elem, "limited", &el->joint.limited, TFAuto_map, 3);
ReadAttrInt(ejnt, "group", &el->joint.group);
ReadAttr(ejnt, "solreflimit", mjNREF, el->joint.solref_limit, text, false, false);
ReadAttr(ejnt, "solimplimit", mjNIMP, el->joint.solimp_limit, text, false, false);
ReadAttr(ejnt,
"solreffriction", mjNREF, comp.def[kind].joint.solref_friction, text, false, false);
"solreffriction", mjNREF, el->joint.solref_friction, text, false, false);
ReadAttr(ejnt,
"solimpfriction", mjNIMP, comp.def[kind].joint.solimp_friction, text, false, false);
ReadAttr(ejnt, "stiffness", 1, &comp.def[kind].joint.stiffness, text);
ReadAttr(ejnt, "range", 2, comp.def[kind].joint.range, text);
ReadAttr(ejnt, "margin", 1, &comp.def[kind].joint.margin, text);
ReadAttr(ejnt, "armature", 1, &comp.def[kind].joint.armature, text);
ReadAttr(ejnt, "damping", 1, &comp.def[kind].joint.damping, text);
ReadAttr(ejnt, "frictionloss", 1, &comp.def[kind].joint.frictionloss, text);
"solimpfriction", mjNIMP, el->joint.solimp_friction, text, false, false);
ReadAttr(ejnt, "stiffness", 1, &el->joint.stiffness, text);
ReadAttr(ejnt, "range", 2, el->joint.range, text);
ReadAttr(ejnt, "margin", 1, &el->joint.margin, text);
ReadAttr(ejnt, "armature", 1, &el->joint.armature, text);
ReadAttr(ejnt, "damping", 1, &el->joint.damping, text);
ReadAttr(ejnt, "frictionloss", 1, &el->joint.frictionloss, text);
// advance
ejnt = ejnt->NextSiblingElement("joint");