diff --git a/doc/includes/references.h b/doc/includes/references.h index 4524bf23..464b4751 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -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 diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index e84f5f33..99fa9ab2 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -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; diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 8d90a7c8..31417b45 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -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( diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 8151e126..305a4c4b 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -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() diff --git a/src/user/user_model.cc b/src/user/user_model.cc index d9d82a37..2f7a4287 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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(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; } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 421e1df5..aaf72291 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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(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", diff --git a/src/user/user_objects.h b/src/user/user_objects.h index d083c404..b25e42f9 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -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 }; diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 0fb2a47c..ad591f5e 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -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);