Skip to content

Commit

Permalink
Implement a more robust test of SubModelKinDynWrapper class.
Browse files Browse the repository at this point in the history
  • Loading branch information
@isorrentino
isorrentino committed Mar 22, 2023
1 parent 084fe1a commit 7588848
Show file tree
Hide file tree
Showing 3 changed files with 155 additions and 28 deletions.
4 changes: 2 additions & 2 deletions src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -117,7 +117,7 @@ class SubModelKinDynWrapper
* @param baseAcceleration the sub-model base acceleration.
* @return a boolean value
*/
bool inverseDynamics(Eigen::Ref<Eigen::VectorXd> motorTorqueAfterGearbox,
bool forwardDynamics(Eigen::Ref<Eigen::VectorXd> motorTorqueAfterGearbox,
Eigen::Ref<Eigen::VectorXd> frictionTorques,
Eigen::Ref<Eigen::VectorXd> tauExt,
Eigen::Ref<Eigen::VectorXd> baseAcceleration,
Expand Down
13 changes: 10 additions & 3 deletions src/Estimators/src/SubModelKinDynWrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -135,7 +141,7 @@ bool RDE::SubModelKinDynWrapper::updateKinDynState()
m_qj,
iDynTree::make_span(m_baseVelocity.data(),
manif::SE3d::Tangent::DoF),
iDynTree::Span<double>(m_dqj.data(), m_dqj.size()),
m_dqj,
m_worldGravity))
{
blf::log()->error("{} Unable to set the robot state.", logPrefix);
Expand Down Expand Up @@ -239,7 +245,7 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState()
return true;
}

bool RDE::SubModelKinDynWrapper::inverseDynamics(Eigen::Ref<Eigen::VectorXd> motorTorqueAfterGearbox,
bool RDE::SubModelKinDynWrapper::forwardDynamics(Eigen::Ref<Eigen::VectorXd> motorTorqueAfterGearbox,
Eigen::Ref<Eigen::VectorXd> frictionTorques,
Eigen::Ref<Eigen::VectorXd> tauExt,
Eigen::Ref<Eigen::VectorXd> baseAcceleration,
Expand All @@ -249,7 +255,7 @@ bool RDE::SubModelKinDynWrapper::inverseDynamics(Eigen::Ref<Eigen::VectorXd> 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;
}
Expand All @@ -269,6 +275,7 @@ bool RDE::SubModelKinDynWrapper::inverseDynamics(Eigen::Ref<Eigen::VectorXd> mot
m_pseudoInverseH = m_H.completeOrthogonalDecomposition().pseudoInverse();

m_FTBaseAcc = m_FTranspose * baseAcceleration;

m_totalTorques
= motorTorqueAfterGearbox - frictionTorques - m_genBiasJointTorques + tauExt + m_FTBaseAcc;

Expand Down
166 changes: 143 additions & 23 deletions src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,48 +15,113 @@
#include <BipedalLocomotion/Conversions/ManifConversions.h>
#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
#include <BipedalLocomotion/ParametersHandler/YarpImplementation.h>
#include <BipedalLocomotion/Math/Constants.h>
#include <BipedalLocomotion/Math/Wrench.h>

// iDynTree
#include <iDynTree/KinDynComputations.h>
#include <iDynTree/ModelIO/ModelLoader.h>
#include <iDynTree/Estimation/ExtWrenchesAndJointTorquesEstimator.h>

#include <BipedalLocomotion/RobotDynamicsEstimator/SubModel.h>
#include <BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h>

using namespace BipedalLocomotion;
using namespace BipedalLocomotion::ParametersHandler;
namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator;

bool setStaticState(std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
namespace Eigen
{
using Vector6d = Eigen::Matrix<double, 6, 1>;
}

bool setStaticStateKinDyn(std::shared_ptr<iDynTree::KinDynComputations> 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<std::string, Eigen::VectorXd> getFTMeasurementFromStaticState(std::shared_ptr<iDynTree::ExtWrenchesAndJointTorquesEstimator> estimator)
{
std::map<std::string, Eigen::VectorXd> 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<iDynTree::ExtWrenchesAndJointTorquesEstimator> createFTAndJointTorquesEstimator(iDynTree::ModelLoader& modelLoader)
{
std::shared_ptr<iDynTree::ExtWrenchesAndJointTorquesEstimator> estimator = std::make_shared<iDynTree::ExtWrenchesAndJointTorquesEstimator>();

REQUIRE(estimator->setModelAndSensors(modelLoader.model(), modelLoader.sensors()));

return estimator;
}

TEST_CASE("SubModelKinDynWrapper")
Expand Down Expand Up @@ -103,6 +168,8 @@ TEST_CASE("SubModelKinDynWrapper")
auto kinDyn = std::make_shared<iDynTree::KinDynComputations>();
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());
Expand All @@ -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();
Expand All @@ -120,12 +194,29 @@ TEST_CASE("SubModelKinDynWrapper")
Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs());
robotJointAcceleration.setZero();

REQUIRE(setStaticState(kinDyn));
REQUIRE(setStaticStateKinDyn(kinDyn));

std::shared_ptr<iDynTree::ExtWrenchesAndJointTorquesEstimator> estimator = createFTAndJointTorquesEstimator(mdlLdr);

std::map<std::string, Eigen::VectorXd> 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());
Expand All @@ -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();
Expand All @@ -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));

Expand Down

0 comments on commit 7588848

Please sign in to comment.