diff --git a/CHANGELOG.md b/CHANGELOG.md index e07b37352a..5e3e8481ed 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,7 @@ All notable changes to this project are documented in this file. ### Changed - Update the `IK tutorial` to use `QPInverseKinematics::build` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/621) +- Handle case where no FT sensors are specified to split the model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/625) - General restructure of the `ContactDetector`and the derived classes (`SchmittTriggerDetector` and `FixedFootDetector`) (https://github.com/ami-iit/bipedal-locomotion-framework/pull/624) Thanks to this refactory the `FixedFootDetector` usage becomes similar to the others `advanceable`. Indeed now `FixedFootDetector::advace()` considers the input set by the user and provides the corresponding output. diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h index 3ef1f5c05b..ddae85d386 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h @@ -73,47 +73,102 @@ class SubModel std::vector m_externalContactList; /**< List of the additional external contacts */ public: + /** + * Determines the validity of the object. + * @return True if the object is valid, false otherwise. + */ + bool isValid() const; + /** * Getters */ /** - * @brief Access model. + * @brief Get the `iDynTree::Model` instance. + * @note The actual implementation of the Model is currently stored in an `iDynTree::Model` * @return The model of the SubModel. */ const iDynTree::Model& getModel() const; /** - * @brief Access jointListMapping. + * @brief Access the `std::vectot` list. * @return the mapping between the joint indeces in the sub-model and the joint indeces in the * full-model. */ const std::vector& getJointMapping() const; /** - * @brief Access ftList. + * @brief Access the `std::vector` list. * @return the list of FT objects which is the list of force/torque sensors. */ const std::vector& getFTList() const; /** - * @brief Access accelerometerList. + * @brief Access the `std::vector` list of acceletometer sensors. * @return a list of Sensor objects describing the accelerometers contained in the sub-model. */ const std::vector& getAccelerometerList() const; /** - * @brief Access gyroscopeList. + * @brief Access the `std::vector` list of gyroscope sensors. * @return a list of Sensor objects describing the gyroscope contained in the sub-model. */ const std::vector& getGyroscopeList() const; /** - * @brief Access externalContactList. + * @brief Access the `std::vector` list of frame names. * @return a list of strings describing frame names of the external contacts for the sub-model. */ const std::vector& getExternalContactList() const; + /** + * @brief access the length of force/torque sensor list. + * @return the number of force/torque sensors in the sub-model. + */ + std::size_t getNrOfFTSensor() const; + + /** + * @brief access the length of accelerometer list. + * @return the number of accelerometer sensors in the sub-model. + */ + std::size_t getNrOfAccelerometer() const; + + /** + * @brief access the length of gyroscope list. + * @return the number of gyroscope sensors in the sub-model. + */ + std::size_t getNrOfGyroscope() const; + + /** + * @brief access the length of the contact frame list. + * @return the number of gyroscope sensors in the sub-model. + */ + std::size_t getNrOfExternalContact() const; + + /** + * @brief Access an element of the force/torque sensor list. + * @return FT object associated with the specified index. + */ + const FT& getFTSensor(const int index) const; + + /** + * @brief Access an element of the accelerometer list. + * @return a Sensor object corresponding to the accelerometer associated with the specified index. + */ + const Sensor& getAccelerometer(const int index) const; + + /** + * @brief Access an element of the gyroscope list. + * @return a Sensor object corresponding to the gyroscope associated with the specified index. + */ + const Sensor& getGyroscope(const int index) const; + + /** + * @brief access an element of the contact frame list. + * @return a string corresponding to the external contact frame associated with the specified index. + */ + const std::string& getExternalContact(const int index) const; + friend class SubModelCreator; }; @@ -229,24 +284,18 @@ class SubModelCreator */ bool createSubModels(std::weak_ptr - parameterHandler); + parameterHandler); /** * Setter */ /** - * @brief Set model - * @param model is an iDynTree Model object - */ - void setModel(const iDynTree::Model& model); - - /** - * @brief Set sensorList - * @param sensorList is an iDynTree SensorsList object containing the list of Sensors - * (gyroscopes, accelerometers, force/torque sensors) in the model + * @brief Set model which is an instance of `iDynTree::ModelLoader` and the sensor list which is an instance of `iDynTree::SensorList` + * @param modelLoader is an iDynTree ModelLoader object + * @param sensors is an iDynTree SensorList object */ - void setSensorList(const iDynTree::SensorsList& sensorList); + void setModelAndSensors(const iDynTree::Model& model, const iDynTree::SensorsList& sensors); /** * @brief set kinDyn @@ -261,13 +310,19 @@ class SubModelCreator */ /** - * @brief get subModelList + * @brief access the length of the list std::vector. + * @return the number of sub-models composing the model + */ + std::size_t getNrOfSubModels() const; + + /** + * @brief get the `std::vector` list. * @return the list of SubModel objects. */ const std::vector& getSubModelList() const; /** - * @brief get subModel + * @brief get a `SubModel` instance of the list of `SubModel`. * @return the SubModel at the position index. */ const SubModel& getSubModel(int index) const; diff --git a/src/Estimators/src/SubModel.cpp b/src/Estimators/src/SubModel.cpp index 5b3b7331b8..bf2a7154a2 100644 --- a/src/Estimators/src/SubModel.cpp +++ b/src/Estimators/src/SubModel.cpp @@ -18,10 +18,15 @@ #include namespace blf = BipedalLocomotion; -namespace blfEstim = BipedalLocomotion::Estimators::RobotDynamicsEstimator; +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; -bool blfEstim::SubModelCreator::splitModel(const std::vector& ftFrameList, - std::vector& idynSubModels) +bool RDE::SubModel::isValid() const +{ + return (m_model.getNrOfLinks() > 0); +} + +bool RDE::SubModelCreator::splitModel(const std::vector& ftFrameList, + std::vector& idynSubModels) { constexpr auto logPrefix = "[BipedalLocomotion::RobotDynamicsEstimator::SubModelCreator::" "splitModel]"; @@ -65,10 +70,10 @@ bool blfEstim::SubModelCreator::splitModel(const std::vector& ftFra return true; } -std::vector -blfEstim::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) +std::vector +RDE::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) { - std::vector ftList; + std::vector ftList; for (int ftIdx = 0; ftIdx < this->m_sensorList.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ftIdx++) @@ -79,27 +84,27 @@ blfEstim::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) // add the ft frame to the model as additional frame and save // information on force directiona and parent link iDynTree::SixAxisForceTorqueSensor* sensorFTFromModel - = static_cast( - this->m_sensorList.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, ftIdx)); + = static_cast( + this->m_sensorList.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, ftIdx)); std::string ftName = sensorFTFromModel->getName(); // Get link the link on which the measure force is applied. std::string linkAppliedWrenchName - = this->m_model.getLinkName(sensorFTFromModel->getAppliedWrenchLink()); + = this->m_model.getLinkName(sensorFTFromModel->getAppliedWrenchLink()); - if (idynSubModel.getFrameIndex(ftName) >= 0) + if (idynSubModel.isFrameNameUsed(ftName)) { - blfEstim::FT ft; + RDE::FT ft; ft.name = ftName; ft.frame = ftName; // Retrieve force direction if (idynSubModel.isLinkNameUsed(linkAppliedWrenchName)) { - ft.forceDirection = blfEstim::FT::Direction::Positive; + ft.forceDirection = RDE::FT::Direction::Positive; } else { - ft.forceDirection = blfEstim::FT::Direction::Negative; + ft.forceDirection = RDE::FT::Direction::Negative; } ftList.push_back(ft); @@ -114,39 +119,39 @@ blfEstim::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) if (idynSubModel.isLinkNameUsed(firstLink)) { idynSubModel - .addAdditionalFrameToLink(firstLink, - ftName, - this->m_kinDyn + .addAdditionalFrameToLink(firstLink, + ftName, + this->m_kinDyn ->getRelativeTransform(this->m_kinDyn->model() - .getLinkIndex( - firstLink), + .getLinkIndex( + firstLink), this->m_kinDyn->model() - .getFrameIndex( - ftName))); + .getFrameIndex( + ftName))); } else { idynSubModel - .addAdditionalFrameToLink(secondLink, - ftName, - this->m_kinDyn + .addAdditionalFrameToLink(secondLink, + ftName, + this->m_kinDyn ->getRelativeTransform(this->m_kinDyn->model() - .getLinkIndex( - secondLink), + .getLinkIndex( + secondLink), this->m_kinDyn->model() - .getFrameIndex( - ftName))); + .getFrameIndex( + ftName))); } - blfEstim::FT ft; + RDE::FT ft; ft.name = ftName; ft.frame = ftName; if (idynSubModel.isLinkNameUsed(linkAppliedWrenchName)) { - ft.forceDirection = blfEstim::FT::Direction::Positive; + ft.forceDirection = RDE::FT::Direction::Positive; } else { - ft.forceDirection = blfEstim::FT::Direction::Negative; + ft.forceDirection = RDE::FT::Direction::Negative; } ftList.push_back(std::move(ft)); @@ -157,16 +162,16 @@ blfEstim::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) return ftList; } -std::vector blfEstim::SubModelCreator::attachAccelerometersToSubModel( - const std::vector& accListFromConfig, const iDynTree::Model& subModel) +std::vector RDE::SubModelCreator::attachAccelerometersToSubModel( + const std::vector& accListFromConfig, const iDynTree::Model& subModel) { - std::vector accList; + std::vector accList; for (int idx = 0; idx < accListFromConfig.size(); idx++) { if (subModel.isFrameNameUsed(accListFromConfig[idx].frame)) { - blfEstim::Sensor acc; + RDE::Sensor acc; acc.name = accListFromConfig[idx].name; acc.frame = accListFromConfig[idx].frame; accList.push_back(std::move(acc)); @@ -176,16 +181,16 @@ std::vector blfEstim::SubModelCreator::attachAccelerometersToS return accList; } -std::vector blfEstim::SubModelCreator::attachGyroscopesToSubModel( - const std::vector& gyroListFromConfig, const iDynTree::Model& subModel) +std::vector RDE::SubModelCreator::attachGyroscopesToSubModel( + const std::vector& gyroListFromConfig, const iDynTree::Model& subModel) { - std::vector gyroList; + std::vector gyroList; for (int idx = 0; idx < gyroListFromConfig.size(); idx++) { if (subModel.isFrameNameUsed(gyroListFromConfig[idx].frame)) { - blfEstim::Sensor gyro; + RDE::Sensor gyro; gyro.name = gyroListFromConfig[idx].name; gyro.frame = gyroListFromConfig[idx].frame; gyroList.push_back(std::move(gyro)); @@ -195,8 +200,8 @@ std::vector blfEstim::SubModelCreator::attachGyroscopesToSubMo return gyroList; } -std::vector blfEstim::SubModelCreator::attachExternalContactsToSubModel( - const std::vector& contactsFromConfig, const iDynTree::Model& subModel) +std::vector RDE::SubModelCreator::attachExternalContactsToSubModel( + const std::vector& contactsFromConfig, const iDynTree::Model& subModel) { std::vector contactList; @@ -211,49 +216,52 @@ std::vector blfEstim::SubModelCreator::attachExternalContactsToSubM return contactList; } -std::vector blfEstim::SubModelCreator::createJointMapping(const iDynTree::Model& subModel) +std::vector RDE::SubModelCreator::createJointMapping(const iDynTree::Model& subModel) { std::vector jointListMapping; for (int idx = 0; idx < subModel.getNrOfJoints(); idx++) { - jointListMapping.push_back(this->m_model.getJointIndex(subModel.getJointName(idx))); + if (subModel.getJoint(idx)->getNrOfDOFs() > 0) + { + jointListMapping.push_back(this->m_model.getJointIndex(subModel.getJointName(idx))); + } } return jointListMapping; } -blfEstim::SubModel -blfEstim::SubModelCreator::populateSubModel(iDynTree::Model& idynSubModel, - const std::vector& accList, - const std::vector& gyroList, - const std::vector& externalContacts) +RDE::SubModel +RDE::SubModelCreator::populateSubModel(iDynTree::Model& idynSubModel, + const std::vector& accList, + const std::vector& gyroList, + const std::vector& externalContacts) { - blfEstim::SubModel subModel; + RDE::SubModel subModel; - subModel.m_ftList = blfEstim::SubModelCreator::attachFTsToSubModel(idynSubModel); + subModel.m_ftList = RDE::SubModelCreator::attachFTsToSubModel(idynSubModel); subModel.m_model = idynSubModel.copy(); subModel.m_accelerometerList - = blfEstim::SubModelCreator::attachAccelerometersToSubModel(accList, idynSubModel); + = RDE::SubModelCreator::attachAccelerometersToSubModel(accList, idynSubModel); subModel.m_gyroscopeList - = blfEstim::SubModelCreator::attachGyroscopesToSubModel(gyroList, idynSubModel); + = RDE::SubModelCreator::attachGyroscopesToSubModel(gyroList, idynSubModel); subModel.m_externalContactList - = blfEstim::SubModelCreator::attachExternalContactsToSubModel(externalContacts, - idynSubModel); + = RDE::SubModelCreator::attachExternalContactsToSubModel(externalContacts, + idynSubModel); - subModel.m_jointListMapping = blfEstim::SubModelCreator::createJointMapping(idynSubModel); + subModel.m_jointListMapping = RDE::SubModelCreator::createJointMapping(idynSubModel); return subModel; } -bool blfEstim::SubModelCreator::createSubModels(const std::vector& ftSensorList, - const std::vector& accList, - const std::vector& gyroList, - const std::vector& externalContacts) +bool RDE::SubModelCreator::createSubModels(const std::vector& ftSensorList, + const std::vector& accList, + const std::vector& gyroList, + const std::vector& externalContacts) { constexpr auto logPrefix = "[BipedalLocomotion::RobotDynamicsEstimator::SubModelCreator::" "getSubModels]"; @@ -272,19 +280,30 @@ bool blfEstim::SubModelCreator::createSubModels(const std::vectorm_subModelList.emplace_back( - blfEstim::SubModelCreator::populateSubModel(idynSubModels[idxSubModel], - accList, - gyroList, - externalContacts)); + RDE::SubModelCreator::populateSubModel(this->m_model, + accList, + gyroList, + externalContacts)); + } + else + { + for (int idxSubModel = 0; idxSubModel < idynSubModels.size(); idxSubModel++) + { + this->m_subModelList.emplace_back( + RDE::SubModelCreator::populateSubModel(idynSubModels[idxSubModel], + accList, + gyroList, + externalContacts)); + } } return true; } -bool blfEstim::SubModelCreator::createSubModels( +bool RDE::SubModelCreator::createSubModels( std::weak_ptr parameterHandler) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::RobotDynamicsEstimator::" @@ -347,10 +366,10 @@ bool blfEstim::SubModelCreator::createSubModels( std::vector ftNames, ftFrames; bool ok = populateSensorParameters("FT", ftNames, ftFrames); - std::vector ftList; + std::vector ftList; for (auto idx = 0; idx < ftNames.size(); idx++) { - blfEstim::Sensor ft; + RDE::Sensor ft; ft.name = ftNames[idx]; ft.frame = ftFrames[idx]; ftList.push_back(std::move(ft)); @@ -359,10 +378,10 @@ bool blfEstim::SubModelCreator::createSubModels( std::vector accNames, accFrames; ok = ok && populateSensorParameters("ACCELEROMETER", accNames, accFrames); - std::vector accList; + std::vector accList; for (auto idx = 0; idx < accNames.size(); idx++) { - blfEstim::Sensor acc; + RDE::Sensor acc; acc.name = accNames[idx]; acc.frame = accFrames[idx]; accList.push_back(std::move(acc)); @@ -371,27 +390,29 @@ bool blfEstim::SubModelCreator::createSubModels( std::vector gyroNames, gyroFrames; ok = ok && populateSensorParameters("GYROSCOPE", gyroNames, gyroFrames); - std::vector gyroList; + std::vector gyroList; for (auto idx = 0; idx < gyroNames.size(); idx++) { - blfEstim::Sensor gyro; + RDE::Sensor gyro; gyro.name = gyroNames[idx]; gyro.frame = gyroFrames[idx]; gyroList.push_back(std::move(gyro)); } ok = ok - && blfEstim::SubModelCreator::createSubModels(ftList, accList, gyroList, extContactFrames); + && RDE::SubModelCreator::createSubModels(ftList, accList, gyroList, extContactFrames); return ok; } -void blfEstim::SubModelCreator::setModel(const iDynTree::Model& model) +void RDE::SubModelCreator::setModelAndSensors(const iDynTree::Model& model, const iDynTree::SensorsList& sensors) { - this->m_model = model; + m_model = model; + + m_sensorList = sensors; } -bool blfEstim::SubModelCreator::setKinDyn(std::shared_ptr kinDyn) +bool RDE::SubModelCreator::setKinDyn(std::shared_ptr kinDyn) { if ((kinDyn == nullptr) || (!kinDyn->isValid())) { @@ -404,47 +425,87 @@ bool blfEstim::SubModelCreator::setKinDyn(std::shared_ptrm_sensorList = sensorList; + return this->m_subModelList.size(); } -const std::vector& blfEstim::SubModelCreator::getSubModelList() const +const std::vector& RDE::SubModelCreator::getSubModelList() const { return this->m_subModelList; } -const blfEstim::SubModel& blfEstim::SubModelCreator::getSubModel(int index) const +const RDE::SubModel& RDE::SubModelCreator::getSubModel(int index) const { return this->m_subModelList.at(index); } -const iDynTree::Model& blfEstim::SubModel::getModel() const +const iDynTree::Model& RDE::SubModel::getModel() const { return this->m_model; } -const std::vector& blfEstim::SubModel::getJointMapping() const +const std::vector& RDE::SubModel::getJointMapping() const { return this->m_jointListMapping; } -const std::vector& blfEstim::SubModel::getFTList() const +const std::vector& RDE::SubModel::getFTList() const { return this->m_ftList; } -const std::vector& blfEstim::SubModel::getAccelerometerList() const +const std::vector& RDE::SubModel::getAccelerometerList() const { return this->m_accelerometerList; } -const std::vector& blfEstim::SubModel::getGyroscopeList() const +const std::vector& RDE::SubModel::getGyroscopeList() const { return this->m_gyroscopeList; } -const std::vector& blfEstim::SubModel::getExternalContactList() const +const std::vector& RDE::SubModel::getExternalContactList() const { return this->m_externalContactList; } + +std::size_t RDE::SubModel::getNrOfFTSensor() const +{ + return m_ftList.size(); +} + +std::size_t RDE::SubModel::getNrOfAccelerometer() const +{ + return m_accelerometerList.size(); +} + +std::size_t RDE::SubModel::getNrOfGyroscope() const +{ + return m_gyroscopeList.size(); +} + +std::size_t RDE::SubModel::getNrOfExternalContact() const +{ + return m_externalContactList.size(); +} + +const RDE::FT& RDE::SubModel::getFTSensor(const int index) const +{ + return m_ftList.at(index); +} + +const RDE::Sensor& RDE::SubModel::getAccelerometer(const int index) const +{ + return m_accelerometerList.at(index); +} + +const RDE::Sensor& RDE::SubModel::getGyroscope(const int index) const +{ + return m_gyroscopeList.at(index); +} + +const std::string& RDE::SubModel::getExternalContact(const int index) const +{ + return m_externalContactList.at(index); +} diff --git a/src/Estimators/src/SubModelKinDynWrapper.cpp b/src/Estimators/src/SubModelKinDynWrapper.cpp index a21ba7f914..26e464416c 100644 --- a/src/Estimators/src/SubModelKinDynWrapper.cpp +++ b/src/Estimators/src/SubModelKinDynWrapper.cpp @@ -234,6 +234,22 @@ bool RDE::SubModelKinDynWrapper::inverseDynamics(Eigen::Ref mot Eigen::Ref baseAcceleration, Eigen::Ref jointAcceleration) { + constexpr auto logPrefix = "[SubModelKinDynWrapper::inverseDynamics]"; + + if (m_subModel.getModel().getNrOfDOFs() == 0) + { + blf::log()->error("{} Inverse dynamics is not defined for sub-models with zero degrees of freedom.", + logPrefix); + return false; + } + + if (motorTorqueAfterGearbox.size() == 0 || frictionTorques.size() == 0 || tauExt.size() == 0 || baseAcceleration.size() == 0) + { + blf::log()->error("{} Wrong size of input parameters.", + logPrefix); + return false; + } + m_FTranspose = m_massMatrix.block(6, 0, m_numOfJoints, 6); m_H = m_massMatrix.block(6, 6, m_numOfJoints, m_numOfJoints); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/SubModelCreatorTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/SubModelCreatorTest.cpp index 92afd7536a..142b2bc965 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/SubModelCreatorTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/SubModelCreatorTest.cpp @@ -73,10 +73,25 @@ TEST_CASE("SubModel Creation") auto kinDyn = std::make_shared(); REQUIRE(kinDyn->loadRobotModel(mdlLdr.model())); - RDE::SubModelCreator subModelCreator; - subModelCreator.setModel(mdlLdr.model()); - REQUIRE(subModelCreator.setKinDyn(kinDyn)); - subModelCreator.setSensorList(mdlLdr.sensors()); - - REQUIRE(subModelCreator.createSubModels(originalHandler)); + // Case with FT, the model will be splitted + RDE::SubModelCreator subModelCreatorWithFT; + subModelCreatorWithFT.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); + REQUIRE(subModelCreatorWithFT.setKinDyn(kinDyn)); + REQUIRE(subModelCreatorWithFT.createSubModels(originalHandler)); + + REQUIRE(subModelCreatorWithFT.getNrOfSubModels() == (1 + mdlLdr.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE))); + + // Case without FT, the model is not splitted and the only sub-model is the full model + RDE::SubModelCreator subModelCreatorWithoutFT; + subModelCreatorWithoutFT.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); + REQUIRE(subModelCreatorWithoutFT.setKinDyn(kinDyn)); + auto groupFT = parameterHandler->getGroup("FT").lock()->clone(); + groupFT->clear(); + std::vector emptyVector; + groupFT->setParameter("names", emptyVector); + groupFT->setParameter("frames", emptyVector); + parameterHandler->setGroup("FT", groupFT); + REQUIRE(subModelCreatorWithoutFT.createSubModels(parameterHandler)); + + REQUIRE(subModelCreatorWithoutFT.getNrOfSubModels() == 1); } diff --git a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp index 4f31bc13b5..d7205cd66c 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp @@ -118,9 +118,8 @@ TEST_CASE("SubModelKinDynWrapper") // List of joints and fts to load the model RDE::SubModelCreator subModelCreator; - subModelCreator.setModel(mdlLdr.model()); + subModelCreator.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); REQUIRE(subModelCreator.setKinDyn(kinDyn)); - subModelCreator.setSensorList(mdlLdr.sensors()); REQUIRE(subModelCreator.createSubModels(originalHandler));