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