From 75888484a2a839fdfc90e02c2c62ceb38083de76 Mon Sep 17 00:00:00 2001 From: Ines Date: Wed, 22 Mar 2023 11:23:11 +0100 Subject: [PATCH] Implement a more robust test of SubModelKinDynWrapper class. --- .../SubModelKinDynWrapper.h | 4 +- src/Estimators/src/SubModelKinDynWrapper.cpp | 13 +- .../SubModelKinDynWrapperTest.cpp | 166 +++++++++++++++--- 3 files changed, 155 insertions(+), 28 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h index ce41076b0b..24581e570d 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h @@ -108,7 +108,7 @@ class SubModelKinDynWrapper bool updateKinDynState(); /** - * @brief inverseDynamics computes the free floaing inverse dynamics + * @brief forwardDynamics computes the free floaing forward dynamics * @param motorTorqueAfterGearbox a vector of size number of joints containing the motor torques * times the gearbox ratio. * @param frictionTorques a vector of size number of joints containing the friction torques. @@ -117,7 +117,7 @@ class SubModelKinDynWrapper * @param baseAcceleration the sub-model base acceleration. * @return a boolean value */ - bool inverseDynamics(Eigen::Ref motorTorqueAfterGearbox, + bool forwardDynamics(Eigen::Ref motorTorqueAfterGearbox, Eigen::Ref frictionTorques, Eigen::Ref tauExt, Eigen::Ref baseAcceleration, diff --git a/src/Estimators/src/SubModelKinDynWrapper.cpp b/src/Estimators/src/SubModelKinDynWrapper.cpp index d7e067124d..82a9d1bd58 100644 --- a/src/Estimators/src/SubModelKinDynWrapper.cpp +++ b/src/Estimators/src/SubModelKinDynWrapper.cpp @@ -44,6 +44,12 @@ bool RDE::SubModelKinDynWrapper::initialize(const RDE::SubModel& subModel) return false; } + if (!m_kinDyn.setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)) + { + blf::log()->error("{} Unable to set the frame velocity representation.", logPrefix); + return false; + } + m_numOfJoints = m_subModel.getModel().getNrOfDOFs(); m_baseFrame = m_kinDyn.getFloatingBase(); @@ -135,7 +141,7 @@ bool RDE::SubModelKinDynWrapper::updateKinDynState() m_qj, iDynTree::make_span(m_baseVelocity.data(), manif::SE3d::Tangent::DoF), - iDynTree::Span(m_dqj.data(), m_dqj.size()), + m_dqj, m_worldGravity)) { blf::log()->error("{} Unable to set the robot state.", logPrefix); @@ -239,7 +245,7 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState() return true; } -bool RDE::SubModelKinDynWrapper::inverseDynamics(Eigen::Ref motorTorqueAfterGearbox, +bool RDE::SubModelKinDynWrapper::forwardDynamics(Eigen::Ref motorTorqueAfterGearbox, Eigen::Ref frictionTorques, Eigen::Ref tauExt, Eigen::Ref baseAcceleration, @@ -249,7 +255,7 @@ bool RDE::SubModelKinDynWrapper::inverseDynamics(Eigen::Ref mot if (m_subModel.getModel().getNrOfDOFs() == 0) { - blf::log()->error("{} Inverse dynamics is not defined for sub-models with zero degrees of freedom.", + blf::log()->error("{} The forward dynamics is not defined for sub-models with zero degrees of freedom.", logPrefix); return false; } @@ -269,6 +275,7 @@ bool RDE::SubModelKinDynWrapper::inverseDynamics(Eigen::Ref mot m_pseudoInverseH = m_H.completeOrthogonalDecomposition().pseudoInverse(); m_FTBaseAcc = m_FTranspose * baseAcceleration; + m_totalTorques = motorTorqueAfterGearbox - frictionTorques - m_genBiasJointTorques + tauExt + m_FTBaseAcc; diff --git a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp index 42a9d69d67..72b8404b0d 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp @@ -15,48 +15,113 @@ #include #include #include +#include +#include // iDynTree #include #include +#include #include #include +using namespace BipedalLocomotion; using namespace BipedalLocomotion::ParametersHandler; namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; -bool setStaticState(std::shared_ptr kinDyn) +namespace Eigen +{ + using Vector6d = Eigen::Matrix; +} + +bool setStaticStateKinDyn(std::shared_ptr kinDyn) { size_t dofs = kinDyn->getNrOfDegreesOfFreedom(); - iDynTree::Transform worldTbase; + Eigen::VectorXd baseVel(6); Eigen::Vector3d gravity; - Eigen::VectorXd qj(dofs), dqj(dofs), ddqj(dofs); + Eigen::VectorXd qj(dofs), dqj(dofs); - worldTbase = iDynTree::Transform(iDynTree::Rotation::Identity(), iDynTree::Position::Zero()); + Eigen::Matrix4d transform; + transform.setIdentity(); - Eigen::Matrix4d transform = toEigen(worldTbase.asHomogeneousTransform()); + baseVel.setZero(); gravity.setZero(); - gravity(2) = -9.80665; - - baseVel.setZero(); + gravity(2) = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; for (size_t dof = 0; dof < dofs; dof++) { qj(dof) = 0.0; dqj(dof) = 0.0; - ddqj(dof) = 0.0; } - return kinDyn->setRobotState(transform, - iDynTree::make_span(qj.data(), qj.size()), - iDynTree::make_span(baseVel.data(), baseVel.size()), - iDynTree::make_span(dqj.data(), dqj.size()), - iDynTree::make_span(gravity.data(), gravity.size())); + return kinDyn->setRobotState(iDynTree::make_span(qj.data(), qj.size()), + iDynTree::make_span(dqj.data(), dqj.size()), + iDynTree::make_span(gravity.data(), gravity.size())); +} + +std::map getFTMeasurementFromStaticState(std::shared_ptr estimator) +{ + std::map ftMeasurement; + + iDynTree::JointPosDoubleArray qj(estimator->model()); + iDynTree::JointDOFsDoubleArray dqj(estimator->model()), ddqj(estimator->model()); + + qj.zero(); + dqj.zero(); + ddqj.zero(); + + int rootLinkIndex = estimator->model().getFrameIndex("root_link"); + + iDynTree::Vector3 gravityOnRootLink; + gravityOnRootLink.zero(); + gravityOnRootLink(2) = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + // The estimated FT sensor measurements + iDynTree::SensorsMeasurements expFTmeasurements(estimator->sensors()); + + // The estimated external wrenches + iDynTree::LinkContactWrenches estContactWrenches(estimator->model()); + + // The estimated joint torques + iDynTree::JointDOFsDoubleArray estJointTorques(estimator->model()); + + iDynTree::UnknownWrenchContact unknownWrench; + unknownWrench.unknownType = iDynTree::FULL_WRENCH; + unknownWrench.contactPoint.zero(); + + iDynTree::LinkUnknownWrenchContacts fullBodyUnknowns(estimator->model()); + fullBodyUnknowns.clear(); + + REQUIRE(fullBodyUnknowns.addNewContactInFrame(estimator->model(), rootLinkIndex, unknownWrench)); + + REQUIRE(estimator->updateKinematicsFromFixedBase(qj, dqj, ddqj, rootLinkIndex, gravityOnRootLink)); + + REQUIRE(estimator->computeExpectedFTSensorsMeasurements(fullBodyUnknowns, expFTmeasurements, estContactWrenches, estJointTorques)); + + iDynTree::Wrench estimatedSensorWrench; + estimatedSensorWrench.zero(); + + for (int index = 0; index < estimator->sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); index++) + { + REQUIRE(expFTmeasurements.getMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE, index, estimatedSensorWrench)); + ftMeasurement[estimator->sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, index)->getName()] = iDynTree::toEigen(estimatedSensorWrench); + } + + return ftMeasurement; +} + +std::shared_ptr createFTAndJointTorquesEstimator(iDynTree::ModelLoader& modelLoader) +{ + std::shared_ptr estimator = std::make_shared(); + + REQUIRE(estimator->setModelAndSensors(modelLoader.model(), modelLoader.sensors())); + + return estimator; } TEST_CASE("SubModelKinDynWrapper") @@ -103,6 +168,8 @@ TEST_CASE("SubModelKinDynWrapper") auto kinDyn = std::make_shared(); REQUIRE(kinDyn->loadRobotModel(mdlLdr.model())); + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)); + // List of joints and fts to load the model RDE::SubModelCreator subModelCreator; subModelCreator.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); @@ -112,6 +179,13 @@ TEST_CASE("SubModelKinDynWrapper") auto subModelList = subModelCreator.getSubModelList(); + auto totalMass = 0.0; + for (int idx = 0; idx < subModelList.size(); idx++) + { + totalMass += subModelList[idx].getModel().getTotalMass(); + } + REQUIRE(totalMass == kinDyn->model().getTotalMass()); + // Full model base acceleration manif::SE3d::Tangent robotBaseAcceleration; robotBaseAcceleration.setZero(); @@ -120,12 +194,29 @@ TEST_CASE("SubModelKinDynWrapper") Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); robotJointAcceleration.setZero(); - REQUIRE(setStaticState(kinDyn)); + REQUIRE(setStaticStateKinDyn(kinDyn)); + + std::shared_ptr estimator = createFTAndJointTorquesEstimator(mdlLdr); + + std::map ftMeasurement = getFTMeasurementFromStaticState(estimator); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + extWrench, + jointTorques); for (int idx = 0; idx < subModelList.size(); idx++) { RDE::SubModelKinDynWrapper kinDynSubModel; + REQUIRE(kinDynSubModel.setKinDyn(kinDyn)); + REQUIRE(kinDynSubModel.initialize(subModelList[idx])); REQUIRE(kinDynSubModel.updateKinDynState()); @@ -138,20 +229,25 @@ TEST_CASE("SubModelKinDynWrapper") Eigen::VectorXd frictionTorques(numberOfJoints); Eigen::VectorXd contactTorques(numberOfJoints); + motorTorques.setZero(); + frictionTorques.setZero(); + contactTorques.setZero(); + const std::string baseFrame = kinDynSubModel.getBaseFrameName(); manif::SE3d::Tangent baseAcceleration; REQUIRE(kinDynSubModel.getBaseAcceleration(robotBaseAcceleration, robotJointAcceleration, baseAcceleration)); - manif::SE3d::Tangent subModelBaseAcceleration; + + manif::SE3d::Tangent baseAccelerationFromFullModel; REQUIRE(kinDyn->getFrameAcc(baseFrame, iDynTree::make_span(robotBaseAcceleration.data(), manif::SE3d::Tangent::DoF), robotJointAcceleration, - iDynTree::make_span(subModelBaseAcceleration.data(), + iDynTree::make_span(baseAccelerationFromFullModel.data(), manif::SE3d::Tangent::DoF))); - REQUIRE(subModelBaseAcceleration == baseAcceleration); + REQUIRE(baseAccelerationFromFullModel == baseAcceleration); manif::SE3d::Tangent baseVelocity; baseVelocity = kinDynSubModel.getBaseVelocity(); @@ -160,21 +256,45 @@ TEST_CASE("SubModelKinDynWrapper") = BipedalLocomotion::Conversions::toManifTwist(kinDyn->getFrameVel(baseFrame)); REQUIRE(baseVelocity == baseVelFromFullModel); + for (auto const& [key, val] : ftMeasurement) + { + if (subModelList[idx].getModel().isFrameNameUsed(key)) + { + Eigen::MatrixXd jacobian = kinDynSubModel.getFTJacobian(key); + + Eigen::MatrixXd jac = jacobian.block(0, 6, 6, subModelList[idx].getModel().getNrOfDOFs()); + + for (int ftIndex = 0; ftIndex < subModelList[idx].getNrOfFTSensor(); ftIndex++) + { + if (subModelList[idx].getFTSensor(ftIndex).name == key) + { + contactTorques.noalias() += (int)subModelList[idx].getFTSensor(ftIndex).forceDirection * jac.transpose() * val; + } + } + } + } + Eigen::VectorXd jointAcceleration(numberOfJoints); - for (int i = 0; i < numberOfJoints; i++) + for (int jointIndex = 0; jointIndex < subModelList[idx].getJointMapping().size(); jointIndex++) { - motorTorques(i) = GENERATE(take(1, random(-50, 50))); - frictionTorques(i) = GENERATE(take(1, random(-30, 30))); - contactTorques(i) = GENERATE(take(1, random(-20, 20))); + motorTorques[jointIndex] = jointTorques.jointTorques()[subModelList[idx].getJointMapping()[jointIndex]]; } - REQUIRE(kinDynSubModel.inverseDynamics(motorTorques, + REQUIRE(kinDynSubModel.forwardDynamics(motorTorques, frictionTorques, contactTorques, baseAcceleration.coeffs(), jointAcceleration)); + Eigen::VectorXd zeroVector(numberOfJoints); + zeroVector.setZero(); + + for (int index = 0; index < jointAcceleration.size(); index++) + { + REQUIRE(std::abs(jointAcceleration[index]) < 0.1); + } + auto massMatrix = kinDynSubModel.getMassMatrix(); REQUIRE(massMatrix.rows() == (6 + numberOfJoints));