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_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