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

Add frame element to MJCF.

Browse Source 
PiperOrigin-RevId: 585598244
Change-Id: I4c06be3dd586dab5a4458f05faaf1e7f2b615458
This commit is contained in:
Alessio Quaglino
2023-11-27 03:31:25 -08:00
committed by Copybara-Service
parent 8010aad7be
commit eb9568a48b
13 changed files with 411 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_model.cc
+3 -1
View File
@@ -1506,7 +1506,9 @@ void mjCModel::CopyTree(mjModel* m) {
if (rotfound ||
!IsNullPose(m->jnt_pos+3*jid, NULL) ||
((pj->type==mjJNT_HINGE || pj->type==mjJNT_SLIDE) &&
((pj->locaxis[0]!=0) + (pj->locaxis[1]!=0) + (pj->locaxis[2]!=0))>1)) {
((mju_abs(pj->locaxis[0])>mjEPS) +
(mju_abs(pj->locaxis[1])>mjEPS) +
(mju_abs(pj->locaxis[2])>mjEPS)) > 1)) {
m->body_simple[i] = 0;
}
src/user/user_objects.cc
+91 -1
View File
@@ -491,6 +491,7 @@ mjCBase::mjCBase() {
xmlpos[0] = xmlpos[1] = -1;
model = 0;
def = 0;
frame = nullptr;
// plugin variables
is_plugin = false;
@@ -534,6 +535,15 @@ std::string mjCBase::GetAssetContentType(std::string_view resource_name,
}
void mjCBase::SetFrame(mjCFrame* _frame) {
if (!_frame) {
return;
}
frame = _frame;
frame->Compile();
}
//------------------ class mjCBody implementation --------------------------------------------------
// constructor
@@ -574,6 +584,7 @@ mjCBody::mjCBody(mjCModel* _model) {
// clear object lists
bodies.clear();
geoms.clear();
frames.clear();
joints.clear();
sites.clear();
cameras.clear();
@@ -587,6 +598,7 @@ mjCBody::~mjCBody() {
// delete objects allocated here
for (int i=0; i<bodies.size(); i++) delete bodies[i];
for (int i=0; i<geoms.size(); i++) delete geoms[i];
for (int i=0; i<frames.size(); i++) delete frames[i];
for (int i=0; i<joints.size(); i++) delete joints[i];
for (int i=0; i<sites.size(); i++) delete sites[i];
for (int i=0; i<cameras.size(); i++) delete cameras[i];
@@ -594,6 +606,7 @@ mjCBody::~mjCBody() {
bodies.clear();
geoms.clear();
frames.clear();
joints.clear();
sites.clear();
cameras.clear();
@@ -616,6 +629,15 @@ mjCBody* mjCBody::AddBody(mjCDef* _def) {
// create new frame and add it to body
mjCFrame* mjCBody::AddFrame(mjCFrame* _frame) {
mjCFrame* obj = new mjCFrame(model, _frame ? _frame : NULL);
frames.push_back(obj);
return obj;
}
// create new joint and add it to body
// _def==NULL means no defaults, unlike all others which inherit from body
mjCJoint* mjCBody::AddJoint(mjCDef* _def, bool isfree) {
@@ -938,7 +960,7 @@ void mjCBody::Compile(void) {
GeomFrame();
}
// both pos and ipos undefiend: error
// both pos and ipos undefined: error
if (!mjuu_defined(ipos[0]) && !mjuu_defined(pos[0])) {
throw mjCError(this, "body pos and ipos are both undefined");
}
@@ -980,6 +1002,11 @@ void mjCBody::Compile(void) {
}
}
// frame
if (frame) {
mjuu_frameaccum(pos, quat, frame->pos, frame->quat);
}
// compute local frame rel. to parent body
if (id>0) {
model->bodies[parentid]->MakeLocal(locpos, locquat, pos, quat);
@@ -1076,6 +1103,39 @@ void mjCBody::Compile(void) {
//------------------ class mjCFrame implementation -------------------------------------------------
// initialize frame
mjCFrame::mjCFrame(mjCModel* _model, mjCFrame* _frame) {
compiled = false;
model = _model;
frame = _frame ? _frame : NULL;
mju_zero3(pos);
mjuu_setvec(quat, 1, 0, 0, 0);
}
void mjCFrame::Compile() {
if (compiled) {
return;
}
const char* err = alt.Set(quat, 0, model->degree, model->euler);
if (err) {
throw mjCError(this, "orientation specification error '%s' in site %d", err, id);
}
// compile parents and accumulate result
if (frame) {
frame->Compile();
mjuu_frameaccum(pos, quat, frame->pos, frame->quat);
}
mjuu_normvec(quat, 4);
compiled = true;
}
//------------------ class mjCJoint implementation -------------------------------------------------
// initialize default joint
@@ -1197,6 +1257,13 @@ int mjCJoint::Compile(void) {
}
}
// frame
if (frame) {
double mat[9];
mjuu_quat2mat(mat, frame->quat);
mjuu_mulvecmat(axis, axis, mat);
}
// FREE or BALL: set axis to (0,0,1)
if (type==mjJNT_FREE || type==mjJNT_BALL) {
axis[0] = axis[1] = 0;
@@ -1223,6 +1290,9 @@ int mjCJoint::Compile(void) {
if (type!=mjJNT_FREE) {
double qunit[4] = {1, 0, 0, 0};
double qloc[4];
if (frame) {
mjuu_frameaccum(pos, qunit, frame->pos, frame->quat);
}
body->MakeLocal(locpos, qloc, pos, qunit);
} else {
mjuu_zerovec(locpos, 3);
@@ -1842,6 +1912,11 @@ void mjCGeom::Compile(void) {
throw mjCError(this, "plugin '%s' does not support sign distance fields", plugin->name);
}
}
// frame
if (frame) {
mjuu_frameaccum(pos, quat, frame->pos, frame->quat);
}
}
@@ -1952,6 +2027,11 @@ void mjCSite::Compile(void) {
}
}
// frame
if (frame) {
mjuu_frameaccum(pos, quat, frame->pos, frame->quat);
}
// normalize quaternion
mjuu_normvec(quat, 4);
@@ -2017,6 +2097,11 @@ void mjCCamera::Compile(void) {
throw mjCError(this, "orientation specification error '%s' in camera %d", err, id);
}
// frame
if (frame) {
mjuu_frameaccum(pos, quat, frame->pos, frame->quat);
}
// normalize quaternion
mjuu_normvec(quat, 4);
@@ -2121,6 +2206,11 @@ mjCLight::mjCLight(mjCModel* _model, mjCDef* _def) {
void mjCLight::Compile(void) {
double locquat[4], quat[4]= {1, 0, 0, 0};
// frame
if (frame) {
mjuu_frameaccum(pos, quat, frame->pos, frame->quat);
}
// normalize direction, make sure it is not zero
if (mjuu_normvec(dir, 3)<mjMINVAL) {
throw mjCError(this, "zero direction in light '%s' (id = %d)", name.c_str(), id);
src/user/user_objects.h
+29
View File
@@ -30,6 +30,7 @@ class mjCError;
class mjCAlternative;
class mjCBase;
class mjCBody;
class mjCFrame;
class mjCJoint;
class mjCGeom;
class mjCSite;
@@ -176,12 +177,16 @@ class mjCBase {
// content type from resource_name; throw on failure
std::string GetAssetContentType(std::string_view resource_name, std::string_view raw_text);
// Add frame transformation
void SetFrame(mjCFrame* _frame);
std::string name; // object name
std::string classname; // defaults class name
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
mjCFrame* frame; // pointer to frame transformation
// plugin support
bool is_plugin;
@@ -215,6 +220,7 @@ class mjCBody : public mjCBase {
public:
// API for adding objects to body
mjCBody* AddBody(mjCDef* = 0);
mjCFrame* AddFrame(mjCFrame* = 0);
mjCJoint* AddJoint(mjCDef* = 0, bool isfree = false);
mjCGeom* AddGeom(mjCDef* = 0);
mjCSite* AddSite(mjCDef* = 0);
@@ -278,6 +284,7 @@ class mjCBody : public mjCBase {
// objects allocated by Add functions
std::vector<mjCBody*> bodies; // child bodies
std::vector<mjCGeom*> geoms; // geoms attached to this body
std::vector<mjCFrame*> frames; // frames attached to this body
std::vector<mjCJoint*> joints; // joints allowing motion relative to parent
std::vector<mjCSite*> sites; // sites attached to this body
std::vector<mjCCamera*> cameras; // cameras attached to this body
@@ -286,6 +293,28 @@ class mjCBody : public mjCBase {
//------------------------- class mjCFrame ---------------------------------------------------------
// Describes a coordinate transformation relative to its parent
class mjCFrame : public mjCBase {
friend class mjCBase;
friend class mjCBody;
friend class mjCModel;
public:
double pos[3]; // frame position
double quat[4]; // frame orientation
mjCAlternative alt; // alternative orientation specification
private:
bool compiled; // frame already compiled
mjCFrame(mjCModel* = 0, mjCFrame* = 0); // constructor
void Compile(void); // compiler
};
//------------------------- class mjCJoint ---------------------------------------------------------
// Describes a motion degree of freedom of a body relative to its parent
src/user/user_util.cc
+8 -3
View File
@@ -191,9 +191,14 @@ void mjuu_mulquat(double* res, const double* qa, const double* qb) {
// multiply vector by 3-by-3 matrix
void mjuu_mulvecmat(double* res, const double* vec, const double* mat) {
res[0] = mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2];
res[1] = mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2];
res[2] = mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2];
double tmp[3] = {
mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2],
mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2],
mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2]
};
res[0] = tmp[0];
res[1] = tmp[1];
res[2] = tmp[2];
}
src/xml/xml_native_reader.cc
+25 -4
View File
@@ -872,7 +872,7 @@ void mjXReader::Parse(XMLElement* root) {
for (XMLElement* section = root->FirstChildElement("worldbody"); section;
section = section->NextSiblingElement("worldbody")) {
Body(section, model->GetWorld());
Body(section, model->GetWorld(), nullptr);
}
for (XMLElement* section = root->FirstChildElement("contact"); section;
@@ -2949,7 +2949,7 @@ void mjXReader::Asset(XMLElement* section) {
// body/world section parser; recursive
void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
string text, name;
XMLElement* elem;
int n;
@@ -2960,7 +2960,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
}
// no attributes allowed in world body
if (pbody->id==0 && section->FirstAttribute()) {
if (pbody->id==0 && section->FirstAttribute() && !frame) {
throw mjXError(section, "World body cannot have attributes");
}
@@ -3000,6 +3000,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
// create joint and parse
mjCJoint* pjoint = pbody->AddJoint(def);
OneJoint(elem, pjoint);
pjoint->SetFrame(frame);
}
// freejoint sub-element
@@ -3011,6 +3012,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
// create free joint without defaults
mjCJoint* pjoint = pbody->AddJoint(NULL, true);
pjoint->SetFrame(frame);
// save defaults after creation, to make sure writing is ok
pjoint->def = def;
@@ -3025,6 +3027,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
// create geom and parse
mjCGeom* pgeom = pbody->AddGeom(def);
OneGeom(elem, pgeom);
pgeom->SetFrame(frame);
// discard visual
if (!pgeom->contype && !pgeom->conaffinity && model->discardvisual) {
@@ -3038,6 +3041,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
// create site and parse
mjCSite* psite = pbody->AddSite(def);
OneSite(elem, psite);
psite->SetFrame(frame);
}
// camera sub-element
@@ -3045,6 +3049,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
// create camera and parse
mjCCamera* pcam = pbody->AddCamera(def);
OneCamera(elem, pcam);
pcam->SetFrame(frame);
}
// light sub-element
@@ -3052,6 +3057,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
// create light and parse
mjCLight* plight = pbody->AddLight(def);
OneLight(elem, plight);
plight->SetFrame(frame);
}
// plugin sub-element
@@ -3071,6 +3077,18 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
OneFlexcomp(elem, pbody);
}
// frame sub-element
else if (name=="frame") {
mjCFrame* pframe = pbody->AddFrame(frame);
GetXMLPos(elem, pframe);
ReadAttr(elem, "pos", 3, pframe->pos, text);
ReadQuat(elem, "quat", pframe->quat, text);
ReadAlternative(elem, pframe->alt);
Body(elem, pbody, pframe);
}
// body sub-element
else if (name=="body") {
// read childdef
@@ -3102,8 +3120,11 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
// read userdata
ReadVector(elem, "user", pchild->userdata, text);
// add frame
pchild->SetFrame(frame);
// make recursive call
Body(elem, pchild);
Body(elem, pchild, nullptr);
}
// no match
src/xml/xml_native_reader.h
+2 -1
View File
@@ -42,7 +42,8 @@ class mjXReader : public mjXBase {
void Visual(tinyxml2::XMLElement* section); // visual section
void Statistic(tinyxml2::XMLElement* section); // statistic section
void Asset(tinyxml2::XMLElement* section); // asset section
void Body(tinyxml2::XMLElement* section, mjCBody* pbody); // body/world section
void Body(tinyxml2::XMLElement* section, mjCBody* pbody,
mjCFrame* pframe); // body/world section
void Contact(tinyxml2::XMLElement* section); // contact section
void Deformable(tinyxml2::XMLElement* section); // deformable section
void Equality(tinyxml2::XMLElement* section); // equality section
src/xml/xml_util.cc
+3 -1
View File
@@ -372,7 +372,9 @@ bool mjXSchema::NameMatch(XMLElement* elem, int level) {
return true;
}
return false;
if (level>=1 && !strcmp(elem->Value(), "frame")) {
return true;
}
}
// regular check