chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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_api.h
+13 -6
View File
@@ -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
+6 -6
View File
@@ -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;
}
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;
src/user/user_objects.h
+7 -15
View File
@@ -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 { 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<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 { 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_;
src/xml/xml_urdf.cc
+5 -4
View File
@@ -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;
}
}