From 9d5063799fc4fb6ed3100ea24e9e51a9a8ea2cd9 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 9 Sep 2025 08:15:34 -0700 Subject: [PATCH] Resize keyframes also at the start of compilation. 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 --- doc/XMLreference.rst | 4 ++- src/user/user_model.cc | 41 ++++++++++++++++++----------- src/user/user_model.h | 7 +++-- src/user/user_objects.cc | 9 ++++--- test/user/user_objects_test.cc | 2 +- test/xml/xml_native_reader_test.cc | 42 ++++++++++++++++++++++++++++++ 6 files changed, 83 insertions(+), 22 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 131132a6..2dc24856 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -7817,7 +7817,9 @@ in :ref:`simulate.cc ` the simulation state can be copied into a sel :el-prefix:`keyframe/` |-| **key** (*) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This element sets the data for one of the keyframes. They are set in the order in which they appear here. +This element sets the data for one of the keyframes. They are set in the order in which they appear here. If the number +of elements specified in the given vectors is less than the size of the corresponding mjData array, missing entries will +be set to their values in the default configuration. .. _keyframe-key-name: diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 964e6e5b..31e81b60 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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(); diff --git a/src/user/user_model.h b/src/user/user_model.h index e6a5ae9f..8639f319 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -432,8 +432,8 @@ class mjCModel : public mjCModel_, private mjSpec { // convert pending keyframes info to actual keyframes void ResolveKeyframes(const mjModel* m); - // resize a keyframe, filling in missing values - void ResizeKeyframe(mjCKey* key, const mjtNum* qpos0_, const mjtNum* bpos, const mjtNum* bquat); + // expand a keyframe, filling in missing values + void ExpandKeyframe(mjCKey* key, const mjtNum* qpos0_, const mjtNum* bpos, const mjtNum* bquat); // compute qpos0 void ComputeReference(); @@ -463,6 +463,9 @@ class mjCModel : public mjCModel_, private mjSpec { // delete all plugins created by the subtree void DeleteSubtreePlugin(mjCBody* subtree); + // expand all keyframes in the model + void ExpandAllKeyframes(); + mjListKeyMap ids; // map from object names to ids mjCError errInfo; // last error info std::vector key_pending_; // attached keyframes diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index a90624ba..d25ef1fa 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -7657,7 +7657,8 @@ void mjCKey::Compile(const mjModel* m) { qpos_[i] = (double)m->qpos0[i]; } } else if (qpos_.size() != m->nq) { - throw mjCError(this, "keyframe %d: invalid qpos size, expected length %d", nullptr, id, m->nq); + throw mjCError(this, "keyframe '%s': invalid qpos size, expected %d, got %d", + name.c_str(), m->nq, qpos_.size()); } // qvel: allocate or check size @@ -7667,7 +7668,8 @@ void mjCKey::Compile(const mjModel* m) { qvel_[i] = 0; } } else if (qvel_.size() != m->nv) { - throw mjCError(this, "keyframe %d: invalid qvel size, expected length %d", nullptr, id, m->nv); + throw mjCError(this, "keyframe '%s': invalid qvel size, expected %d, got %d", + name.c_str(), m->nv, qvel_.size()); } // act: allocate or check size @@ -7677,7 +7679,8 @@ void mjCKey::Compile(const mjModel* m) { act_[i] = 0; } } else if (act_.size() != m->na) { - throw mjCError(this, "keyframe %d: invalid act size, expected length %d", nullptr, id, m->na); + throw mjCError(this, "keyframe '%s': invalid act size, expected %d, got %d", + name.c_str(), m->na, act_.size()); } // mpos: allocate or check size diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 7d955ce6..57a66d91 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -464,7 +464,7 @@ TEST_F(KeyframeTest, BadSize) { size_t error_sz = 1024; mjModel* model = LoadModelFromString(xml, error, error_sz); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("invalid qpos size, expected length 0")); + EXPECT_THAT(error, HasSubstr("invalid qpos size, expected 0, got 1")); } // ------------- test relative frame sensor compilation------------------------- diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 5ef01ecc..b6c0779a 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -1925,6 +1925,48 @@ TEST_F(XMLReaderTest, LookupCompilerOptionWithoutSpecCopy) { mj_deleteVFS(vfs.get()); } +TEST_F(XMLReaderTest, ResizeKeyframeAfterParsing) { + static constexpr char parent_xml[] = R"( + + + + + + + + + + + + + + + + )"; + + static constexpr char child_xml[] = R"( + + + + + + + + + )"; + + auto vfs = std::make_unique(); + mj_defaultVFS(vfs.get()); + mj_addBufferVFS(vfs.get(), "child.xml", child_xml, sizeof(child_xml)); + + std::array error; + mjModel* m = + LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get()); + EXPECT_THAT(m, NotNull()) << error.data(); + mj_deleteModel(m); + mj_deleteVFS(vfs.get()); +} + // ----------------------- test camera parsing --------------------------------- TEST_F(XMLReaderTest, CameraInvalidFovyAndSensorsize) {