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

Add mocap state to SaveState/RestoreState and attached keyframes.

Browse Source 
PiperOrigin-RevId: 662975257
Change-Id: I3edb44f34426525813033902caf31db16ad7b2ab
This commit is contained in:
Alessio Quaglino
2024-08-14 10:41:40 -07:00
committed by Copybara-Service
parent 5927b3d4d9
commit 4c64e6f036
8 changed files with 264 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_model.cc
+109 -36
View File
@@ -109,9 +109,6 @@ mjCModel::mjCModel() {
// this class allocated the plugins
plugin_owner = true;
// default state name
state_name_ = "state";
}
@@ -216,6 +213,7 @@ void mjCModel::SaveDofOffsets() {
int qposadr = 0;
int dofadr = 0;
int actadr = 0;
int nmocap = 0;
for (auto joint : joints_) {
joint->qposadr_ = qposadr;
@@ -233,6 +231,15 @@ void mjCModel::SaveDofOffsets() {
actuator->actadr_ = actuator->actdim_ ? actadr : -1;
actadr += actuator->actdim_;
}
for (mjCBody* body : bodies_) {
if (body->spec.mocap) {
body->mocapid = nmocap;
nmocap++;
} else {
body->mocapid = -1;
}
}
}
@@ -2863,23 +2870,36 @@ void mjCModel::CopyObjects(mjModel* m) {
// save the current state
template <class T>
void mjCModel::SaveState(const T* qpos, const T* qvel, const T* act, const T* ctrl) {
void mjCModel::SaveState(const std::string& state_name, const T* qpos, const T* qvel, const T* act,
const T* ctrl, const T* mpos, const T* mquat) {
for (auto joint : joints_) {
if (joint->qposadr_ == -1 || joint->dofadr_ == -1) {
throw mjCError(NULL, "SaveState: joint %s has no address", joint->name.c_str());
}
if (qpos) mjuu_copyvec(joint->qpos(), qpos + joint->qposadr_, joint->nq());
if (qvel) mjuu_copyvec(joint->qvel(), qvel + joint->dofadr_, joint->nv());
if (qpos) mjuu_copyvec(joint->qpos(state_name), qpos + joint->qposadr_, joint->nq());
if (qvel) mjuu_copyvec(joint->qvel(state_name), qvel + joint->dofadr_, joint->nv());
}
for (unsigned int i=0; i<actuators_.size(); i++) {
auto actuator = actuators_[i];
if (actuator->actadr_ != -1 && actuator->actdim_ != -1 && act) {
actuator->act().assign(actuator->actdim_, 0);
mjuu_copyvec(actuator->act().data(), act + actuator->actadr_, actuator->actdim_);
actuator->act(state_name).assign(actuator->actdim_, 0);
mjuu_copyvec(actuator->act(state_name).data(), act + actuator->actadr_, actuator->actdim_);
}
if (ctrl) {
actuator->ctrl() = ctrl[i];
actuator->ctrl(state_name) = ctrl[i];
}
}
for (auto body : bodies_) {
if (!body->spec.mocap) {
continue;
}
if (mpos) {
mjuu_copyvec(body->mpos(state_name), mpos + 3*body->mocapid, 3);
}
if (mquat) {
mjuu_copyvec(body->mquat(state_name), mquat + 4*body->mocapid, 4);
}
}
}
@@ -2900,42 +2920,63 @@ void mjCModel::MakeData(const mjModel* m, mjData** dest) {
// restore the previous state
template <class T>
void mjCModel::RestoreState(const mjtNum* pos0, T* qpos, T* qvel, T* act, T* ctrl) {
void mjCModel::RestoreState(const std::string& state_name, const mjtNum* pos0,
const mjtNum* mpos0, const mjtNum* mquat0, T* qpos,
T* qvel, T* act, T* ctrl, T* mpos, T* mquat) {
for (auto joint : joints_) {
if (qpos) {
if (mjuu_defined(joint->qpos()[0])) {
mjuu_copyvec(qpos + joint->qposadr_, joint->qpos(), joint->nq());
if (mjuu_defined(joint->qpos(state_name)[0])) {
mjuu_copyvec(qpos + joint->qposadr_, joint->qpos(state_name), joint->nq());
} else {
mjuu_copyvec(qpos + joint->qposadr_, pos0 + joint->qposadr_, joint->nq());
}
}
if (mjuu_defined(joint->qvel()[0]) && qvel) {
mjuu_copyvec(qvel + joint->dofadr_, joint->qvel(), joint->nv());
if (mjuu_defined(joint->qvel(state_name)[0]) && qvel) {
mjuu_copyvec(qvel + joint->dofadr_, joint->qvel(state_name), joint->nv());
}
}
// restore act
for (unsigned int i=0; i<actuators_.size(); i++) {
auto actuator = actuators_[i];
if (!actuator->act().empty() && mjuu_defined(actuator->act()[0]) && act) {
mjuu_copyvec(act + actuator->actadr_, actuator->act().data(), actuator->actdim_);
if (!actuator->act(state_name).empty() && mjuu_defined(actuator->act(state_name)[0]) && act) {
mjuu_copyvec(act + actuator->actadr_, actuator->act(state_name).data(), actuator->actdim_);
}
if (ctrl) {
ctrl[i] = mjuu_defined(actuator->ctrl()) ? actuator->ctrl() : 0;
ctrl[i] = mjuu_defined(actuator->ctrl(state_name)) ? actuator->ctrl(state_name) : 0;
}
}
for (unsigned int i=0; i<bodies_.size(); i++) {
auto body = bodies_[i];
if (!body->mocap) {
continue;
}
if (mpos) {
if (mjuu_defined(body->mpos(state_name)[0])) {
mjuu_copyvec(mpos + 3*body->mocapid, body->mpos(state_name), 3);
} else {
mjuu_copyvec(mpos + 3*body->mocapid, mpos0 + 3*i, 3);
}
}
if (mquat) {
if (mjuu_defined(body->mquat(state_name)[0])) {
mjuu_copyvec(mquat + 4*body->mocapid, body->mquat(state_name), 4);
} else {
mjuu_copyvec(mquat + 4*body->mocapid, mquat0 + 4*i, 4);
}
}
}
}
// force explicit instantiations
template void mjCModel::SaveState<mjtNum>(const mjtNum* qpos,
const mjtNum* qvel,
const mjtNum* act,
const mjtNum* ctrl);
template void mjCModel::SaveState<mjtNum>(
const std::string& name, const mjtNum* qpos, const mjtNum* qvel, const mjtNum* act,
const mjtNum* ctrl, const mjtNum* mpos, const mjtNum* mquat);
template void mjCModel::RestoreState<mjtNum>(const mjtNum* qpos0, mjtNum* qpos,
mjtNum* qvel, mjtNum* act, mjtNum* ctrl);
template void mjCModel::RestoreState<mjtNum>(
const std::string& name, const mjtNum* qpos0, const mjtNum* mpos0, const mjtNum* mquat0,
mjtNum* qpos, mjtNum* qvel, mjtNum* act, mjtNum* ctrl, mjtNum* mpos, mjtNum* mquat);
@@ -2957,10 +2998,12 @@ void mjCModel::StoreKeyframes() {
info.qvel = !key->spec_qvel_.empty();
info.act = !key->spec_act_.empty();
info.ctrl = !key->spec_ctrl_.empty();
info.mpos = !key->spec_mpos_.empty();
info.mquat = !key->spec_mquat_.empty();
key_pending_.push_back(info);
state_name_ = info.name;
SaveState(key->spec_qpos_.data(), key->spec_qvel_.data(),
key->spec_act_.data(), key->spec_ctrl_.data());
SaveState(info.name, key->spec_qpos_.data(), key->spec_qvel_.data(),
key->spec_act_.data(), key->spec_ctrl_.data(),
key->spec_mpos_.data(), key->spec_mquat_.data());
}
if (resetlists) {
@@ -3502,7 +3545,10 @@ void mjCModel::ResolveKeyframes(const mjModel* m) {
return;
}
// resize non-pending keyframes to the new number of dofs
// store dof offsets in joints and actuators
SaveDofOffsets();
// resize existing keyframes to the new state, fill in missing default values
for (unsigned int i = 0; i < nkey - key_pending_.size(); i++) {
mjCKey* key = keys_[i];
if (!key->spec_qpos_.empty()) {
@@ -3521,12 +3567,36 @@ void mjCModel::ResolveKeyframes(const mjModel* m) {
if (!key->spec_ctrl_.empty()) {
key->spec_ctrl_.resize(nu);
}
if (!key->spec_mpos_.empty()) {
int nmocap0 = key->spec_mpos_.size() / 3;
key->spec_mpos_.resize(3*nmocap);
for (unsigned int j = 0; j < bodies_.size(); j++) {
if (bodies_[j]->mocapid < nmocap0) {
continue;
}
int i = bodies_[j]->mocapid;
key->spec_mpos_[3*i+0] = (double)m->body_pos[3*j+0];
key->spec_mpos_[3*i+1] = (double)m->body_pos[3*j+1];
key->spec_mpos_[3*i+2] = (double)m->body_pos[3*j+2];
}
}
if (!key->spec_mquat_.empty()) {
int nmocap0 = key->spec_mquat_.size() / 4;
key->spec_mquat_.resize(4*nmocap);
for (unsigned int j = 0; j < bodies_.size(); j++) {
if (bodies_[j]->mocapid < nmocap0) {
continue;
}
int i = bodies_[j]->mocapid;
key->spec_mquat_[4*i+0] = (double)m->body_quat[4*j+0];
key->spec_mquat_[4*i+1] = (double)m->body_quat[4*j+1];
key->spec_mquat_[4*i+2] = (double)m->body_quat[4*j+2];
key->spec_mquat_[4*i+3] = (double)m->body_quat[4*j+3];
}
}
}
// store dof offsets in joints and actuators
SaveDofOffsets();
// copy state stored in joints and actuators to keyframes
// create new keyframes, fill in missing default values
for (const auto& info : key_pending_) {
mjCKey* key = (mjCKey*)FindObject(mjOBJ_KEY, info.name);
key->name = info.name;
@@ -3535,9 +3605,12 @@ void mjCModel::ResolveKeyframes(const mjModel* m) {
if (info.qvel) key->spec_qvel_.assign(nv, 0);
if (info.act) key->spec_act_.assign(na, 0);
if (info.ctrl) key->spec_ctrl_.assign(nu, 0);
state_name_ = info.name;
RestoreState(m->qpos0, key->spec_qpos_.data(), key->spec_qvel_.data(),
key->spec_act_.data(), key->spec_ctrl_.data());
if (info.mpos) key->spec_mpos_.assign(3*nmocap, 0);
if (info.mquat) key->spec_mquat_.assign(4*nmocap, 0);
RestoreState(info.name, m->qpos0, m->body_pos, m->body_quat,
key->spec_qpos_.data(), key->spec_qvel_.data(),
key->spec_act_.data(), key->spec_ctrl_.data(),
key->spec_mpos_.data(), key->spec_mquat_.data());
}
// the attached keyframes have been copied into the model