From ce7d3d61b011ffc645879a8e0bf148331e77ad0a Mon Sep 17 00:00:00 2001 From: Ines Date: Tue, 31 Oct 2023 17:42:39 +0100 Subject: [PATCH] Add KinDynWrapper test --- src/Estimators/src/KinDynWrapper.cpp | 2 +- .../RobotDynamicsEstimator/CMakeLists.txt | 7 + .../KinDynWrapperTest.cpp | 232 ++++++++++++++++++ 3 files changed, 240 insertions(+), 1 deletion(-) create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp diff --git a/src/Estimators/src/KinDynWrapper.cpp b/src/Estimators/src/KinDynWrapper.cpp index 10ba655ca6..f6c36ef4e9 100644 --- a/src/Estimators/src/KinDynWrapper.cpp +++ b/src/Estimators/src/KinDynWrapper.cpp @@ -83,7 +83,7 @@ bool KinDynWrapper::forwardDynamics(Eigen::Ref motorTorqu return false; } - m_FTvBaseDot = m_massMatrix.block(6, 0, this->getNrOfDegreesOfFreedom(), 6) * baseAcceleration.coeffs(); + m_FTvBaseDot = m_massMatrix.bottomLeftCorner(this->getNrOfDegreesOfFreedom(), 6) * baseAcceleration.coeffs(); m_totalJointTorques = motorTorqueAfterGearbox - frictionTorques + tauExt - m_generalizedBiasForces.tail(this->getNrOfDegreesOfFreedom()) diff --git a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt index e5c21770d6..0cb30d7cc6 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt +++ b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt @@ -11,6 +11,13 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) BipedalLocomotion::ParametersHandlerYarpImplementation icub-models::icub-models) + add_bipedal_test(NAME KinDynWrapper + SOURCES KinDynWrapperTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + icub-models::icub-models) + add_bipedal_test(NAME ZeroVelocityStateDynamics SOURCES ZeroVelocityStateDynamicsTest.cpp LINKS BipedalLocomotion::RobotDynamicsEstimator diff --git a/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp new file mode 100644 index 0000000000..ab92548a09 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp @@ -0,0 +1,232 @@ +/** + * @file KinDynWrapperTest.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +void createModelLoader(IParametersHandler::shared_ptr group, iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +void createSubModels(iDynTree::ModelLoader& mdlLdr, + std::shared_ptr kinDynFullModel, + IParametersHandler::shared_ptr group, + SubModelCreator& subModelCreator) +{ + subModelCreator.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDynFullModel)); + + REQUIRE(subModelCreator.createSubModels(group)); +} + +bool setStaticState(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); + + worldTbase = iDynTree::Transform(iDynTree::Rotation::Identity(), iDynTree::Position::Zero()); + + Eigen::Matrix4d transform = toEigen(worldTbase.asHomogeneousTransform()); + + gravity.setZero(); + gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + baseVel.setZero(); + + 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())); +} + +IParametersHandler::shared_ptr createModelParameterHandler() +{ + // Create model variable handler to load the robot model + auto modelParamHandler = std::make_shared(); + + const std::vector jointList + = {"r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}; + + auto emptyGroupNamesFrames = std::make_shared(); + std::vector emptyVectorString; + emptyGroupNamesFrames->setParameter("names", emptyVectorString); + emptyGroupNamesFrames->setParameter("frames", emptyVectorString); + emptyGroupNamesFrames->setParameter("associated_joints", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("FT", emptyGroupNamesFrames)); + REQUIRE(modelParamHandler->setGroup("GYROSCOPE", emptyGroupNamesFrames)); + REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", emptyGroupNamesFrames)); + auto emptyGroupFrames = std::make_shared(); + emptyGroupFrames->setParameter("frames", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("EXTERNAL_CONTACT", emptyGroupFrames)); + + modelParamHandler->setParameter("joint_list", jointList); + + return modelParamHandler; +} + +TEST_CASE("KinDynWrapper Test") +{ + // Create parameter handler to load the model + auto modelParamHandler = createModelParameterHandler(); + + // Load model + iDynTree::ModelLoader modelLoader; + createModelLoader(modelParamHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)); + + REQUIRE(setStaticState(kinDyn)); + + SubModelCreator subModelCreator; + createSubModels(modelLoader, kinDyn, modelParamHandler, subModelCreator); + + std::vector> kinDynWrapperList; + std::vector subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList[idx]->setModel(subModelList[idx])); + } + + // Test kindynwrapper + Eigen::Vector3d gravity; + gravity.setZero(); + gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + int numJoints = 6; + + Eigen::VectorXd jointPos = Eigen::VectorXd::Random(numJoints); + Eigen::VectorXd jointVel = Eigen::VectorXd::Random(numJoints); + Eigen::VectorXd jointAcc = Eigen::VectorXd::Random(numJoints); + + manif::SE3d basePose + = BipedalLocomotion::Conversions::toManifPose(iDynTree::getRandomTransform()); + + manif::SE3Tangentd baseVel + = BipedalLocomotion::Conversions::toManifTwist(iDynTree::getRandomTwist()); + + manif::SE3Tangentd baseAcc + = BipedalLocomotion::Conversions::toManifTwist(iDynTree::getRandomTwist()); + + REQUIRE(kinDyn->setRobotState(basePose.transform(), + jointPos, + iDynTree::make_span(baseVel.data(), + manif::SE3d::Tangent::DoF), + jointVel, + gravity)); + + // Compute joint torques from inverse dynamics on the full model + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + kinDyn->inverseDynamics(iDynTree::make_span(baseAcc.data(), + manif::SE3d::Tangent::DoF), + jointAcc, + extWrench, + jointTorques); + + Eigen::VectorXd jointTrq = iDynTree::toEigen(jointTorques.jointTorques()); + + // The submodel is only one and it is the full model + // Indeed the model does not have ft sensors in this test + // Get sub-model base pose + manif::SE3d worldTBase = BipedalLocomotion::Conversions::toManifPose( + kinDyn->getWorldTransform(kinDynWrapperList[0]->getFloatingBase())); + + // Set the sub-model state + kinDynWrapperList[0]->setRobotState(worldTBase.transform(), + jointPos, + iDynTree::make_span(baseVel.data(), + manif::SE3d::Tangent::DoF), + jointVel, + gravity); + + // Forward dynamics + Eigen::VectorXd jointAccFD = Eigen::VectorXd(numJoints); + REQUIRE(kinDynWrapperList[0]->forwardDynamics(jointTrq, + Eigen::VectorXd::Zero(numJoints), + Eigen::VectorXd::Zero(numJoints), + baseAcc, + jointAccFD)); + + constexpr double tolerance = 1e-2; + REQUIRE(jointAcc.isApprox(jointAccFD, tolerance)); + + // Compute wrench at the base + Eigen::VectorXd trqExt = Eigen::VectorXd::Zero(6+numJoints); + Eigen::VectorXd genTrq = Eigen::VectorXd::Zero(6+numJoints); + REQUIRE(kinDynWrapperList[0]->generalizedBiasForces(genTrq)); + Eigen::MatrixXd massMatrix = Eigen::MatrixXd::Zero(6+numJoints, 6+numJoints); + REQUIRE(kinDynWrapperList[0]->getFreeFloatingMassMatrix(massMatrix)); + + Eigen::VectorXd tempVec = massMatrix.topLeftCorner(6,6) * baseAcc.coeffs(); + tempVec += massMatrix.topRightCorner(6,numJoints) * jointAcc; + trqExt.head(6) = tempVec + genTrq.head(6); + + Eigen::VectorXd nuDot = Eigen::VectorXd(6+numJoints); + REQUIRE(kinDynWrapperList[0]->forwardDynamics(jointTrq, + Eigen::VectorXd::Zero(numJoints), + trqExt, + nuDot)); + + REQUIRE(nuDot.head(6).isApprox(baseAcc.coeffs(), tolerance)); + REQUIRE(nuDot.tail(numJoints).isApprox(jointAcc, tolerance)); +}