void checkInvariance(const Transform & trans, const SpatialAcc & acc1, const SpatialAcc & acc2)
{
SpatialAcc sumAccTrans = trans*(acc1+acc2);
SpatialAcc sumAccTransCheck = trans*acc1 + trans*acc2;
ASSERT_EQUAL_VECTOR(sumAccTrans.asVector(),sumAccTransCheck.asVector());
}
void checkInversion()
{
SpatialInertia I = getRandomInertia();
Transform trans = getRandomTransform();
ArticulatedBodyInertia Ia = I;
Wrench f(LinearForceVector3(1.0,5.0,6.0), AngularForceVector3(3.0,6.0,-4.0));
SpatialAcc a = Ia.applyInverse(f);
SpatialAcc aTrans = trans*a;
SpatialAcc aTransCheck = (trans*Ia).applyInverse(trans*f);
ASSERT_EQUAL_VECTOR(aTrans.asVector(),aTransCheck.asVector());
}
SpatialAcc Axis::getRotationSpatialAcc(const double d2theta) const
{
SpatialAcc ret;
Eigen::Map<Eigen::Vector3d> lin(ret.getLinearVec3().data());
Eigen::Map<Eigen::Vector3d> ang(ret.getAngularVec3().data());
Eigen::Map<const Eigen::Vector3d> dir(direction.data());
Eigen::Map<const Eigen::Vector3d> orig(origin.data());
lin = d2theta*(orig.cross(dir));
ang = dir*d2theta;
return ret;
}
void checkInvariance(const Transform & trans, const ArticulatedBodyInertia & inertia, const SpatialAcc & twist)
{
Transform invTrans = trans.inverse();
Wrench momentumTranslated = trans*(inertia*twist);
Wrench momentumTranslatedCheck = (trans*inertia)*(trans*twist);
SpatialAcc twistTranslated = trans*twist;
ArticulatedBodyInertia inertiaTranslated = trans*inertia;
Vector6 momentumTranslatedCheck2;
Vector6 momentumTranslatedCheck3;
Vector6 twistTranslatedCheck;
Matrix6x6 transAdjWrench = trans.asAdjointTransformWrench();
Matrix6x6 inertiaRaw = inertia.asMatrix();
Matrix6x6 transInvAdj = trans.inverse().asAdjointTransform();
Matrix6x6 transAdj = trans.asAdjointTransform();
Matrix6x6 inertiaTranslatedCheck;
Vector6 twistPlain = twist.asVector();
toEigen(momentumTranslatedCheck2) = toEigen(transAdjWrench)*
toEigen(inertiaRaw)*
toEigen(twistPlain);
toEigen(momentumTranslatedCheck3) = toEigen(transAdjWrench)*
toEigen(inertiaRaw)*
toEigen(transInvAdj)*
toEigen(transAdj)*
toEigen(twistPlain);
toEigen(twistTranslatedCheck) = toEigen(transAdj)*
toEigen(twistPlain);
toEigen(inertiaTranslatedCheck) = toEigen(transAdjWrench)*
toEigen(inertiaRaw)*
toEigen(transInvAdj);
ASSERT_EQUAL_MATRIX(inertiaTranslatedCheck,inertiaTranslated.asMatrix());
ASSERT_EQUAL_VECTOR(twistTranslated.asVector(),twistTranslatedCheck);
ASSERT_EQUAL_VECTOR(momentumTranslated.asVector(),momentumTranslatedCheck3);
ASSERT_EQUAL_VECTOR(momentumTranslated.asVector(),momentumTranslatedCheck2);
ASSERT_EQUAL_VECTOR(momentumTranslated.asVector(),momentumTranslatedCheck.asVector());
Wrench momentum = invTrans*momentumTranslated;
Wrench momentumCheck = (invTrans*(trans*inertia))*(invTrans*(trans*twist));
ASSERT_EQUAL_VECTOR(momentum.asVector(),momentumCheck.asVector());
}
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;
}
