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