void testMassMatrixConsistency(iDynTree::HighLevel::DynamicsComputations & dynComp,
iCub::iDynTree::DynTree & dynTree,
iDynTree::KinDynComputations & kinDynComp)
{
size_t dofs = dynComp.getNrOfDegreesOfFreedom();
yarp::sig::Matrix dynTreeMassMatrixYarp(6+dofs,6+dofs);
MatrixDynSize dynTreeMassMatrix(6+dofs,6+dofs);
MatrixDynSize dynCompMassMatrix(6+dofs,6+dofs);
FreeFloatingMassMatrix kinDynMassMatrix(kinDynComp.getRobotModel());
// iCub::iDynTree::DynTree
bool ok = dynTree.getFloatingBaseMassMatrix(dynTreeMassMatrixYarp);
ASSERT_IS_TRUE(ok);
ok = toiDynTree(dynTreeMassMatrixYarp,dynTreeMassMatrix);
ASSERT_IS_TRUE(ok);
// iDynTree::HighLevel::DynamicsComputations
ok = dynComp.getFreeFloatingMassMatrix(dynCompMassMatrix);
ASSERT_IS_TRUE(ok);
// iDynTree::KinDynComputations
ok = kinDynComp.getFreeFloatingMassMatrix(kinDynMassMatrix);
ASSERT_IS_TRUE(ok);
// Check all matrices are equal
ASSERT_EQUAL_MATRIX(dynTreeMassMatrix,dynCompMassMatrix);
ASSERT_EQUAL_MATRIX(dynCompMassMatrix,kinDynMassMatrix);
}
void testInverseDynamicsConsistency(iDynTree::HighLevel::DynamicsComputations & dynComp,
iDynTree::KinDynComputations & kinDynComp,
const iDynTree::Vector6& baseAccKinDyn,
const iDynTree::JointDOFsDoubleArray& jointAccKinDyn)
{
size_t dofs = dynComp.getNrOfDegreesOfFreedom();
Wrench baseWrenchDynComp;
VectorDynSize jntTorquesDynComp(dofs);
FreeFloatingGeneralizedTorques generalizedTorquesKinDyn(kinDynComp.model());
bool ok = dynComp.inverseDynamics(jntTorquesDynComp,baseWrenchDynComp);
LinkNetExternalWrenches linkExtWrenches(kinDynComp.model());
for(int l=0; l < kinDynComp.model().getNrOfLinks(); l++)
{
linkExtWrenches(l).zero();
}
ASSERT_IS_TRUE(ok);
ok = kinDynComp.inverseDynamics(baseAccKinDyn,jointAccKinDyn,linkExtWrenches,generalizedTorquesKinDyn);
ASSERT_EQUAL_SPATIAL_FORCE(baseWrenchDynComp,generalizedTorquesKinDyn.baseWrench());
ASSERT_EQUAL_VECTOR(jntTorquesDynComp,generalizedTorquesKinDyn.jointTorques());
}
void testRegressorConsistency(iDynTree::HighLevel::DynamicsComputations & dynComp,
iCub::iDynTree::DynTree & dynTree)
{
size_t dofs = dynComp.getNrOfDegreesOfFreedom();
size_t nrOfLinks = dynComp.getNrOfLinks();
// check regressor matrix
MatrixDynSize dynTreeRegressor(6+dofs,10*nrOfLinks),dynCompRegressor(6+dofs,10*nrOfLinks);
yarp::sig::Matrix dynTreeRegressorYarp(6+dofs,10*nrOfLinks);
dynTree.kinematicRNEA();
dynTree.getDynamicsRegressor(dynTreeRegressorYarp);
yarp2idyntree(dynTreeRegressorYarp,dynTreeRegressor);
dynComp.getDynamicsRegressor(dynCompRegressor);
ASSERT_EQUAL_MATRIX(dynTreeRegressor,dynCompRegressor);
// check parameter vector
VectorDynSize dynTreeParams(10*nrOfLinks),dynCompParams(10*nrOfLinks);
yarp::sig::Vector dynTreeParamsYarp(10*nrOfLinks);
dynTree.getDynamicsParameters(dynTreeParamsYarp);
yarp2idyntree(dynTreeParamsYarp,dynTreeParams);
dynComp.getModelDynamicsParameters(dynCompParams);
ASSERT_EQUAL_VECTOR(dynTreeParams,dynCompParams);
}
void testJacobianConsistency(iDynTree::HighLevel::DynamicsComputations & dynComp,
iCub::iDynTree::DynTree & dynTree,
iDynTree::KinDynComputations & kinDynComp)
{
size_t dofs = dynComp.getNrOfDegreesOfFreedom();
MatrixDynSize dynTreeJacobian(6,6+dofs),dynCompJacobian(6,6+dofs);
yarp::sig::Matrix dynTreeJacobianYarp(6,6+dofs);
FrameFreeFloatingJacobian kinDynCompJacobian(kinDynComp.getRobotModel());
for(size_t frame=0; frame < dynComp.getNrOfFrames(); frame++ )
{
dynTree.getJacobian(frame,dynTreeJacobianYarp);
yarp2idyntree(dynTreeJacobianYarp,dynTreeJacobian);
dynComp.getFrameJacobian(frame,dynCompJacobian);
std::string frameName = dynComp.getFrameName(frame);
ASSERT_EQUAL_MATRIX(dynTreeJacobian,dynCompJacobian);
std::cerr << "Testing frame " << frameName << std::endl;
bool ok = kinDynComp.getFrameFreeFloatingJacobian(frameName,kinDynCompJacobian);
std::cerr << "DynamicsComputations: " << std::endl;
std::cerr << dynCompJacobian.toString() << std::endl;
std::cerr << "KinamicsDynamicsComputations : " << std::endl;
std::cerr << kinDynCompJacobian.toString() << std::endl;
ASSERT_IS_TRUE(ok);
ASSERT_EQUAL_MATRIX(kinDynCompJacobian,dynCompJacobian);
}
}
void testJacobianConsistency(iDynTree::HighLevel::DynamicsComputations & dynComp,
iCub::iDynTree::DynTree & dynTree)
{
size_t dofs = dynComp.getNrOfDegreesOfFreedom();
MatrixDynSize dynTreeJacobian(6,6+dofs),dynCompJacobian(6,6+dofs);
yarp::sig::Matrix dynTreeJacobianYarp(6,6+dofs);
for(size_t frame=0; frame < dynComp.getNrOfFrames(); frame++ )
{
dynTree.getJacobian(frame,dynTreeJacobianYarp);
yarp2idyntree(dynTreeJacobianYarp,dynTreeJacobian);
dynComp.getFrameJacobian(frame,dynCompJacobian);
ASSERT_EQUAL_MATRIX(dynTreeJacobian,dynCompJacobian);
}
}
void setRandomState(iDynTree::HighLevel::DynamicsComputations & dynComp,
iCub::iDynTree::DynTree & dynTree)
{
size_t dofs = dynComp.getNrOfDegreesOfFreedom();
Transform worldTbase;
Twist baseVel;
ClassicalAcc baseAcc;
SpatialAcc gravity;
Vector6 properAcc;
iDynTree::VectorDynSize qj(dofs), dqj(dofs), ddqj(dofs);
worldTbase = iDynTree::Transform(Rotation::RPY(actual_random_double(),random_double(),random_double()),
Position(random_double(),random_double(),random_double()));
for(int i=0; i < 3; i++)
{
gravity(i) = random_double();
}
gravity(2) = 10.0;
for(int i=0; i < 6; i++)
{
baseVel(i) = random_double();
baseAcc(i) = random_double();
properAcc(i) = baseAcc(i) - gravity(i);
}
for(size_t dof=0; dof < dofs; dof++)
{
qj(dof) = random_double();
dqj(dof) = random_double();
ddqj(dof) = random_double();
}
bool ok = dynComp.setRobotState(qj,dqj,ddqj,worldTbase,baseVel,baseAcc,gravity);
dynTree.setAng(idyntree2yarp(qj));
dynTree.setDAng(idyntree2yarp(dqj));
dynTree.setD2Ang(idyntree2yarp(ddqj));
dynTree.setWorldBasePose(idyntreeMat2yarp(worldTbase.asHomogeneousTransform()));
dynTree.setKinematicBaseVelAcc(idyntree2yarp(baseVel),idyntree2yarp(properAcc));
ASSERT_EQUAL_DOUBLE(ok,true);
}
void testCOMConsistency(iDynTree::HighLevel::DynamicsComputations & dynComp,
iCub::iDynTree::DynTree & dynTree,
iDynTree::KinDynComputations & kinDynComp)
{
size_t dofs = dynComp.getNrOfDegreesOfFreedom();
// check COM position
iDynTree::Position COMnew, COMold, COMKinDyn;
yarp::sig::Vector COMYarp;
COMYarp = dynTree.getCOM();
toiDynTree(COMYarp,COMold);
COMnew = dynComp.getCenterOfMass();
COMKinDyn = kinDynComp.getCenterOfMassPosition();
ASSERT_EQUAL_VECTOR(COMnew,COMold);
ASSERT_EQUAL_VECTOR(COMnew,COMKinDyn);
// check COM Jacobian
iDynTree::MatrixDynSize jacNew(3,6+dofs), jacOld(3,6+dofs), jacKinDyn(3,6+dofs);
yarp::sig::Matrix jacYARP(6,6+dofs);
bool ok = dynTree.getCOMJacobian(jacYARP);
ASSERT_IS_TRUE(ok);
iDynTree::toEigen(jacOld) = iDynTree::toEigen(jacYARP).topRows<3>();
ok = dynComp.getCenterOfMassJacobian(jacNew);
ASSERT_IS_TRUE(ok);
ASSERT_EQUAL_MATRIX(jacNew,jacOld);
ok = kinDynComp.getCenterOfMassJacobian(jacKinDyn);
ASSERT_IS_TRUE(ok);
ASSERT_EQUAL_MATRIX(jacNew,jacKinDyn);
}
void testTransformsConsistency(iDynTree::HighLevel::DynamicsComputations & dynComp,
iCub::iDynTree::DynTree & dynTree)
{
for(size_t frame=0; frame < dynComp.getNrOfFrames(); frame++ )
{
std::string frameNameDynComp;
std::string frameNameDynTree;
dynTree.getFrameName(frame,frameNameDynTree);
frameNameDynComp = dynComp.getFrameName(frame);
ASSERT_EQUAL_STRING(frameNameDynComp,frameNameDynTree);
// todo assert equal string
Transform dynTreeTransform = yarpTransform2idyntree(dynTree.getPosition(frame));
Transform dynCompTransform = dynComp.getWorldTransform(frame);
ASSERT_EQUAL_TRANSFORM(dynCompTransform,dynTreeTransform);
}
}
void setRandomState(iDynTree::HighLevel::DynamicsComputations & dynComp,
iCub::iDynTree::DynTree & dynTree,
iDynTree::KinDynComputations& kinDynComp,
iDynTree::Vector6& baseAccKinDyn,
iDynTree::JointDOFsDoubleArray& jointAccKinDyn)
{
std::cerr << " setRandomState" << std::endl;
size_t dofs = dynComp.getNrOfDegreesOfFreedom();
Transform worldTbase;
Twist baseVel;
ClassicalAcc baseAcc;
SpatialAcc gravity;
Vector6 properAcc;
iDynTree::VectorDynSize qj(dofs), dqj(dofs), ddqj(dofs);
worldTbase = iDynTree::Transform(Rotation::RPY(random_double(),random_double(),random_double()),
Position(random_double(),random_double(),random_double()));
for(int i=0; i < 3; i++)
{
gravity(i) = random_double();
}
gravity(2) = 10.0;
for(int i=0; i < 6; i++)
{
baseVel(i) = random_double();
baseAcc(i) = random_double();
properAcc(i) = baseAcc(i) - gravity(i);
}
jointAccKinDyn.resize(dofs);
for(size_t dof=0; dof < dofs; dof++)
{
qj(dof) = random_double();
dqj(dof) = random_double();
ddqj(dof) = random_double();
}
bool ok = dynComp.setRobotState(qj,dqj,ddqj,worldTbase,baseVel,baseAcc,gravity);
ASSERT_IS_TRUE(ok);
dynTree.setAng(idyntree2yarp(qj));
dynTree.setDAng(idyntree2yarp(dqj));
dynTree.setD2Ang(idyntree2yarp(ddqj));
dynTree.setWorldBasePose(idyntreeMat2yarp(worldTbase.asHomogeneousTransform()));
dynTree.setKinematicBaseVelAcc(idyntree2yarp(baseVel),idyntree2yarp(properAcc));
std::cerr << " Setting state in kinDyn" << std::endl;
ASSERT_EQUAL_DOUBLE(qj.size(),kinDynComp.getRobotModel().getNrOfPosCoords());
ASSERT_EQUAL_DOUBLE(dqj.size(),kinDynComp.getRobotModel().getNrOfDOFs());
Vector3 grav3d = gravity.getLinearVec3();
ok = kinDynComp.setRobotState(worldTbase,qj,baseVel,dqj,grav3d);
toEigen(jointAccKinDyn) = toEigen(ddqj);
toEigen(baseAccKinDyn) = toEigen(baseAcc);
ASSERT_IS_TRUE(ok);
std::cerr << "state setted" << std::endl;
}