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 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;
}