int main()
{
std::string icub_urdf_filename = "icub_model.urdf";
// Create the iCub model
KDL::Tree icub_tree;
bool ok = iDynTree::treeFromUrdfFile(icub_urdf_filename,icub_tree);
if( !ok )
{
std::cerr <<"Could not import icub urdf" << std::endl;
return EXIT_FAILURE;
}
// Create the iCub undirected tree, that contains also
// a default serialization (i.e. a mapping between links/joints
// and integers (if you want to provide a user-defined serialization
// to the undirected tree, pass it as a second argument to the constructor
KDL::CoDyCo::UndirectedTree icub_undirected_tree(icub_tree);
std::cout << "icub_tree serialization 1 : " << icub_undirected_tree.getSerialization().toString();
// Load a sensors tree (for ft sensors) from the information extracted from urdf file
// and using the serialization provided in the undirected tree
iDynTree::SensorsList sensors_tree = iDynTree::sensorsListFromURDF(icub_undirected_tree,icub_urdf_filename);
//Create a regressor generator
KDL::CoDyCo::Regressors::DynamicRegressorGenerator regressor_generator(icub_undirected_tree,sensors_tree);
//Add a set of rows of the regressor of right arm
std::vector<std::string> l_arm_subtree;
l_arm_subtree.push_back("l_upper_arm");
regressor_generator.addSubtreeRegressorRows(l_arm_subtree);
//Create a random state for the robot
KDL::Twist base_velocity, base_acceleration;
base_velocity = base_acceleration = KDL::Twist::Zero();
KDL::JntArray q(regressor_generator.getNrOfDOFs());
SetToZero(q);
srand(time(0));
for(int i=0; i < q.rows(); i++ )
{
q(i) = random_double();
}
double gravity_norm = 9.8;
base_acceleration.vel[2] = -gravity_norm;
regressor_generator.setRobotState(q,q,q,base_velocity,base_acceleration);
// Estimate sensor measurements from the model
iDynTree::Wrench simulate_measurement = simulateFTSensorFromKinematicState(icub_undirected_tree,
q,q,q,base_velocity,base_acceleration,"l_arm_ft_sensor",sensors_tree);
//Create a regressor and check the returned sensor value
Eigen::MatrixXd regressor(regressor_generator.getNrOfOutputs(),regressor_generator.getNrOfParameters());
Eigen::VectorXd kt(regressor_generator.getNrOfOutputs());
regressor_generator.computeRegressor(regressor,kt);
//std::cout << "regressors : " << regressor << std::endl;
Eigen::VectorXd parameters(regressor_generator.getNrOfParameters());
parameters.setZero();
regressor_generator.getModelParameters(parameters);
assert(parameters.rows() == regressor_generator.getNrOfParameters());
Eigen::Matrix<double,6,1> sens = regressor*parameters;
////////////////////////////////////////////////////////////
///// Test also the new iDynTree regressor infrastructure
////////////////////////////////////////////////////////////
iDynTree::Regressors::DynamicsRegressorGenerator new_generator;
// load robot and sensor model
ok = ok && new_generator.loadRobotAndSensorsModelFromFile(icub_urdf_filename);
assert(ok);
// load regressor structure (this should be actually loaded from file)
std::string regressor_structure
= "<regressor> "
" <subtreeBaseDynamics> "
" <FTSensorLink>l_upper_arm</FTSensorLink> "
" </subtreeBaseDynamics> "
"</regressor>";
ok = ok && new_generator.loadRegressorStructureFromString(regressor_structure);
assert(ok);
iDynTree::VectorDynSize q_idyntree(q.rows());
ok = ok && iDynTree::ToiDynTree(q,q_idyntree);
assert(ok);
iDynTree::Twist gravity;
gravity(2) = gravity_norm;
ok = ok && new_generator.setRobotState(q_idyntree,q_idyntree,q_idyntree,gravity);
assert(ok);
iDynTree::MatrixDynSize regr_idyntree(new_generator.getNrOfOutputs(),new_generator.getNrOfParameters());
iDynTree::VectorDynSize kt_idyntree(new_generator.getNrOfOutputs());
iDynTree::VectorDynSize param_idyntree(new_generator.getNrOfParameters());
ok = ok && new_generator.getModelParameters(param_idyntree);
int sensorIndex = new_generator.getSensorsModel().getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE,"l_arm_ft_sensor");
ok = ok && new_generator.getSensorsMeasurements().setMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,sensorIndex,simulate_measurement);
ok = ok && new_generator.computeRegressor(regr_idyntree,kt_idyntree);
Eigen::Matrix<double,6,1> sens_idyntree = Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(regr_idyntree.data(),regr_idyntree.rows(),regr_idyntree.cols())*
Eigen::Map<Eigen::VectorXd>(param_idyntree.data(),param_idyntree.size());
Eigen::Matrix<double,6,1> kt_eigen = Eigen::Map< Eigen::Matrix<double,6,1> >(kt_idyntree.data(),kt_idyntree.size());
std::cout << "Parameters norm old " << parameters.norm() << std::endl;
std::cout << "Parameters norm new " << Eigen::Map<Eigen::VectorXd>(param_idyntree.data(),param_idyntree.size()).norm() << std::endl;
std::cout << "Sensor measurement from regressor*model_parameters: " << sens << std::endl;
std::cout << "Sensor measurement from regressor*model_parameters (new): " << sens_idyntree << std::endl;
std::cout << "Sensor measurement from RNEA: " << KDL::CoDyCo::toEigen( iDynTree::ToKDL(simulate_measurement)) << std::endl;
std::cout << "Sensor measurement from known terms (new) " << kt_eigen << std::endl;
double tol = 1e-5;
if( (KDL::CoDyCo::toEigen(iDynTree::ToKDL(simulate_measurement))+sens).norm() > tol )
{
std::cerr << "[ERR] iCubLeftArmRegressor error" << std::endl;
return EXIT_FAILURE;
}
if( (KDL::CoDyCo::toEigen(iDynTree::ToKDL(simulate_measurement))+sens_idyntree).norm() > tol )
{
std::cerr << "[ERR] iCubLeftArmRegressor error: failure in new iDynTree regressor generator" << std::endl;
return EXIT_FAILURE;
}
if( (KDL::CoDyCo::toEigen(iDynTree::ToKDL(simulate_measurement))+kt_eigen).norm() > tol )
{
std::cerr << "[ERR] iCubLeftArmRegressor error: failure in new iDynTree regressor generator" << std::endl;
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
int main()
{
std::string urdf_to_test = getAbsModelPath("twoLinks.urdf");
srand(time(NULL));
//Creating an DynTree
iCub::iDynTree::DynTree urdf_dyntree;
bool ok = urdf_dyntree.loadURDFModel(urdf_to_test);
//Creating a DynamicsComputations class
iDynTree::HighLevel::DynamicsComputations urdf_dyncomp;
ok = ok && urdf_dyncomp.loadRobotModelFromFile(urdf_to_test);
if( !ok )
{
std::cerr << "Impossible to load URDF " << urdf_to_test << std::endl;
return EXIT_FAILURE;
}
// Create random joint configuration
unsigned int dofs = urdf_dyncomp.getNrOfDegreesOfFreedom();
iDynTree::VectorDynSize q_idyntree(dofs),zero_idyntree(dofs);
iDynTree::SpatialAcc gravity;
yarp::sig::Vector q_yarp(dofs);
for(unsigned int i=0; i < dofs; i++ )
{
q_yarp(i) = q_idyntree(i) = random_double();
}
urdf_dyntree.setAng(q_yarp);
urdf_dyncomp.setRobotState(q_idyntree,zero_idyntree,zero_idyntree,gravity);
//Check consistenct of all the transformations
unsigned int nrOfFrames = urdf_dyncomp.getNrOfFrames();
for(unsigned refFrame = 0; refFrame < nrOfFrames; refFrame++ )
{
for(unsigned frame = 0; frame < nrOfFrames; frame++ )
{
KDL::Frame H_kdl = urdf_dyntree.getPositionKDL(refFrame,(int)frame);
iDynTree::Transform H_transform = urdf_dyncomp.getRelativeTransform(refFrame,frame);
if( !checkFrameConsistency(H_kdl,H_transform) )
{
std::cerr << "Error : " << refFrame << "_T_" << frame << " is inconsistent " << std::endl;
std::cerr << "DynamicsComputations : " << H_transform.toString() << std::endl;
std::cerr << "DynTree " << H_kdl << std::endl;
return EXIT_FAILURE;
}
}
}
// The same, but with names
nrOfFrames = urdf_dyncomp.getNrOfFrames();
for(unsigned refFrame = 0; refFrame < nrOfFrames; refFrame++ )
{
for(unsigned frame = 0; frame < nrOfFrames; frame++ )
{
std::string refFrameName, frameName;
urdf_dyntree.getFrameName(refFrame,refFrameName);
urdf_dyntree.getFrameName(frame,frameName);
KDL::Frame H_kdl = urdf_dyntree.getPositionKDL(refFrame,(int)frame);
iDynTree::Transform H_transform = urdf_dyncomp.getRelativeTransform(refFrameName,frameName);
if( !checkFrameConsistency(H_kdl,H_transform) )
{
std::cerr << "Error : " << refFrameName << "_T_" << frameName << " is inconsistent " << std::endl;
std::cerr << "DynamicsComputations : " << H_transform.toString() << std::endl;
std::cerr << "DynTree " << H_kdl << std::endl;
return EXIT_FAILURE;
}
if( refFrame != frame )
{
std::cerr << "Check: " << refFrameName << "_T_" << frameName << " is inconsistent " << std::endl;
std::cerr << "DynamicsComputations : " << H_transform.toString() << std::endl;
std::cerr << "DynTree " << H_kdl << std::endl;
std::cerr << "Rotation " << std::endl;
std::cerr << "DynamicsComputations rot: " << H_transform.getRotation().toString() << std::endl;
std::cerr << "DynTree rot " << H_kdl.M << std::endl;
for(int row = 0; row <3; row++ )
{
for(int col = 0; col < 3; col++ )
{
}
}
}
}
}
return EXIT_SUCCESS;
}
