diff --git a/src/user/user_api.h b/src/user/user_api.h index 29a55fa6..5042a916 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -60,6 +60,13 @@ typedef enum _mjtMark { // mark type for procedural textures } mjtMark; +typedef enum _mjtLimited { // type of limit specification + mjLIMITED_FALSE = 0, // not limited + mjLIMITED_TRUE, // limited + mjLIMITED_AUTO, // limited inferred from presence of range +} mjtLimited; + + //---------------------------------- attribute structs (mjm) --------------------------------------- typedef struct _mjmOrientation { // alternative orientation specifiers @@ -132,12 +139,12 @@ typedef struct _mjmJoint { // joint specification double springdamper[2]; // timeconst, dampratio // limits - int limited; // does joint have limits + int limited; // does joint have limits (mjtLimited) double range[2]; // joint limits double margin; // margin value for joint limit detection mjtNum solref_limit[mjNREF]; // solver reference: joint limits mjtNum solimp_limit[mjNIMP]; // solver impedance: joint limits - int actfrclimited; // are actuator forces on joint limited + int actfrclimited; // are actuator forces on joint limited (mjtLimited) double actfrcrange[2]; // actuator force limits // dof properties @@ -466,7 +473,7 @@ typedef struct _mjmTendon { // tendon specification mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction // length range - int limited; // does tendon have limits: 0 false, 1 true, 2 auto + int limited; // does tendon have limits (mjtLimited) double range[2]; // length limits double margin; // margin value for tendon limit detection mjtNum solref_limit[mjNREF]; // solver reference: tendon limits @@ -520,11 +527,11 @@ typedef struct _mjmActuator { // actuator specification double lengthrange[2]; // transmission length range // input/output clamping - int ctrllimited; // are control limits defined: 0 false, 1 true, 2 auto + int ctrllimited; // are control limits defined (mjtLimited) double ctrlrange[2]; // control range - int forcelimited; // are force limits defined: 0 false, 1 true, 2 auto + int forcelimited; // are force limits defined (mjtLimited) double forcerange[2]; // force range - int actlimited; // are activation limits defined: 0 false, 1 true, 2 auto + int actlimited; // are activation limits defined (mjtLimited) double actrange[2]; // activation range // other diff --git a/src/user/user_init.cc b/src/user/user_init.cc index 71cd94c3..ff284e3b 100644 --- a/src/user/user_init.cc +++ b/src/user/user_init.cc @@ -55,8 +55,8 @@ void mjm_defaultJoint(mjmJoint& joint) { joint.type = mjJNT_HINGE; joint.axis[2] = 1; - joint.limited = 2; - joint.actfrclimited = 2; + joint.limited = mjLIMITED_AUTO; + joint.actfrclimited = mjLIMITED_AUTO; mj_defaultSolRefImp(joint.solref_limit, joint.solimp_limit); mj_defaultSolRefImp(joint.solref_friction, joint.solimp_friction); joint.urdfeffort = -1; @@ -267,7 +267,7 @@ void mjm_defaultEquality(mjmEquality& equality) { // default tendon attributes void mjm_defaultTendon(mjmTendon& tendon) { memset(&tendon, 0, sizeof(mjmTendon)); - tendon.limited = 2; + tendon.limited = mjLIMITED_AUTO; tendon.springlength[0] = tendon.springlength[1] = -1; mj_defaultSolRefImp(tendon.solref_limit, tendon.solimp_limit); mj_defaultSolRefImp(tendon.solref_friction, tendon.solimp_friction); @@ -297,9 +297,9 @@ void mjm_defaultActuator(mjmActuator& actuator) { actuator.gear[0] = 1; // input/output clamping - actuator.ctrllimited = 2; - actuator.forcelimited = 2; - actuator.actlimited = 2; + actuator.ctrllimited = mjLIMITED_AUTO; + actuator.forcelimited = mjLIMITED_AUTO; + actuator.actlimited = mjLIMITED_AUTO; } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index a7b30490..e25b0abf 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -86,6 +86,14 @@ static void checklimited( } } +// returns true if limits should be active +static bool islimited(int limited, const double range[2]) { + if (limited == mjLIMITED_TRUE || (limited == mjLIMITED_AUTO && range[0] < range[1])) { + return true; + } + return false; +} + //------------------------- class mjCError implementation ------------------------------------------ @@ -1237,8 +1245,6 @@ 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) { @@ -1259,6 +1265,11 @@ mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) { +bool mjCJoint::is_limited() const { return islimited(limited, range); } +bool mjCJoint::is_actfrclimited() const { return islimited(actfrclimited, actfrcrange); } + + + void mjCJoint::PointToLocal() { spec.element = (mjElement)this; spec.name = (mjString)&name; @@ -1299,20 +1310,16 @@ int mjCJoint::Compile(void) { // free joints cannot be limited if (type==mjJNT_FREE) { - limited_ = false; + limited = mjLIMITED_FALSE; } - // otherwise if limited is auto, set according to whether range is specified - else if (limited==2) { + // otherwise if limited is auto, check consistency wrt auto-limits + else if (limited == mjLIMITED_AUTO) { bool hasrange = !(range[0]==0 && range[1]==0); checklimited(this, model->autolimits, "joint", "", limited, hasrange); - limited_ = hasrange; - } else { - // just copy - limited_ = (limited == 1); } // resolve limits - if (limited_) { + if (is_limited()) { // check data if (range[0]>=range[1] && type!=mjJNT_BALL) { throw mjCError(this, @@ -1336,19 +1343,16 @@ int mjCJoint::Compile(void) { // actuator force range: none for free or ball joints if (type==mjJNT_FREE || type==mjJNT_BALL) { - actfrclimited_ = false; + actfrclimited = mjLIMITED_FALSE; } - // otherwise if actfrclimited is auto, set according to whether actfrcrange is specified - else if (actfrclimited==2) { + // otherwise if actfrclimited is auto, check consistency wrt auto-limits + else if (actfrclimited == mjLIMITED_AUTO) { bool hasrange = !(actfrcrange[0]==0 && actfrcrange[1]==0); checklimited(this, model->autolimits, "joint", "", actfrclimited, hasrange); - actfrclimited_ = hasrange; - } else { - actfrclimited_ = actfrclimited == 1; } // resolve actuator force range limits - if (actfrclimited_) { + if (is_actfrclimited()) { // check data if (actfrcrange[0]>=actfrcrange[1]) { throw mjCError(this, @@ -1381,7 +1385,7 @@ int mjCJoint::Compile(void) { } // check data - if (type==mjJNT_FREE && limited == 1) { + if (type==mjJNT_FREE && limited == mjLIMITED_TRUE) { throw mjCError(this, "limits should not be defined in free joint '%s' (id = %d)", name.c_str(), id); } @@ -3789,7 +3793,6 @@ mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) { spec_userdata_.clear(); path.clear(); matid = -1; - limited_ = false; // reset to default if given if (_def) { @@ -3808,6 +3811,8 @@ mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) { } +bool mjCTendon::is_limited() const { return islimited(limited, range); } + void mjCTendon::PointToLocal() { spec.element = (mjElement)this; @@ -4032,16 +4037,13 @@ void mjCTendon::Compile(void) { } // if limited is auto, set to 1 if range is specified, otherwise unlimited - if (limited==2) { + if (limited == mjLIMITED_AUTO) { bool hasrange = !(range[0]==0 && range[1]==0); checklimited(this, model->autolimits, "tendon", "", limited, hasrange); - limited_ = hasrange; - } else { - limited_ = (limited == 1); } // check limits - if (range[0]>=range[1] && limited_) { + if (range[0]>=range[1] && is_limited()) { throw mjCError(this, "invalid limits in tendon '%s (id = %d)'", name.c_str(), id); } @@ -4170,7 +4172,6 @@ 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) { @@ -4190,6 +4191,12 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { +bool mjCActuator::is_ctrllimited() const { return islimited(ctrllimited, ctrlrange); } +bool mjCActuator::is_forcelimited() const { return islimited(forcelimited, forcerange); } +bool mjCActuator::is_actlimited() const { return islimited(actlimited, actrange); } + + + void mjCActuator::MakePointerLocal() { spec.element = (mjElement)this; spec.name = (mjString)&name; @@ -4235,42 +4242,31 @@ void mjCActuator::Compile(void) { } userdata_.resize(model->nuser_actuator); - // if limited is auto, set to 1 if range is specified, otherwise unlimited - if (forcelimited==2) { + // if limited is auto, check for inconsistency wrt to autolimits + if (forcelimited == mjLIMITED_AUTO) { bool hasrange = !(forcerange[0]==0 && forcerange[1]==0); checklimited(this, model->autolimits, "actuator", "force", forcelimited, hasrange); - forcelimited_ = hasrange; - } else { - forcelimited_ = (forcelimited == 1); } - - if (ctrllimited==2) { + if (ctrllimited == mjLIMITED_AUTO) { bool hasrange = !(ctrlrange[0]==0 && ctrlrange[1]==0); checklimited(this, model->autolimits, "actuator", "ctrl", ctrllimited, hasrange); - ctrllimited_ = hasrange; - } else { - ctrllimited_ = (ctrllimited == 1); } - - if (actlimited==2) { + if (actlimited == mjLIMITED_AUTO) { bool hasrange = !(actrange[0]==0 && actrange[1]==0); checklimited(this, model->autolimits, "actuator", "act", actlimited, hasrange); - actlimited_ = hasrange; - } else { - actlimited_ = (actlimited == 1); } // check limits - if (forcerange[0]>=forcerange[1] && forcelimited_) { + if (forcerange[0]>=forcerange[1] && is_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] && is_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] && is_actlimited()) { throw mjCError(this, "invalid actrange for actuator '%s' (id = %d)", name.c_str(), id); } - if (actlimited_ && dyntype == mjDYN_NONE) { + if (is_actlimited() && dyntype == mjDYN_NONE) { throw mjCError(this, "actrange specified but dyntype is 'none' in actuator '%s' (id = %d)", name.c_str(), id); } @@ -4348,7 +4344,7 @@ void mjCActuator::Compile(void) { if (pjnt->spec.urdfeffort>0) { forcerange[0] = -pjnt->spec.urdfeffort; forcerange[1] = pjnt->spec.urdfeffort; - forcelimited_ = true; + forcelimited = mjLIMITED_TRUE; } break; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 5cb9a3a9..545d3218 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -337,9 +337,8 @@ 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_; } + bool is_limited() const; + bool is_actfrclimited() const; private: @@ -349,8 +348,7 @@ 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_; @@ -1087,8 +1085,7 @@ class mjCTendon : public mjCBase, private mjmTendon { void CopyFromSpec(); void PointToLocal(); - // public getters - bool is_limited() const { return limited_; } + bool is_limited() const; private: mjCTendon(mjCModel* = 0, mjCDef* = 0); // constructor @@ -1096,7 +1093,6 @@ class mjCTendon : public mjCBase, private mjmTendon { void Compile(void); // compiler int matid; // material id for rendering - bool limited_; // actual (inferred) value of limited // variable-size data std::string material_; @@ -1179,10 +1175,9 @@ 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_; } + bool is_ctrllimited() const; + bool is_forcelimited() const; + bool is_actlimited() const; private: mjCActuator(mjCModel* = 0, mjCDef* = 0); // constructor @@ -1191,9 +1186,6 @@ 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_urdf.cc b/src/xml/xml_urdf.cc index 168d705b..9a180c11 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -487,14 +487,15 @@ void mjXURDF::Joint(XMLElement* joint_elem) { if ((elem = FindSubElem(joint_elem, "limit"))) { ReadAttr(elem, "lower", 1, pjoint->range, text); ReadAttr(elem, "upper", 1, pjoint->range+1, text); - pjoint->limited = (mjuu_defined(pjoint->range[0]) && - mjuu_defined(pjoint->range[1]) && - pjoint->range[0] < pjoint->range[1]); + bool is_limited = mjuu_defined(pjoint->range[0]) && + mjuu_defined(pjoint->range[1]) && + pjoint->range[0] < pjoint->range[1]; + pjoint->limited = is_limited ? mjLIMITED_TRUE : mjLIMITED_FALSE; // ReadAttr(elem, "velocity", 1, &pjoint->maxvel, text); // no maxvel in MuJoCo ReadAttr(elem, "effort", 1, &pjoint->urdfeffort, text); } else { - pjoint->limited = 0; + pjoint->limited = mjLIMITED_FALSE; } }