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

Add mjmBody and mjmPlugin and their compiler C APIs.

Browse Source 
PiperOrigin-RevId: 604937231
Change-Id: Ib8a2105ce6c0500d6a610258b46f192d9e73169d
This commit is contained in:
Alessio Quaglino
2024-02-07 04:16:20 -08:00
committed by Copybara-Service
parent b9c50ad79c
commit a3e49d582e
17 changed files with 626 additions and 332 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/xml/xml_native_reader.cc
+57 -43
View File
@@ -873,7 +873,7 @@ void mjXReader::Parse(XMLElement* root) {
for (XMLElement* section = root->FirstChildElement("worldbody"); section;
section = section->NextSiblingElement("worldbody")) {
Body(section, model->GetWorld(), nullptr);
Body(section, &model->GetWorld()->spec, nullptr);
}
for (XMLElement* section = root->FirstChildElement("contact"); section;
@@ -1350,7 +1350,7 @@ void mjXReader::OneMesh(XMLElement* elem, mjCMesh* pmesh) {
XMLElement* eplugin = elem->FirstChildElement("plugin");
if (eplugin) {
OnePlugin(eplugin, pmesh);
OnePlugin(eplugin, &pmesh->plugin);
}
if (MapValue(elem, "smoothnormal", &n, bool_map, 2)) {
@@ -1542,7 +1542,7 @@ void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) {
// plugin sub-element
XMLElement* eplugin = elem->FirstChildElement("plugin");
if (eplugin) {
OnePlugin(eplugin, pgeom);
OnePlugin(eplugin, &pgeom->plugin);
}
// remaining attributes
@@ -2044,7 +2044,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
}
else if (type == "plugin") {
OnePlugin(elem, pact);
OnePlugin(elem, &pact->plugin);
int n;
if (MapValue(elem, "dyntype", &n, dyn_map, dyn_sz)) {
pact->dyntype = (mjtDyn)n;
@@ -2068,7 +2068,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
// make composite
void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) {
void mjXReader::OneComposite(XMLElement* elem, mjmBody* pbody, mjCDef* def) {
string text;
int n;
@@ -2091,7 +2091,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) {
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 = (mjCPlugin*)mjm_addPlugin(model);
comp.plugin_instance->name = "composite"+comp.prefix;
comp.plugin_instance_name = comp.plugin_instance->name;
} else {
@@ -2286,7 +2286,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) {
// make composite
char error[200];
bool res = comp.Make(pbody->model, pbody, error, 200);
bool res = comp.Make((mjCModel*)mjm_getModel(pbody), pbody, error, 200);
// throw error
if (!res) {
@@ -2297,7 +2297,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) {
// make flexcomp
void mjXReader::OneFlexcomp(XMLElement* elem, mjCBody* pbody) {
void mjXReader::OneFlexcomp(XMLElement* elem, mjmBody* pbody) {
string text;
int n;
@@ -2407,7 +2407,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjCBody* pbody) {
ReadAttrTxt(eplugin, "plugin", fcomp.plugin_name);
ReadAttrTxt(eplugin, "instance", fcomp.plugin_instance_name);
if (fcomp.plugin_instance_name.empty()) {
fcomp.plugin_instance = model->AddPlugin();
fcomp.plugin_instance = (mjCPlugin*)mjm_addPlugin(model);
fcomp.plugin_instance->name = "flexcomp_" + fcomp.name;
fcomp.plugin_instance_name = fcomp.plugin_instance->name;
} else {
@@ -2418,7 +2418,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjCBody* pbody) {
// make flexcomp
char error[200];
bool res = fcomp.Make(pbody->model, pbody, error, 200);
bool res = fcomp.Make((mjCModel*)mjm_getModel(pbody), pbody, error, 200);
// throw error
if (!res) {
@@ -2429,13 +2429,17 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjCBody* pbody) {
// add plugin
void mjXReader::OnePlugin(XMLElement* elem, mjCBase* object) {
object->is_plugin = true;
ReadAttrTxt(elem, "plugin", object->plugin_name);
ReadAttrTxt(elem, "instance", object->plugin_instance_name);
if (object->plugin_instance_name.empty()) {
object->plugin_instance = model->AddPlugin();
ReadPluginConfigs(elem, object->plugin_instance);
void mjXReader::OnePlugin(XMLElement* elem, mjmPlugin* plugin) {
plugin->active = true;
std::string name = "";
std::string instance_name = "";
ReadAttrTxt(elem, "plugin", name);
ReadAttrTxt(elem, "instance", instance_name);
mjm_setString(plugin->name, name.c_str());
mjm_setString(plugin->instance_name, instance_name.c_str());
if (instance_name.empty()) {
plugin->instance = mjm_addPlugin(model);
ReadPluginConfigs(elem, (mjCPlugin*)plugin->instance);
} else {
model->hasImplicitPluginElem = true;
}
@@ -2579,7 +2583,7 @@ void mjXReader::Extension(XMLElement* section) {
throw mjXError(
child, "explicit plugin instance must appear before implicit plugin elements");
}
mjCPlugin* pp = model->AddPlugin();
mjCPlugin* pp = (mjCPlugin*)mjm_addPlugin(model);
GetXMLPos(child, pp);
ReadAttrTxt(child, "name", pp->name, /* required = */ true);
if (pp->name.empty()) {
@@ -2983,7 +2987,7 @@ void mjXReader::Asset(XMLElement* section) {
// body/world section parser; recursive
void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjCFrame* frame) {
string text, name;
XMLElement* elem;
int n;
@@ -2994,7 +2998,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
}
// no attributes allowed in world body
if (pbody->id==0 && section->FirstAttribute() && !frame) {
if (mjm_getId(pbody->element)==0 && section->FirstAttribute() && !frame) {
throw mjXError(section, "World body cannot have attributes");
}
@@ -3007,13 +3011,13 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
// get class if specified, otherwise use body
mjCDef* def = GetClass(elem);
if (!def) {
def = pbody->def;
def = (mjCDef*)mjm_getDefault(pbody->element);
}
// inertial sub-element
if (name=="inertial") {
// no inertia allowed in world body
if (pbody->id==0) {
if (mjm_getId(pbody->element)==0) {
throw mjXError(elem, "World body cannot have inertia");
}
pbody->explicitinertial = true;
@@ -3031,12 +3035,12 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
// joint sub-element
else if (name=="joint") {
// no joints allowed in world body
if (pbody->id==0) {
if (mjm_getId(pbody->element)==0) {
throw mjXError(elem, "World body cannot have joints");
}
// create joint and parse
mjCJoint* pjoint = pbody->AddJoint(def);
mjCJoint* pjoint = (mjCJoint*)mjm_addJoint(pbody, def);
OneJoint(elem, pjoint);
pjoint->SetFrame(frame);
}
@@ -3044,12 +3048,12 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
// freejoint sub-element
else if (name=="freejoint") {
// no joints allowed in world body
if (pbody->id==0) {
if (mjm_getId(pbody->element)==0) {
throw mjXError(elem, "World body cannot have joints");
}
// create free joint without defaults
mjCJoint* pjoint = pbody->AddFreeJoint();
mjCJoint* pjoint = (mjCJoint*)mjm_addFreeJoint(pbody);
pjoint->SetFrame(frame);
// save defaults after creation, to make sure writing is ok
@@ -3063,7 +3067,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
// geom sub-element
else if (name=="geom") {
// create geom and parse
mjCGeom* pgeom = pbody->AddGeom(def);
mjCGeom* pgeom = (mjCGeom*)mjm_addGeom(pbody, def);
OneGeom(elem, pgeom);
pgeom->SetFrame(frame);
}
@@ -3073,13 +3077,13 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
// create site and parse
mjmSite* site = mjm_addSite(pbody, def);
OneSite(elem, *site);
mjm_setFrame(site, frame);
mjm_setFrame(site->element, frame);
}
// camera sub-element
else if (name=="camera") {
// create camera and parse
mjCCamera* pcam = pbody->AddCamera(def);
mjCCamera* pcam = (mjCCamera*)mjm_addCamera(pbody, def);
OneCamera(elem, pcam);
pcam->SetFrame(frame);
}
@@ -3087,14 +3091,14 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
// light sub-element
else if (name=="light") {
// create light and parse
mjCLight* plight = pbody->AddLight(def);
mjCLight* plight = (mjCLight*)mjm_addLight(pbody, def);
OneLight(elem, plight);
plight->SetFrame(frame);
}
// plugin sub-element
else if (name == "plugin") {
OnePlugin(elem, pbody);
OnePlugin(elem, &(pbody->plugin));
}
// composite sub-element
@@ -3111,7 +3115,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
// frame sub-element
else if (name=="frame") {
mjCFrame* pframe = pbody->AddFrame(frame);
mjCFrame* pframe = (mjCFrame*)mjm_addFrame(pbody, frame);
GetXMLPos(elem, pframe);
ReadAttr(elem, "pos", 3, pframe->pos, text);
@@ -3133,12 +3137,16 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
}
// create child body
mjCBody* pchild = pbody->AddBody(childdef);
GetXMLPos(elem, pchild);
mjmBody* pchild = mjm_addBody(pbody, childdef);
mjm_setString(pchild->info,
std::string("line = " + std::to_string(elem->GetLineNum())).c_str());
// read attributes
ReadAttrTxt(elem, "name", pchild->name);
ReadAttrTxt(elem, "childclass", pchild->classname);
std::string name, childclass;
ReadAttrTxt(elem, "name", name);
mjm_setString(pchild->name, name.c_str());
ReadAttrTxt(elem, "childclass", childclass);
mjm_setString(pchild->classname, childclass.c_str());
ReadAttr(elem, "pos", 3, pchild->pos, text);
ReadQuat(elem, "quat", pchild->quat, text);
if (MapValue(elem, "mocap", &n, bool_map, 2)) {
@@ -3150,10 +3158,12 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
ReadAttr(elem, "gravcomp", 1, &pchild->gravcomp, text);
// read userdata
ReadVector(elem, "user", pchild->userdata, text);
std::vector<double> userdata;
ReadVector(elem, "user", userdata, text);
mjm_setDouble(pchild->userdata, userdata.data(), userdata.size());
// add frame
pchild->SetFrame(frame);
mjm_setFrame(pchild->element, frame);
// make recursive call
Body(elem, pchild, nullptr);
@@ -3373,6 +3383,8 @@ void mjXReader::Sensor(XMLElement* section) {
// create sensor, get string type
mjCSensor* psen = model->AddSensor();
string type = elem->Value();
string plugin_name = "";
string instance_name = "";
// read name, noise, userdata
ReadAttrTxt(elem, "name", psen->name);
@@ -3630,14 +3642,16 @@ void mjXReader::Sensor(XMLElement* section) {
else if (type=="plugin") {
psen->type = mjSENS_PLUGIN;
ReadAttrTxt(elem, "plugin", psen->plugin_name);
ReadAttrTxt(elem, "instance", psen->plugin_instance_name);
if (psen->plugin_instance_name.empty()) {
psen->plugin_instance = model->AddPlugin();
ReadAttrTxt(elem, "plugin", plugin_name);
ReadAttrTxt(elem, "instance", instance_name);
mjm_setString(psen->plugin.name, plugin_name.c_str());
mjm_setString(psen->plugin.instance_name, instance_name.c_str());
if (instance_name.empty()) {
psen->plugin.instance = mjm_addPlugin(model);
} else {
model->hasImplicitPluginElem = true;
}
ReadPluginConfigs(elem, psen->plugin_instance);
ReadPluginConfigs(elem, (mjCPlugin*)psen->plugin.instance);
ReadAttrTxt(elem, "objtype", text);
psen->objtype = (mjtObj)mju_str2Type(text.c_str());
ReadAttrTxt(elem, "objname", psen->objname);
src/xml/xml_native_reader.h
+4 -4
View File
@@ -42,7 +42,7 @@ 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,
void Body(tinyxml2::XMLElement* section, mjmBody* pbody,
mjCFrame* pframe); // body/world section
void Contact(tinyxml2::XMLElement* section); // contact section
void Deformable(tinyxml2::XMLElement* section); // deformable section
@@ -66,9 +66,9 @@ class mjXReader : public mjXBase {
void OneEquality(tinyxml2::XMLElement* elem, mjCEquality* pequality);
void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon);
void OneActuator(tinyxml2::XMLElement* elem, mjCActuator* pactuator);
void OneComposite(tinyxml2::XMLElement* elem, mjCBody* pbody, mjCDef* def);
void OneFlexcomp(tinyxml2::XMLElement* elem, mjCBody* pbody);
void OnePlugin(tinyxml2::XMLElement* elem, mjCBase* object);
void OneComposite(tinyxml2::XMLElement* elem, mjmBody* pbody, mjCDef* def);
void OneFlexcomp(tinyxml2::XMLElement* elem, mjmBody* pbody);
void OnePlugin(tinyxml2::XMLElement* elem, mjmPlugin* plugin);
mjXSchema schema; // schema used for validation
mjCDef* GetClass(tinyxml2::XMLElement* section); // get default class name
src/xml/xml_native_writer.cc
+23 -16
View File
@@ -26,6 +26,7 @@
#include "engine/engine_plugin.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_api.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
@@ -417,7 +418,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
}
// write plugin
if (pgeom->is_plugin) {
if (pgeom->plugin.active) {
OnePlugin(InsertEnd(elem, "plugin"), pgeom);
}
}
@@ -725,7 +726,7 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->actuator.dynprm);
// plugins: write config attributes
if (pact->is_plugin) {
if (pact->plugin.active) {
OnePlugin(elem, pact);
}
@@ -756,13 +757,15 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
// write plugin
void mjXWriter::OnePlugin(XMLElement* elem, mjCBase* object) {
if (!object->plugin_instance_name.empty()) {
WriteAttrTxt(elem, "instance", object->plugin_instance_name);
const std::string instance_name = std::string(mjm_getString(object->plugin.instance_name));
const std::string plugin_name = std::string(mjm_getString(object->plugin.name));
if (!instance_name.empty()) {
WriteAttrTxt(elem, "instance", instance_name);
} else {
WriteAttrTxt(elem, "plugin", object->plugin_name);
WriteAttrTxt(elem, "plugin", plugin_name);
const mjpPlugin* plugin = mjp_getPluginAtSlot(
object->plugin_instance->plugin_slot);
const char* c = &object->plugin_instance->flattened_attributes[0];
((mjCPlugin*)object->plugin.instance)->plugin_slot);
const char* c = &((mjCPlugin*)object->plugin.instance)->flattened_attributes[0];
for (int i = 0; i < plugin->nattribute; ++i) {
std::string value(c);
if (!value.empty()) {
@@ -1424,7 +1427,7 @@ void mjXWriter::Asset(XMLElement* root) {
for (int i=0; i<nmesh; i++) {
// create element and write
mjCMesh* pmesh = (mjCMesh*)model->GetObject(mjOBJ_MESH, i);
if (pmesh->is_plugin) {
if (pmesh->plugin.active) {
elem = InsertEnd(section, "mesh");
WriteAttrTxt(elem, "name", pmesh->name);
OnePlugin(InsertEnd(elem, "plugin"), pmesh);
@@ -1487,7 +1490,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
WriteAttr(elem, "gravcomp", 1, &body->gravcomp);
}
// userdata
WriteVector(elem, "user", body->userdata);
WriteVector(elem, "user", body->get_userdata());
// write inertial
if (body->explicitinertial &&
@@ -1526,7 +1529,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
}
// write plugin
if (body->is_plugin) {
if (body->plugin.active) {
OnePlugin(InsertEnd(elem, "plugin"), body);
}
@@ -1709,7 +1712,7 @@ void mjXWriter::Actuator(XMLElement* root) {
for (int i=0; i<num; i++) {
mjCActuator* pact = (mjCActuator*)model->GetObject(mjOBJ_ACTUATOR, i);
XMLElement* elem;
if (pact->is_plugin) {
if (pact->plugin.active) {
elem = InsertEnd(section, "plugin");
} else {
elem = InsertEnd(section, "general");
@@ -1737,6 +1740,8 @@ void mjXWriter::Sensor(XMLElement* root) {
for (int i=0; i<num; i++) {
XMLElement* elem = 0;
mjCSensor* psen = model->sensors[i];
std::string instance_name = "";
std::string plugin_name = "";
// write sensor type and type-specific attributes
switch (psen->type) {
@@ -1923,13 +1928,15 @@ void mjXWriter::Sensor(XMLElement* root) {
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->objname);
}
if (!psen->plugin_instance_name.empty()) {
WriteAttrTxt(elem, "instance", psen->plugin_instance_name);
instance_name = std::string(mjm_getString(psen->plugin.instance_name));
plugin_name = std::string(mjm_getString(psen->plugin.name));
if (!instance_name.empty()) {
WriteAttrTxt(elem, "instance", instance_name);
} else {
WriteAttrTxt(elem, "plugin", psen->plugin_name);
WriteAttrTxt(elem, "plugin", plugin_name);
const mjpPlugin* plugin = mjp_getPluginAtSlot(
psen->plugin_instance->plugin_slot);
const char* c = &psen->plugin_instance->flattened_attributes[0];
((mjCPlugin*)psen->plugin.instance)->plugin_slot);
const char* c = &((mjCPlugin*)psen->plugin.instance)->flattened_attributes[0];
for (int i = 0; i < plugin->nattribute; ++i) {
std::string value(c);
if (!value.empty()) {
src/xml/xml_urdf.cc
+28 -22
View File
@@ -18,6 +18,7 @@
#include <vector>
#include <mujoco/mjmodel.h>
#include "user/user_api.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
@@ -201,14 +202,15 @@ void mjXURDF::Parse(
// override the pose for the base link and add a free joint
for (int i = 0; i < (int)urName.size(); i++) {
if (urParent[i] < 0) {
mjCBody* pbody = (mjCBody*)model->GetWorld()->FindObject(mjOBJ_BODY, urName[i]);
mjmBody* world = mjm_findBody(model, "world");
mjmBody* pbody = mjm_findChild(world, urName[i].c_str());
mjuu_copyvec(pbody->pos, pos, 3);
mjuu_copyvec(pbody->quat, quat, 4);
// add a free joint to allow motion of the body
// if the mass is 0, assume the object is static
if (!static_body && pbody->mass > 0) {
auto pjoint = pbody->AddJoint();
mjCJoint* pjoint = (mjCJoint*)mjm_addJoint(pbody, 0);
pjoint->name = urName[i] + "_free_joint";
pjoint->type = mjJNT_FREE;
}
@@ -220,13 +222,14 @@ void mjXURDF::Parse(
void mjXURDF::Body(XMLElement* body_elem) {
std::string name, text;
XMLElement *elem, *temp, *temp1;
mjCBody* pbody;
mjmBody *pbody, *world;
mjCGeom* pgeom;
// get body name and pointer to mjCBody
// get body name and pointer to mjmBody
ReadAttrTxt(body_elem, "name", name, true);
name = GetPrefixedName(name);
pbody = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, name);
world = mjm_findBody(model, "world");
pbody = mjm_findChild(world, name.c_str());
if (!pbody) {
throw mjXError(body_elem, "URDF body not found"); // SHOULD NOT OCCUR
}
@@ -264,7 +267,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
// process inertia
// lquat = rotation from specified to default (joint/body) inertial frame
double lquat[4], tmpquat[4];
const char* altres = pbody->FullInertia(lquat, pbody->inertia);
const char* altres = mjm_setFullInertia(pbody, lquat, pbody->inertia);
// inertia are sometimes 0 in URDF files: ignore error in altres, fix later
(void) altres;
@@ -366,7 +369,7 @@ void mjXURDF::Parse(XMLElement* root) {
void mjXURDF::Joint(XMLElement* joint_elem) {
std::string jntname, name, text;
XMLElement *elem;
mjCBody *pbody, *parent;
mjmBody *pbody, *parent, *world;
mjCJoint *pjoint=0, *pjoint1=0, *pjoint2=0;
int jointtype;
@@ -382,7 +385,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
elem = FindSubElem(joint_elem, "parent", true);
ReadAttrTxt(elem, "link", name, true);
name = GetPrefixedName(name);
parent = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, name);
world = mjm_findBody(model, "world");
parent = mjm_findChild(world, name.c_str());
if (!parent) { // SHOULD NOT OCCUR
throw mjXError(elem, "invalid parent name in URDF joint definition");
}
@@ -391,7 +395,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
elem = FindSubElem(joint_elem, "child", true);
ReadAttrTxt(elem, "link", name, true);
name = GetPrefixedName(name);
pbody = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, name);
world = mjm_findBody(model, "world");
pbody = mjm_findChild(world, name.c_str());
if (!pbody) { // SHOULD NOT OCCUR
throw mjXError(elem, "invalid child name in URDF joint definition");
}
@@ -408,7 +413,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
switch (jointtype) {
case 0: // revolute
case 1: // continuous
pjoint = pbody->AddJoint();
pjoint = (mjCJoint*)mjm_addJoint(pbody, 0);
pjoint->name = jntname;
pjoint->type = mjJNT_HINGE;
mjuu_setvec(pjoint->pos, 0, 0, 0);
@@ -416,7 +421,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
break;
case 2: // prismatic
pjoint = pbody->AddJoint();
pjoint = (mjCJoint*)mjm_addJoint(pbody, 0);
pjoint->name = jntname;
pjoint->type = mjJNT_SLIDE;
mjuu_setvec(pjoint->pos, 0, 0, 0);
@@ -427,7 +432,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
return;
case 4: // floating
pjoint = pbody->AddJoint();
pjoint = (mjCJoint*)mjm_addJoint(pbody, 0);
pjoint->name = jntname;
pjoint->type = mjJNT_FREE;
break;
@@ -438,7 +443,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
mjuu_quat2mat(mat, quat);
// construct slider along x
pjoint = pbody->AddJoint();
pjoint = (mjCJoint*)mjm_addJoint(pbody, 0);
pjoint->name = jntname + "_TX";
pjoint->type = mjJNT_SLIDE;
tmpaxis[0] = mat[0];
@@ -448,7 +453,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
mjuu_copyvec(pjoint->axis, tmpaxis, 3);
// construct slider along y
pjoint1 = pbody->AddJoint();
pjoint1 = (mjCJoint*)mjm_addJoint(pbody, 0);
pjoint1->name = jntname + "_TY";
pjoint1->type = mjJNT_SLIDE;
tmpaxis[0] = mat[1];
@@ -458,7 +463,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
mjuu_copyvec(pjoint1->axis, tmpaxis, 3);
// construct hinge around z = locaxis
pjoint2 = pbody->AddJoint();
pjoint2 = (mjCJoint*)mjm_addJoint(pbody, 0);
pjoint2->name = jntname + "_RZ";
pjoint2->type = mjJNT_HINGE;
mjuu_setvec(pjoint2->pos, 0, 0, 0);
@@ -495,7 +500,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
// parse origin and geometry elements of visual or collision
mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjCBody* pbody, bool collision) {
mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) {
XMLElement *elem, *temp;
std::string text, meshfile;
@@ -503,7 +508,7 @@ mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjCBody* pbody, bool collision) {
elem = FindSubElem(geom_elem, "geometry", true);
// add BOX geom, modify type later
mjCGeom* pgeom = pbody->AddGeom();
mjCGeom* pgeom = (mjCGeom*)mjm_addGeom(pbody, 0);
pgeom->name = "";
pgeom->type = mjGEOM_BOX;
if (collision) {
@@ -664,21 +669,22 @@ void mjXURDF::AddBody(std::string name) {
// add body with given number to the mjCModel tree, process children
void mjXURDF::AddToTree(int n) {
// get pointer to parent in mjCModel tree
mjCBody *parent = 0, *child = 0;
mjmBody *parent = 0, *child = 0, *world = 0;
if (urParent[n]>=0) {
parent = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, urName[urParent[n]]);
world = mjm_findBody(model, "world");
parent = mjm_findChild(world, urName[urParent[n]].c_str());
if (!parent)
throw mjXError(0, "URDF body parent should already be in tree: %s",
urName[urParent[n]].c_str()); // SHOULD NOT OCCUR
} else {
parent = model->GetWorld();
parent = &model->GetWorld()->spec;
}
// add this body
if (urName[n] != "world") {
child = parent->AddBody();
child->name = urName[n];
child = mjm_addBody(parent, 0);
mjm_setString(child->name, urName[n].c_str());
}
// add children recursively
src/xml/xml_urdf.h
+2 -1
View File
@@ -19,6 +19,7 @@
#include <unordered_set>
#include <vector>
#include "user/user_api.h"
#include "user/user_model.h"
#include "xml/xml_base.h"
#include "tinyxml2.h"
@@ -52,7 +53,7 @@ class mjXURDF : public mjXBase {
void Body(tinyxml2::XMLElement* body_elem); // parse body
void Joint(tinyxml2::XMLElement* joint_elem); // parse joint
mjCGeom* Geom(tinyxml2::XMLElement* geom_elem,
mjCBody* pbody, bool collision); // parse origin and geometry of geom
mjmBody* pbody, bool collision); // parse origin and geometry of geom
void Origin(tinyxml2::XMLElement* origin_elem, double* pos, double* quat); // parse origin element
void MakeMaterials(tinyxml2::XMLElement* elem); // find all materials recursively