Display equality constraints in the simulate UI.

Add section for toggling constraint activity to the UI, similar to actuators or joint controls.

PiperOrigin-RevId: 772866923
Change-Id: I017b9195fcf67b5865e9d44d6c8aded64c8bc46d

simulate/simulate.cc

@@ -112,6 +112,7 @@ enum {
   // right ui
   SECT_JOINT = 0,
   SECT_CONTROL,
+  SECT_EQUALITY,
   NSECT1
 };
 
@@ -1127,6 +1128,36 @@ void MakeControlSection(mj::Simulate* sim) {
   }
 }
 
+// make equality section of UI
+void MakeEqualitySection(mj::Simulate* sim) {
+  mjuiDef defEquality[] = {
+    {mjITEM_SECTION, "Equality", mjPRESERVE, nullptr, "AE"},
+    {mjITEM_END}
+  };
+  mjuiDef defCheckBox[] = {
+    {mjITEM_CHECKBYTE, "", 2, nullptr, ""},
+    {mjITEM_END}
+  };
+
+  // add section
+  mjui_add(&sim->ui1, defEquality);
+
+  // add equalities, exit if UI limit reached
+  for (int i= 0; i < sim->m_->neq && i<mjMAXUIITEM; i++) {
+    // set data
+    defCheckBox[0].pdata = &sim->d_->eq_active[i];
+
+    // set name
+    if (!sim->equality_names_[i].empty()) {
+      mju::strcpy_arr(defCheckBox[0].name, sim->equality_names_[i].c_str());
+    } else {
+      mju::sprintf_arr(defCheckBox[0].name, "equality %d", i);
+    }
+
+    mjui_add(&sim->ui1, defCheckBox);
+  }
+}
+
 // make model-dependent UI sections
 void MakeUiSections(mj::Simulate* sim, const mjModel* m, const mjData* d) {
   // clear model-dependent sections of UI
@@ -1140,6 +1171,7 @@ void MakeUiSections(mj::Simulate* sim, const mjModel* m, const mjData* d) {
   MakeGroupSection(sim);
   MakeJointSection(sim);
   MakeControlSection(sim);
+  MakeEqualitySection(sim);
 }
 
 //---------------------------------- utility functions ---------------------------------------------
@@ -1931,6 +1963,18 @@ void Simulate::Sync() {
     }
   }
 
+  for (int i = 0; i < m_->neq; ++i) {
+    if (eq_active_[i] != eq_active_prev_[i]) {
+      d_->eq_active[i] = eq_active_[i];
+    } else {
+      eq_active_[i] = d_->eq_active[i];
+    }
+    if (eq_active_prev_[i] != eq_active_[i]) {
+      pending_.ui_update_equality = true;
+      eq_active_prev_[i] = eq_active_[i];
+    }
+  }
+
   // in passive mode, synchronize user's mjModel with changes made via the UI
   if (is_passive_) {
     // synchronize mjModel.opt
@@ -2261,6 +2305,12 @@ void Simulate::LoadOnRenderThread() {
     actuator_names_.emplace_back(this->m_->names + this->m_->name_actuatoradr[i]);
   }
 
+  equality_names_.clear();
+  equality_names_.reserve(this->m_->neq);
+  for (int i = 0; i < this->m_->neq; ++i) {
+    equality_names_.emplace_back(this->m_->names + this->m_->name_eqadr[i]);
+  }
+
   qpos_.resize(this->m_->nq);
   std::memcpy(qpos_.data(), this->d_->qpos, sizeof(this->d_->qpos[0]) * this->m_->nq);
   qpos_prev_ = qpos_;
@@ -2269,6 +2319,10 @@ void Simulate::LoadOnRenderThread() {
   std::memcpy(ctrl_.data(), this->d_->ctrl, sizeof(this->d_->ctrl[0]) * this->m_->nu);
   ctrl_prev_ = ctrl_;
 
+  eq_active_.resize(this->m_->neq);
+  std::memcpy(eq_active_.data(), this->d_->eq_active, sizeof(this->d_->eq_active[0]) * this->m_->neq);
+  eq_active_prev_ = eq_active_;
+
   // allocate history buffer: smaller of {2000 states, 100 MB}
   if (!this->is_passive_) {
     constexpr int kMaxHistoryBytes = 1e8;
@@ -2518,6 +2572,13 @@ void Simulate::Render() {
     pending_.ui_update_ctrl = false;
   }
 
+  if (pending_.ui_update_equality) {
+    if (this->ui1_enable && this->ui1.sect[SECT_EQUALITY].state) {
+      mjui_update(SECT_EQUALITY, -1, &this->ui1, &this->uistate, &this->platform_ui->mjr_context());
+    }
+    pending_.ui_update_equality = false;
+  }
+
   // render scene
   mjr_render(rect, &this->scn, &this->platform_ui->mjr_context());
 

simulate/simulate.h

@@ -122,6 +122,8 @@ class Simulate {
   std::vector<std::optional<std::pair<mjtNum, mjtNum>>> actuator_ctrlrange_;
   std::vector<std::string> actuator_names_;
 
+  std::vector<std::string> equality_names_;
+
   std::vector<mjtNum> history_;  // history buffer (nhistory x state_size)
 
   // mjModel and mjData fields that can be modified by the user through the GUI
@@ -129,6 +131,8 @@ class Simulate {
   std::vector<mjtNum> qpos_prev_;
   std::vector<mjtNum> ctrl_;
   std::vector<mjtNum> ctrl_prev_;
+  std::vector<mjtByte> eq_active_;
+  std::vector<mjtByte> eq_active_prev_;
 
   // in passive mode the user owns m_ and d_, these "passive" instances are
   // owned by Simulate, updated from the user by the Sync() method
@@ -167,6 +171,7 @@ class Simulate {
     bool ui_update_visualization;
     bool ui_update_joint;
     bool ui_update_ctrl;
+    bool ui_update_equality;
     bool ui_remake_ctrl;
   } pending_ = {};