diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 9e24fa3d..7fab7efe 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1560,8 +1560,8 @@ void mjCModel::CopyTree(mjModel* m) { // set joint fields m->jnt_type[jid] = pj->type; m->jnt_group[jid] = pj->group; - m->jnt_limited[jid] = pj->limited; - m->jnt_actfrclimited[jid] = pj->actfrclimited; + m->jnt_limited[jid] = (mjtByte)pj->is_limited(); + m->jnt_actfrclimited[jid] = (mjtByte)pj->is_actfrclimited(); m->jnt_qposadr[jid] = qposadr; m->jnt_dofadr[jid] = dofadr; m->jnt_bodyid[jid] = pj->body->id; @@ -2237,7 +2237,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->tendon_num[i] = (int)pte->path.size(); m->tendon_matid[i] = pte->matid; m->tendon_group[i] = pte->group; - m->tendon_limited[i] = pte->limited; + m->tendon_limited[i] = (mjtByte)pte->is_limited(); m->tendon_width[i] = (mjtNum)pte->width; copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF); copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP); @@ -2285,9 +2285,9 @@ void mjCModel::CopyObjects(mjModel* m) { m->actuator_actadr[i] = m->actuator_actnum[i] ? adr : -1; adr += m->actuator_actnum[i]; m->actuator_group[i] = pac->group; - m->actuator_ctrllimited[i] = pac->ctrllimited; - m->actuator_forcelimited[i] = pac->forcelimited; - m->actuator_actlimited[i] = pac->actlimited; + m->actuator_ctrllimited[i] = (mjtByte)pac->is_ctrllimited(); + m->actuator_forcelimited[i] = (mjtByte)pac->is_forcelimited(); + m->actuator_actlimited[i] = (mjtByte)pac->is_actlimited(); m->actuator_actearly[i] = pac->actearly; m->actuator_cranklength[i] = (mjtNum)pac->cranklength; copyvec(m->actuator_gear + 6*i, pac->gear, 6); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index a9e4db43..2d7e2379 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -1213,6 +1213,8 @@ mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) { // clear internal variables spec_userdata_.clear(); body = 0; + limited_ = false; + actfrclimited_ = false; // reset to default if given if (_def) { @@ -1273,17 +1275,20 @@ int mjCJoint::Compile(void) { // free joints cannot be limited if (type==mjJNT_FREE) { - limited = 0; + limited_ = false; } // otherwise if limited is auto, set according to whether range is specified else if (limited==2) { bool hasrange = !(range[0]==0 && range[1]==0); checklimited(this, model->autolimits, "joint", "", limited, hasrange); - limited = hasrange ? 1 : 0; + limited_ = hasrange; + } else { + // just copy + limited_ = (limited == 1); } // resolve limits - if (limited) { + if (limited_) { // check data if (range[0]>=range[1] && type!=mjJNT_BALL) { throw mjCError(this, @@ -1307,17 +1312,19 @@ int mjCJoint::Compile(void) { // actuator force range: none for free or ball joints if (type==mjJNT_FREE || type==mjJNT_BALL) { - actfrclimited = 0; + actfrclimited_ = false; } // otherwise if actfrclimited is auto, set according to whether actfrcrange is specified else if (actfrclimited==2) { bool hasrange = !(actfrcrange[0]==0 && actfrcrange[1]==0); checklimited(this, model->autolimits, "joint", "", actfrclimited, hasrange); - actfrclimited = hasrange ? 1 : 0; + actfrclimited_ = hasrange; + } else { + actfrclimited_ = actfrclimited == 1; } // resolve actuator force range limits - if (actfrclimited) { + if (actfrclimited_) { // check data if (actfrcrange[0]>=actfrcrange[1]) { throw mjCError(this, @@ -1350,7 +1357,7 @@ int mjCJoint::Compile(void) { } // check data - if (type==mjJNT_FREE && limited) { + if (type==mjJNT_FREE && limited == 1) { throw mjCError(this, "limits should not be defined in free joint '%s' (id = %d)", name.c_str(), id); } @@ -3642,6 +3649,7 @@ mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) { spec_userdata_.clear(); path.clear(); matid = -1; + limited_ = false; // reset to default if given if (_def) { @@ -3887,11 +3895,13 @@ void mjCTendon::Compile(void) { if (limited==2) { bool hasrange = !(range[0]==0 && range[1]==0); checklimited(this, model->autolimits, "tendon", "", limited, hasrange); - limited = hasrange ? 1 : 0; + limited_ = hasrange; + } else { + limited_ = (limited == 1); } // check limits - if (range[0]>=range[1] && limited) { + if (range[0]>=range[1] && limited_) { throw mjCError(this, "invalid limits in tendon '%s (id = %d)'", name.c_str(), id); } @@ -4020,6 +4030,7 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { spec_refsite_.clear(); spec_userdata_.clear(); trnid[0] = trnid[1] = -1; + ctrllimited_ = forcelimited_ = actlimited_ = false; // reset to default if given if (_def) { @@ -4088,30 +4099,38 @@ void mjCActuator::Compile(void) { if (forcelimited==2) { bool hasrange = !(forcerange[0]==0 && forcerange[1]==0); checklimited(this, model->autolimits, "actuator", "force", forcelimited, hasrange); - forcelimited = hasrange ? 1 : 0; + forcelimited_ = hasrange; + } else { + forcelimited_ = (forcelimited == 1); } + if (ctrllimited==2) { bool hasrange = !(ctrlrange[0]==0 && ctrlrange[1]==0); checklimited(this, model->autolimits, "actuator", "ctrl", ctrllimited, hasrange); - ctrllimited = hasrange ? 1 : 0; + ctrllimited_ = hasrange; + } else { + ctrllimited_ = (ctrllimited == 1); } + if (actlimited==2) { bool hasrange = !(actrange[0]==0 && actrange[1]==0); checklimited(this, model->autolimits, "actuator", "act", actlimited, hasrange); - actlimited = hasrange ? 1 : 0; + actlimited_ = hasrange; + } else { + actlimited_ = (actlimited == 1); } // check limits - if (forcerange[0]>=forcerange[1] && forcelimited) { + if (forcerange[0]>=forcerange[1] && forcelimited_) { throw mjCError(this, "invalid force range for actuator '%s' (id = %d)", name.c_str(), id); } - if (ctrlrange[0]>=ctrlrange[1] && ctrllimited) { + if (ctrlrange[0]>=ctrlrange[1] && ctrllimited_) { throw mjCError(this, "invalid control range for actuator '%s' (id = %d)", name.c_str(), id); } - if (actrange[0]>=actrange[1] && actlimited) { + if (actrange[0]>=actrange[1] && actlimited_) { throw mjCError(this, "invalid actrange for actuator '%s' (id = %d)", name.c_str(), id); } - if (actlimited && dyntype == mjDYN_NONE) { + if (actlimited_ && dyntype == mjDYN_NONE) { throw mjCError(this, "actrange specified but dyntype is 'none' in actuator '%s' (id = %d)", name.c_str(), id); } @@ -4189,7 +4208,7 @@ void mjCActuator::Compile(void) { if (pjnt->spec.urdfeffort>0) { forcerange[0] = -pjnt->spec.urdfeffort; forcerange[1] = pjnt->spec.urdfeffort; - forcelimited = 1; + forcelimited_ = true; } break; @@ -4551,7 +4570,7 @@ void mjCSensor::Compile(void) { } // make sure joint has limit - if (!((mjCJoint*)obj)->limited) { + if (!((mjCJoint*)obj)->is_limited()) { throw mjCError(this, "joint must be limited in sensor '%s' (id = %d)", name.c_str(), id); } @@ -4577,7 +4596,7 @@ void mjCSensor::Compile(void) { } // make sure tendon has limit - if (!((mjCTendon*)obj)->spec.limited) { + if (!((mjCTendon*)obj)->is_limited()) { throw mjCError(this, "tendon must be limited in sensor '%s' (id = %d)", name.c_str(), id); } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index d37c388b..fdae10b9 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -347,6 +347,11 @@ class mjCJoint : public mjCBase, private mjmJoint { // used by mjXWriter and mjCModel const std::vector& get_userdata() { return userdata_; } + // public getters + bool is_limited() const { return limited_; } + bool is_actfrclimited() const { return actfrclimited_; } + + private: mjCJoint(mjCModel* = 0, mjCDef* = 0); @@ -354,6 +359,8 @@ class mjCJoint : public mjCBase, private mjmJoint { void PointToLocal(void); mjCBody* body; // joint's body + bool limited_; // actual (inferred) value of limited + bool actfrclimited_; // actual (inferred) value of actfrclimited // variable-size data std::vector userdata_; std::vector spec_userdata_; @@ -1081,12 +1088,16 @@ class mjCTendon : public mjCBase, private mjmTendon { void CopyFromSpec(); void PointToLocal(); + // public getters + bool is_limited() const { return limited_; } + private: mjCTendon(mjCModel* = 0, mjCDef* = 0); // constructor ~mjCTendon(); // destructor void Compile(void); // compiler int matid; // material id for rendering + bool limited_; // actual (inferred) value of limited // variable-size data std::string material_; @@ -1169,6 +1180,11 @@ class mjCActuator : public mjCBase, private mjmActuator { const std::string& get_slidersite() { return spec_slidersite_; } const std::string& get_refsite() { return spec_refsite_; } + // public getters + bool is_ctrllimited() const { return ctrllimited_; } + bool is_forcelimited() const { return forcelimited_; } + bool is_actlimited() const { return actlimited_; } + private: mjCActuator(mjCModel* = 0, mjCDef* = 0); // constructor void Compile(void); // compiler @@ -1176,6 +1192,9 @@ class mjCActuator : public mjCBase, private mjmActuator { void MakePointerLocal(); int trnid[2]; // id of transmission target + bool ctrllimited_; // actual (inferred) value of ctrllimited + bool forcelimited_; // actual (inferred) value of forcelimited + bool actlimited_; // actual (inferred) value of actlimited // variable-size data std::string target_; diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index f089ef28..63671cb9 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -288,19 +288,6 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) { } } - // special handling of limits - bool range_defined = pjoint->range[0]!=0 || pjoint->range[1]!=0; - bool limited_inferred = def->joint.limited==2 && pjoint->limited==(int)range_defined; - if (writingdefaults || !limited_inferred) { - WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited); - } - bool afrange_defined = pjoint->actfrcrange[0]!=0 || pjoint->actfrcrange[1]!=0; - bool aflimited_inferred = def->joint.actfrclimited==2 && pjoint->actfrclimited==(int)afrange_defined; - if (writingdefaults || !aflimited_inferred) { - WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, - pjoint->actfrclimited, def->joint.actfrclimited); - } - // defaults and regular if (pjoint->type != def->joint.type) { WriteAttrTxt(elem, "type", FindValue(joint_map, joint_sz, pjoint->type)); @@ -313,7 +300,10 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) { WriteAttr(elem, "solreffriction", mjNREF, pjoint->solref_friction, def->joint.solref_friction); WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->joint.solimp_friction); WriteAttr(elem, "stiffness", 1, &pjoint->stiffness, &def->joint.stiffness); + WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited); WriteAttr(elem, "range", 2, pjoint->range, def->joint.range); + WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, pjoint->actfrclimited, + def->joint.actfrclimited); WriteAttr(elem, "actuatorfrcrange", 2, pjoint->actfrcrange, def->joint.actfrcrange); WriteAttr(elem, "margin", 1, &pjoint->margin, &def->joint.margin); WriteAttr(elem, "armature", 1, &pjoint->armature, &def->joint.armature); @@ -611,19 +601,13 @@ void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) { WriteAttrTxt(elem, "class", pten->classname); } - // special handling of limits - bool range_defined = pten->range[0]!=0 || pten->range[1]!=0; - bool limited_inferred = def->tendon.limited==2 && pten->limited==(int)range_defined; - if (writingdefaults || !limited_inferred) { - WriteAttrKey(elem, "limited", TFAuto_map, 3, pten->limited, def->tendon.limited); - } - // defaults and regular WriteAttrInt(elem, "group", pten->group, def->tendon.group); WriteAttr(elem, "solreflimit", mjNREF, pten->solref_limit, def->tendon.solref_limit); WriteAttr(elem, "solimplimit", mjNIMP, pten->solimp_limit, def->tendon.solimp_limit); WriteAttr(elem, "solreffriction", mjNREF, pten->solref_friction, def->tendon.solref_friction); WriteAttr(elem, "solimpfriction", mjNIMP, pten->solimp_friction, def->tendon.solimp_friction); + WriteAttrKey(elem, "limited", TFAuto_map, 3, pten->limited, def->tendon.limited); WriteAttr(elem, "range", 2, pten->range, def->tendon.range); WriteAttr(elem, "margin", 1, &pten->margin, &def->tendon.margin); WriteAttr(elem, "stiffness", 1, &pten->stiffness, &def->tendon.stiffness); @@ -694,28 +678,13 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) { } } - // special handling of limits - bool range_defined, limited_inferred; - range_defined = pact->ctrlrange[0]!=0 || pact->ctrlrange[1]!=0; - limited_inferred = def->actuator.ctrllimited==2 && pact->ctrllimited==(int)range_defined; - if (writingdefaults || !limited_inferred) { - WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, pact->ctrllimited, def->actuator.ctrllimited); - } - range_defined = pact->forcerange[0]!=0 || pact->forcerange[1]!=0; - limited_inferred = def->actuator.forcelimited==2 && pact->forcelimited==(int)range_defined; - if (writingdefaults || !limited_inferred) { - WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, pact->forcelimited, def->actuator.forcelimited); - } - range_defined = pact->actrange[0]!=0 || pact->actrange[1]!=0; - limited_inferred = def->actuator.actlimited==2 && pact->actlimited==(int)range_defined; - if (writingdefaults || !limited_inferred) { - WriteAttrKey(elem, "actlimited", TFAuto_map, 3, pact->actlimited, def->actuator.actlimited); - } - // defaults and regular WriteAttrInt(elem, "group", pact->group, def->actuator.group); + WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, pact->ctrllimited, def->actuator.ctrllimited); WriteAttr(elem, "ctrlrange", 2, pact->ctrlrange, def->actuator.ctrlrange); + WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, pact->forcelimited, def->actuator.forcelimited); WriteAttr(elem, "forcerange", 2, pact->forcerange, def->actuator.forcerange); + WriteAttrKey(elem, "actlimited", TFAuto_map, 3, pact->actlimited, def->actuator.actlimited); WriteAttr(elem, "actrange", 2, pact->actrange, def->actuator.actrange); WriteAttr(elem, "lengthrange", 2, pact->lengthrange, def->actuator.lengthrange); WriteAttr(elem, "gear", 6, pact->gear, def->actuator.gear); diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index f2039fd4..9da99ec8 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -1266,13 +1266,16 @@ constexpr char kKeyAutoLimits[] = "user/testdata/auto_limits.xml"; // check joint limit values when automatically inferred based on range TEST_F(LimitedTest, JointLimited) { - const std::string xml_path = GetTestDataFilePath(kKeyAutoLimits); - mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); - ASSERT_THAT(model, NotNull()); + const std::string path = GetTestDataFilePath(kKeyAutoLimits); + std::array err; + mjModel* model = mj_loadXML(path.c_str(), nullptr, err.data(), err.size()); + ASSERT_THAT(model, NotNull()) << err.data(); // see `user/testdata/auto_limits.xml` for expected values for (int i=0; i < model->njnt; i++) { - EXPECT_EQ(model->jnt_limited[i], (mjtByte)model->jnt_user[i]); + EXPECT_EQ(model->jnt_limited[i], (mjtByte)model->jnt_user[i]) + << i << " " << (int)model->jnt_limited[i] << " " + << (int)model->jnt_user[i]; } mj_deleteModel(model); diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 378c1b8b..d3e000cd 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -290,7 +290,7 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredJointLimited) { mj_deleteModel(model); } -TEST_F(XMLWriterTest, DoesNotKeepExplicitJointLimitedIfAutoLimits) { +TEST_F(XMLWriterTest, KeepsExplicitJointLimited) { static constexpr char xml[] = R"( @@ -307,7 +307,7 @@ TEST_F(XMLWriterTest, DoesNotKeepExplicitJointLimitedIfAutoLimits) { std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, Not(HasSubstr("autolimits=\"true\""))); EXPECT_THAT(saved_xml, HasSubstr("range=\"-1 1\"")); - EXPECT_THAT(saved_xml, Not(HasSubstr("limited=\"true\""))); + EXPECT_THAT(saved_xml, HasSubstr("limited=\"true\"")); mj_deleteModel(model); } @@ -359,7 +359,7 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredTendonLimited) { mj_deleteModel(model); } -TEST_F(XMLWriterTest, DoesNotKeepExplicitTendonLimitedIfAutoLimits) { +TEST_F(XMLWriterTest, KeepsExplicitTendonLimitedIfAutoLimits) { static constexpr char xml[] = R"( @@ -384,7 +384,7 @@ TEST_F(XMLWriterTest, DoesNotKeepExplicitTendonLimitedIfAutoLimits) { std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, Not(HasSubstr("autolimits=\"true\""))); EXPECT_THAT(saved_xml, HasSubstr("range=\"-1 1\"")); - EXPECT_THAT(saved_xml, Not(HasSubstr("limited=\"true\""))); + EXPECT_THAT(saved_xml, HasSubstr("limited=\"true\"")); mj_deleteModel(model); } @@ -439,7 +439,7 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredActlimited) { mj_deleteModel(model); } -TEST_F(XMLWriterTest, DoesNotKeepExplicitActlimitedIfAutoLimits) { +TEST_F(XMLWriterTest, KeepsExplicitActlimitedIfAutoLimits) { static constexpr char xml[] = R"( @@ -459,7 +459,7 @@ TEST_F(XMLWriterTest, DoesNotKeepExplicitActlimitedIfAutoLimits) { std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, Not(HasSubstr("autolimits=\"true\""))); EXPECT_THAT(saved_xml, HasSubstr("actrange=\"-1 1\"")); - EXPECT_THAT(saved_xml, Not(HasSubstr("actlimited=\"true\""))); + EXPECT_THAT(saved_xml, HasSubstr("actlimited=\"true\"")); mj_deleteModel(model); } @@ -508,7 +508,7 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredCtrllimited) { mj_deleteModel(model); } -TEST_F(XMLWriterTest, DoesNotKeepExplicitCtrllimitedIfAutoLimits) { +TEST_F(XMLWriterTest, KeepsExplicitCtrllimitedIfAutoLimits) { static constexpr char xml[] = R"( @@ -527,7 +527,7 @@ TEST_F(XMLWriterTest, DoesNotKeepExplicitCtrllimitedIfAutoLimits) { ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("ctrlrange=\"-1 1\"")); - EXPECT_THAT(saved_xml, Not(HasSubstr("ctrllimited=\"true\""))); + EXPECT_THAT(saved_xml, HasSubstr("ctrllimited=\"true\"")); mj_deleteModel(model); } @@ -576,7 +576,7 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredForcelimited) { mj_deleteModel(model); } -TEST_F(XMLWriterTest, DoesNotKeepExplicitForcelimited) { +TEST_F(XMLWriterTest, KeepsExplicitForcelimited) { static constexpr char xml[] = R"( @@ -595,7 +595,7 @@ TEST_F(XMLWriterTest, DoesNotKeepExplicitForcelimited) { std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, Not(HasSubstr("autolimits=\"true\""))); EXPECT_THAT(saved_xml, HasSubstr("forcerange=\"-1 1\"")); - EXPECT_THAT(saved_xml, Not(HasSubstr("forcelimited=\"true\""))); + EXPECT_THAT(saved_xml, HasSubstr("forcelimited=\"true\"")); mj_deleteModel(model); }