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

Clean up handling of auto-limits.

Browse Source 
PiperOrigin-RevId: 607266967
Change-Id: Iec8332cc8467ea3c2dc046acf87d221b5af67259
This commit is contained in:
Yuval Tassa
2024-02-15 02:42:50 -08:00
committed by Copybara-Service
parent d561995c03
commit ddabfb9b6a
5 changed files with 72 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_objects.cc
+41 -45
View File
@@ -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;