Clean up handling of auto-limits.

PiperOrigin-RevId: 607266967
Change-Id: Iec8332cc8467ea3c2dc046acf87d221b5af67259

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

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.limited = mjLIMITED_AUTO;
-  joint.actfrclimited = 2;
+  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.ctrllimited = mjLIMITED_AUTO;
-  actuator.forcelimited = 2;
+  actuator.forcelimited = mjLIMITED_AUTO;
-  actuator.actlimited = 2;
+  actuator.actlimited = mjLIMITED_AUTO;
 }
 
 

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
+  // otherwise if limited is auto, check consistency wrt auto-limits
-  else if (limited==2) {
+  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
+  // otherwise if actfrclimited is auto, check consistency wrt auto-limits
-  else if (actfrclimited==2) {
+  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 limited is auto, check for inconsistency wrt to autolimits
-  if (forcelimited==2) {
+  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 == mjLIMITED_AUTO) {
-  if (ctrllimited==2) {
     bool hasrange = !(ctrlrange[0]==0 && ctrlrange[1]==0);
     checklimited(this, model->autolimits, "actuator", "ctrl", ctrllimited, hasrange);
-    ctrllimited_ = hasrange;
-  } else {
-    ctrllimited_ = (ctrllimited == 1);
   }
-
+  if (actlimited == mjLIMITED_AUTO) {
-  if (actlimited==2) {
     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;
 

src/user/user_objects.h

@@ -337,9 +337,8 @@ class mjCJoint : public mjCBase, private mjmJoint {
   // used by mjXWriter and mjCModel
   const std::vector<double>& get_userdata() { return userdata_; }
 
-  // public getters
+  bool is_limited() const;
-  bool is_limited() const { return limited_; }
+  bool is_actfrclimited() const;
-  bool is_actfrclimited() const { return actfrclimited_; }
 
 
  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<double> userdata_;
   std::vector<double> spec_userdata_;
@@ -1087,8 +1085,7 @@ class mjCTendon : public mjCBase, private mjmTendon {
   void CopyFromSpec();
   void PointToLocal();
 
-  // public getters
+  bool is_limited() const;
-  bool is_limited() const { return limited_; }
 
  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;
-  bool is_ctrllimited() const { return ctrllimited_; }
+  bool is_forcelimited() const;
-  bool is_forcelimited() const { return forcelimited_; }
+  bool is_actlimited() const;
-  bool is_actlimited() const { return actlimited_; }
 
  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_;

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]) &&
+    bool is_limited = mjuu_defined(pjoint->range[0]) &&
-                       mjuu_defined(pjoint->range[1]) &&
+                      mjuu_defined(pjoint->range[1]) &&
-                       pjoint->range[0] < 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;
   }
 }