diff --git a/doc/includes/references.h b/doc/includes/references.h index 9a318e10..0b9dc319 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -71,11 +71,11 @@ struct mjContact_ { // result of collision detection functions int geom1; // id of geom 1 int geom2; // id of geom 2 - // flag set by mj_fuseContact or mj_instantianteEquality - int exclude; // 0: include, 1: in gap, 2: fused, 3: equality, 4: no dofs + // flag set by mj_instantianteEquality + int exclude; // 0: include, 1: in gap, 2: fused, 3: no dofs // address computed by mj_instantiateContact - int efc_address; // address in efc; -1: not included, -2-i: distance constraint i + int efc_address; // address in efc; -1: not included }; typedef struct mjContact_ mjContact; struct mjWarningStat_ { // warning statistics diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 66e96662..3f0c22c2 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -84,11 +84,11 @@ struct mjContact_ { // result of collision detection functions int geom1; // id of geom 1 int geom2; // id of geom 2 - // flag set by mj_fuseContact or mj_instantianteEquality - int exclude; // 0: include, 1: in gap, 2: fused, 3: equality, 4: no dofs + // flag set by mj_instantianteEquality + int exclude; // 0: include, 1: in gap, 2: fused, 3: no dofs // address computed by mj_instantiateContact - int efc_address; // address in efc; -1: not included, -2-i: distance constraint i + int efc_address; // address in efc; -1: not included }; typedef struct mjContact_ mjContact; diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 7a47b00a..80adf0df 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -540,10 +540,6 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { size = 1; break; - case mjEQ_DISTANCE: - mju_error("distance equality constraints are no longer supported"); - break; - default: // SHOULD NOT OCCUR mju_error_i("Invalid equality constraint type %d", m->eq_type[i]); } @@ -806,7 +802,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { // skip contact if no DOFs affected if (NV==0) { con->efc_address = -1; - con->exclude = 4; + con->exclude = 3; continue; } @@ -920,8 +916,8 @@ void mj_diagApprox(const mjModel* m, mjData* d) { m->tendon_invweight0[m->eq_obj2id[id]]); break; - case mjEQ_DISTANCE: - mju_error("distance equality constraints are no longer supported"); + default: + mju_error_i("Unknown constraint type type %d", d->efc_type[i]); // SHOULD NOT OCCUR } break; diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 9fda1bab..89dafcec 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -908,7 +908,7 @@ void mj_transmission(const mjModel* m, mjData* d) { } } - // excluded contact, get sparse or dense Jacobian, accumulate + // excluded contact in gap: get sparse or dense Jacobian, accumulate else if (con->exclude == 1) { counter++; @@ -2034,10 +2034,6 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { i++; break; - case mjEQ_DISTANCE: - mju_error("distance equality constraints are no longer supported"); - break; - default: mju_error_i("Unknown constraint type type %d", m->eq_type[id]); // SHOULD NOT OCCUR } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 149d2eeb..d1a71b11 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -468,7 +468,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjtNum *cur, *nxt, *xpos, *xfrc; mjtNum vec[3], end[3], axis[3], rod, len, det, tmp[9], quat[4]; mjtByte broken; - mjContact *con; mjvGeom* thisgeom; mjvPerturb localpert; float scl = m->stat.meansize, rgba[4]; @@ -1667,39 +1666,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, FINISH } } - - // distance: find constraints at the end of the contact list - int j = d->ncon-1; - while (j>=0 && d->contact[j].exclude==3) { - // recover constraint id - int i = -d->contact[j].efc_address-2; - - // segment = pos +/- normal*len - con = d->contact+j; - len = con->dist - m->eq_data[mjNEQDATA*i]; - mju_addScl3(vec, con->pos, con->frame, 0.5*len); - mju_addScl3(end, con->pos, con->frame, -0.5*len); - - // connect endpoints - START - - // construct geom - sz[0] = scl * m->vis.scale.constraint; - mjv_makeConnector(thisgeom, mjGEOM_CAPSULE, sz[0], - vec[0], vec[1], vec[2], - end[0], end[1], end[2]); - - f2f(thisgeom->rgba, m->vis.rgba.constraint, 4); - - // vopt->label flag - if (vopt->label==mjLABEL_CONSTRAINT) { - makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label); - } - - FINISH - - j--; - } } // contact diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 7142b6bb..09bac1fb 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -2957,8 +2957,6 @@ void mjCEquality::Compile(void) { objtype = mjOBJ_JOINT; } else if (type==mjEQ_TENDON) { objtype = mjOBJ_TENDON; - } else if (type==mjEQ_DISTANCE) { - objtype = mjOBJ_GEOM; } else { throw mjCError(this, "invalid type in equality constraint '%s' (id = %d)'", name.c_str(), id); } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 77934565..2161d0c2 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -483,11 +483,8 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) { WriteAttr(elem, "polycoef", 5, peq->data); break; - case mjEQ_DISTANCE: - WriteAttrTxt(elem, "geom1", peq->name1); - WriteAttrTxt(elem, "geom2", peq->name2); - WriteAttr(elem, "distance", 1, peq->data); - break; + default: + mju_error("mjXWriter: unknown equality type."); } }