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

Resize keyframes also at the start of compilation.

Browse Source 
Previously, it was assumed that attaching was the last operation performed on the mjSpec, so keyframes resulted in the incorrect size. In order to prevent information loss, we now only allow the resize function to expand the keyframe array, since attach should never remove a degree of freedom.

PiperOrigin-RevId: 804917828
Change-Id: Icfacd207ced1c6aac600d514f8d9065ceff2d347
This commit is contained in:
Alessio Quaglino
2025-09-09 08:15:34 -07:00
committed by Copybara-Service
parent 3b738ebb67
commit 9d5063799f
6 changed files with 83 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_model.cc
+26 -15
View File
@@ -476,14 +476,7 @@ mjCModel& mjCModel::operator+=(const mjCModel& other) {
}
// resize keyframes in the parent model
if (!keys_.empty()) {
SaveDofOffsets(/*computesize=*/true);
ComputeReference();
for (auto* key : keys_) {
ResizeKeyframe(key, qpos0.data(), body_pos0.data(), body_quat0.data());
}
nq = nv = na = nu = nmocap = 0;
}
ExpandAllKeyframes();
// update pointers to local elements
PointToLocal();
@@ -4543,26 +4536,41 @@ void mjCModel::ComputeReference() {
// resize keyframes in the model
void mjCModel::ExpandAllKeyframes() {
if (keys_.empty()) {
return;
}
SaveDofOffsets(/*computesize=*/true);
ComputeReference();
for (auto* key : keys_) {
ExpandKeyframe(key, qpos0.data(), body_pos0.data(), body_quat0.data());
}
nq = nv = na = nu = nmocap = 0;
}
// resizes a keyframe, filling in missing values
void mjCModel::ResizeKeyframe(mjCKey* key, const mjtNum* qpos0_,
void mjCModel::ExpandKeyframe(mjCKey* key, const mjtNum* qpos0_,
const mjtNum* bpos, const mjtNum* bquat) {
if (!key->spec_qpos_.empty()) {
if (!key->spec_qpos_.empty() && nq > key->spec_qpos_.size()) {
int nq0 = key->spec_qpos_.size();
key->spec_qpos_.resize(nq);
for (int i=nq0; i < nq; i++) {
key->spec_qpos_[i] = (double)qpos0_[i];
}
}
if (!key->spec_qvel_.empty()) {
if (!key->spec_qvel_.empty() && nv > key->spec_qvel_.size()) {
key->spec_qvel_.resize(nv);
}
if (!key->spec_act_.empty()) {
if (!key->spec_act_.empty() && na > key->spec_act_.size()) {
key->spec_act_.resize(na);
}
if (!key->spec_ctrl_.empty()) {
if (!key->spec_ctrl_.empty() && nu > key->spec_ctrl_.size()) {
key->spec_ctrl_.resize(nu);
}
if (!key->spec_mpos_.empty()) {
if (!key->spec_mpos_.empty() && nmocap > key->spec_mpos_.size() / 3) {
int nmocap0 = key->spec_mpos_.size() / 3;
key->spec_mpos_.resize(3*nmocap);
for (unsigned int j = 0; j < bodies_.size(); j++) {
@@ -4575,7 +4583,7 @@ void mjCModel::ResizeKeyframe(mjCKey* key, const mjtNum* qpos0_,
key->spec_mpos_[3*i+2] = (double)bpos[3*j+2];
}
}
if (!key->spec_mquat_.empty()) {
if (!key->spec_mquat_.empty() && nmocap > key->spec_mquat_.size() / 4) {
int nmocap0 = key->spec_mquat_.size() / 4;
key->spec_mquat_.resize(4*nmocap);
for (unsigned int j = 0; j < bodies_.size(); j++) {
@@ -4670,6 +4678,9 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
for (const auto& asset : textures_) asset->CopyFromSpec();
CheckEmptyNames();
// resize keyframes in case the spec was edited after the last attach
ExpandAllKeyframes();
// create pending keyframes
for (const auto& info : key_pending_) {
mjCKey* key = AddKey();