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) {