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

Move mjCWrap type to mjsWrap.

Browse Source 
PiperOrigin-RevId: 821851540
Change-Id: I53382a8a21312a96aea46eec130f43e7cad70f92
This commit is contained in:
Sam Haves
2025-10-20 16:27:59 -07:00
committed by Copybara-Service
parent 88ac02dad3
commit 59f14485e0
8 changed files with 111 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/includes/references.h
+1
View File
@@ -2302,6 +2302,7 @@ typedef struct mjsTendon_ { // tendon specification
} mjsTendon;
typedef struct mjsWrap_ { // wrapping object specification
mjsElement* element; // element type
mjtWrap type; // wrap type
mjString* info; // message appended to errors
} mjsWrap;
typedef struct mjsActuator_ { // actuator specification
include/mujoco/mjspec.h
+1
View File
@@ -652,6 +652,7 @@ typedef struct mjsTendon_ { // tendon specification
typedef struct mjsWrap_ { // wrapping object specification
mjsElement* element; // element type
mjtWrap type; // wrap type
mjString* info; // message appended to errors
} mjsWrap;
python/mujoco/introspect/structs.py
+5
View File
@@ -10142,6 +10142,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='element type',
),
StructFieldDecl(
name='type',
type=ValueType(name='mjtWrap'),
doc='wrap type',
),
StructFieldDecl(
name='info',
type=PointerType(
python/mujoco/specs_test.py
+64
View File
@@ -1491,5 +1491,69 @@ class SpecsTest(absltest.TestCase):
self.assertEqual(model.geom_matid[0], 1)
def test_tendon_path(self):
spec = mujoco.MjSpec()
body = spec.worldbody.add_body(name='body')
body.add_geom(name='body_geom', pos=[0, 0, 0], size=[.1, 0, 0])
body.add_site(name='site1', pos=[0, 0, 0])
body.add_site(name='site2', pos=[0, 0, -1])
body.add_site(name='site3', pos=[0, 0, -4])
body.add_site(name='site4', pos=[0, 1, -6])
spec.worldbody.add_geom(name='sphere', size=[.2, 0, 0], pos=[0, 0, -2])
spec.worldbody.add_geom(
name='cylinder',
type=mujoco.mjtGeom.mjGEOM_CYLINDER,
size=[0.1, 0.2, 0.3],
pos=[0, 0, -5]
)
body.add_joint(
name='joint1', type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]
)
body2 = spec.worldbody.add_body(name='body2', pos=[2, 0, 0])
body2.add_geom(name='body2_geom', pos=[0, 0, 0], size=[.1, 0, 0])
body2.add_joint(
name='joint2', type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]
)
spatial_tendon = spec.add_tendon()
fixed_tendon = spec.add_tendon()
wrap_site1 = spatial_tendon.wrap_site('site1')
wrap_site2 = spatial_tendon.wrap_site('site2')
wrap_pulley1 = spatial_tendon.wrap_pulley(2.0)
wrap_site3_1 = spatial_tendon.wrap_site('site3')
wrap_sphere = spatial_tendon.wrap_geom('sphere', '')
wrap_site4_1 = spatial_tendon.wrap_site('site4')
wrap_pulley2 = spatial_tendon.wrap_pulley(2.0)
wrap_site3_2 = spatial_tendon.wrap_site('site3')
wrap_cylinder = spatial_tendon.wrap_geom('cylinder', '')
wrap_site4_2 = spatial_tendon.wrap_site('site4')
wrap_joint1 = fixed_tendon.wrap_joint('joint1', 1.0)
wrap_joint2 = fixed_tendon.wrap_joint('joint2', 1.0)
# Wrap type for geom is only set during compilation.
spec.compile()
self.assertEqual(wrap_site1.type, mujoco.mjtWrap.mjWRAP_SITE)
self.assertEqual(wrap_site2.type, mujoco.mjtWrap.mjWRAP_SITE)
self.assertEqual(wrap_site3_1.type, mujoco.mjtWrap.mjWRAP_SITE)
self.assertEqual(wrap_site4_1.type, mujoco.mjtWrap.mjWRAP_SITE)
self.assertEqual(wrap_site3_2.type, mujoco.mjtWrap.mjWRAP_SITE)
self.assertEqual(wrap_site4_2.type, mujoco.mjtWrap.mjWRAP_SITE)
self.assertEqual(wrap_pulley1.type, mujoco.mjtWrap.mjWRAP_PULLEY)
self.assertEqual(wrap_sphere.type, mujoco.mjtWrap.mjWRAP_SPHERE)
self.assertEqual(wrap_cylinder.type, mujoco.mjtWrap.mjWRAP_CYLINDER)
self.assertEqual(wrap_pulley2.type, mujoco.mjtWrap.mjWRAP_PULLEY)
self.assertEqual(wrap_joint1.type, mujoco.mjtWrap.mjWRAP_JOINT)
self.assertEqual(wrap_joint2.type, mujoco.mjtWrap.mjWRAP_JOINT)
if __name__ == '__main__':
absltest.main()
src/user/user_model.cc
+3 -3
View File
@@ -120,7 +120,7 @@ bool IsNullPose(const T pos[3], const T quat[4]) {
// get body id from wrap object
int GetBodyIdFromWrap(const mjCWrap* wrap) {
if (!wrap || !wrap->obj) return -1;
switch (wrap->type) {
switch (wrap->Type()) {
case mjWRAP_SITE:
return static_cast<mjCSite*>(wrap->obj)->Body()->id;
case mjWRAP_CYLINDER:
@@ -3606,10 +3606,10 @@ void mjCModel::CopyObjects(mjModel* m) {
// set wraps
for (int j=0; j < (int)pte->path.size(); j++) {
m->wrap_type[adr+j] = pte->path[j]->type;
m->wrap_type[adr+j] = pte->path[j]->Type();
m->wrap_objid[adr+j] = pte->path[j]->obj ? pte->path[j]->obj->id : -1;
m->wrap_prm[adr+j] = (mjtNum)pte->path[j]->prm;
if (pte->path[j]->type == mjWRAP_SPHERE || pte->path[j]->type == mjWRAP_CYLINDER) {
if (pte->path[j]->Type() == mjWRAP_SPHERE || pte->path[j]->Type() == mjWRAP_CYLINDER) {
m->wrap_prm[adr+j] = (mjtNum)pte->path[j]->sideid;
}
}
src/user/user_objects.cc
+30 -20
View File
@@ -6138,8 +6138,8 @@ void mjCTendon::CopyFromSpec() {
// clear precompiled
for (int i=0; i < path.size(); i++) {
if (path[i]->type == mjWRAP_CYLINDER) {
path[i]->type = mjWRAP_SPHERE;
if (path[i]->Type() == mjWRAP_CYLINDER) {
path[i]->spec.type = mjWRAP_SPHERE;
}
}
}
@@ -6175,7 +6175,7 @@ void mjCTendon::WrapSite(std::string wrapname, std::string_view wrapinfo) {
wrap->info = wrapinfo;
// set parameters, add to path
wrap->type = mjWRAP_SITE;
wrap->spec.type = mjWRAP_SITE;
wrap->name = wrapname;
wrap->id = (int)path.size();
path.push_back(wrap);
@@ -6190,7 +6190,7 @@ void mjCTendon::WrapGeom(std::string wrapname, std::string sidesite, std::string
wrap->info = wrapinfo;
// set parameters, add to path
wrap->type = mjWRAP_SPHERE; // replace with cylinder later if needed
wrap->spec.type = mjWRAP_SPHERE; // replace with cylinder later if needed
wrap->name = wrapname;
wrap->sidesite = sidesite;
wrap->id = (int)path.size();
@@ -6206,7 +6206,7 @@ void mjCTendon::WrapJoint(std::string wrapname, double coef, std::string_view wr
wrap->info = wrapinfo;
// set parameters, add to path
wrap->type = mjWRAP_JOINT;
wrap->spec.type = mjWRAP_JOINT;
wrap->name = wrapname;
wrap->prm = coef;
wrap->id = (int)path.size();
@@ -6222,7 +6222,7 @@ void mjCTendon::WrapPulley(double divisor, std::string_view wrapinfo) {
wrap->info = wrapinfo;
// set parameters, add to path
wrap->type = mjWRAP_PULLEY;
wrap->spec.type = mjWRAP_PULLEY;
wrap->prm = divisor;
wrap->id = (int)path.size();
path.push_back(wrap);
@@ -6253,7 +6253,7 @@ void mjCTendon::ResolveReferences(const mjCModel* m) {
for (int i=0; i < path.size(); i++) {
std::string pname = path[i]->name;
std::string psidesite = path[i]->sidesite;
if (path[i]->type == mjWRAP_PULLEY) {
if (path[i]->Type() == mjWRAP_PULLEY) {
npulley++;
}
try {
@@ -6280,6 +6280,11 @@ void mjCTendon::ResolveReferences(const mjCModel* m) {
// compiler
void mjCTendon::Compile(void) {
// compile all wraps in the path
for (mjCWrap* wrap : path) {
wrap->Compile();
}
CopyFromSpec();
// resize userdata
@@ -6297,7 +6302,7 @@ void mjCTendon::Compile(void) {
}
// determine type
bool spatial = (path[0]->type != mjWRAP_JOINT);
bool spatial = (path[0]->Type() != mjWRAP_JOINT);
// require at least two objects in spatial path
if (spatial && sz < 2) {
@@ -6318,7 +6323,7 @@ void mjCTendon::Compile(void) {
// fixed
if (!spatial) {
// make sure all objects are joints
if (path[i]->type != mjWRAP_JOINT) {
if (path[i]->Type() != mjWRAP_JOINT) {
throw mjCError(this, "tendon '%s' (id = %d): spatial object found in fixed path at pos %d",
name.c_str(), id, i);
}
@@ -6332,10 +6337,10 @@ void mjCTendon::Compile(void) {
name.c_str(), id);
}
switch (path[i]->type) {
switch (path[i]->Type()) {
case mjWRAP_PULLEY:
// pulley should not follow other pulley
if (i > 0 && path[i-1]->type == mjWRAP_PULLEY) {
if (i > 0 && path[i-1]->Type() == mjWRAP_PULLEY) {
throw mjCError(this, "tendon '%s' (id = %d): consecutive pulleys (pos %d)",
name.c_str(), id, i);
}
@@ -6348,15 +6353,15 @@ void mjCTendon::Compile(void) {
case mjWRAP_SITE:
// site needs a neighbor that is not a pulley
if ((i == 0 || path[i-1]->type == mjWRAP_PULLEY) &&
(i == sz-1 || path[i+1]->type == mjWRAP_PULLEY)) {
if ((i == 0 || path[i-1]->Type() == mjWRAP_PULLEY) &&
(i == sz-1 || path[i+1]->Type() == mjWRAP_PULLEY)) {
throw mjCError(this,
"tendon '%s' (id = %d): site %d needs a neighbor that is not a pulley",
name.c_str(), id, i);
}
// site cannot be repeated
if (i < sz-1 && path[i+1]->type == mjWRAP_SITE && path[i]->obj->id == path[i+1]->obj->id) {
if (i < sz-1 && path[i+1]->Type() == mjWRAP_SITE && path[i]->obj->id == path[i+1]->obj->id) {
throw mjCError(this,
"tendon '%s' (id = %d): site %d is repeated",
name.c_str(), id, i);
@@ -6367,7 +6372,7 @@ void mjCTendon::Compile(void) {
case mjWRAP_SPHERE:
case mjWRAP_CYLINDER:
// geom must be bracketed by sites
if (i == 0 || i == sz-1 || path[i-1]->type != mjWRAP_SITE || path[i+1]->type != mjWRAP_SITE) {
if (i == 0 || i == sz-1 || path[i-1]->Type() != mjWRAP_SITE || path[i+1]->Type() != mjWRAP_SITE) {
throw mjCError(this,
"tendon '%s' (id = %d): geom at pos %d not bracketed by sites",
name.c_str(), id, i);
@@ -6442,7 +6447,7 @@ mjCWrap::mjCWrap(mjCModel* _model, mjCTendon* _tendon) {
tendon = _tendon;
// clear variables
type = mjWRAP_NONE;
spec.type = mjWRAP_NONE;
obj = nullptr;
sideid = -1;
prm = 0;
@@ -6450,6 +6455,7 @@ mjCWrap::mjCWrap(mjCModel* _model, mjCTendon* _tendon) {
// point to local
PointToLocal();
CopyFromSpec();
}
@@ -6478,7 +6484,9 @@ void mjCWrap::PointToLocal() {
spec.info = &info;
}
void mjCWrap::CopyFromSpec() {
*static_cast<mjsWrap*>(this) = spec;
}
void mjCWrap::NameSpace(const mjCModel* m) {
name = m->prefix + name + m->suffix;
@@ -6487,13 +6495,15 @@ void mjCWrap::NameSpace(const mjCModel* m) {
}
}
void mjCWrap::Compile(void) {
CopyFromSpec();
}
void mjCWrap::ResolveReferences(const mjCModel* m) {
mjCBase *pside;
// handle wrap object types
switch (type) {
switch (spec.type) {
case mjWRAP_JOINT: // joint
// find joint by name
obj = m->FindObject(mjOBJ_JOINT, name);
@@ -6516,7 +6526,7 @@ void mjCWrap::ResolveReferences(const mjCModel* m) {
// set/check geom type
if (((mjCGeom*)obj)->type == mjGEOM_CYLINDER) {
type = mjWRAP_CYLINDER;
spec.type = mjWRAP_CYLINDER;
} else if (((mjCGeom*)obj)->type != mjGEOM_SPHERE) {
throw mjCError(this,
"geom '%s' in tendon %d, wrap %d is not sphere or cylinder",
src/user/user_objects.h
+4 -1
View File
@@ -1705,7 +1705,6 @@ class mjCTendon : public mjCTendon_, private mjsTendon {
class mjCWrap_ : public mjCBase {
public:
mjtWrap type; // wrap object type
int sideid; // side site id; -1 if not applicable
double prm; // parameter: divisor, coefficient
std::string sidesite; // name of side site
@@ -1719,9 +1718,11 @@ class mjCWrap : public mjCWrap_, private mjsWrap {
mjsWrap spec;
using mjCBase::info;
void CopyFromSpec();
void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
mjtWrap Type() const { return spec.type; }
mjCBase* obj; // wrap object pointer
@@ -1730,6 +1731,8 @@ class mjCWrap : public mjCWrap_, private mjsWrap {
mjCWrap(const mjCWrap& other); // copy constructor
mjCWrap& operator=(const mjCWrap& other); // copy assignment
void Compile(void); // compiler
mjCTendon* tendon; // tendon owning this wrap
};
src/xml/xml_native_writer.cc
+3 -3
View File
@@ -715,7 +715,7 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe
// write tendon
void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* tendon, mjCDef* def) {
bool fixed = (tendon->GetWrap(0) && tendon->GetWrap(0)->type == mjWRAP_JOINT);
bool fixed = (tendon->GetWrap(0) && tendon->GetWrap(0)->Type() == mjWRAP_JOINT);
// regular
if (!writingdefaults) {
@@ -1886,14 +1886,14 @@ void mjXWriter::Tendon(XMLElement* root) {
continue;
}
XMLElement* elem = InsertEnd(section,
tendon->GetWrap(0)->type == mjWRAP_JOINT ? "fixed" : "spatial");
tendon->GetWrap(0)->Type() == mjWRAP_JOINT ? "fixed" : "spatial");
OneTendon(elem, tendon, model->def_map[tendon->classname]);
// write wraps
XMLElement* wrapelem;
for (int j=0; j < tendon->NumWraps(); j++) {
const mjCWrap* wrap = tendon->GetWrap(j);
switch (wrap->type) {
switch (wrap->Type()) {
case mjWRAP_JOINT:
wrapelem = InsertEnd(elem, "joint");
WriteAttrTxt(wrapelem, "joint", wrap->obj->name);