diff --git a/simulate/simulate.cc b/simulate/simulate.cc index b6a1b9e4..273b7f7a 100644 --- a/simulate/simulate.cc +++ b/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 && id_->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()); diff --git a/simulate/simulate.h b/simulate/simulate.h index 38234e4c..03c0a0b1 100644 --- a/simulate/simulate.h +++ b/simulate/simulate.h @@ -122,6 +122,8 @@ class Simulate { std::vector>> actuator_ctrlrange_; std::vector actuator_names_; + std::vector equality_names_; + std::vector 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 qpos_prev_; std::vector ctrl_; std::vector ctrl_prev_; + std::vector eq_active_; + std::vector 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_ = {};