iDynTree::SensorsList generateSensorsTree(const KDL::CoDyCo::UndirectedTree & undirected_tree, const std::vector<std::string> & ft_names, const std::vector<bool> & is_measure_direction_child_to_parent) { iDynTree::SensorsList sensors_tree; for(size_t i=0; i < ft_names.size(); i++ ) { //Creating a new ft sensor to be added in the ft sensors structure iDynTree::SixAxisForceTorqueSensor new_sens; if( undirected_tree.getJunction(ft_names[i]) != undirected_tree.getInvalidJunctionIterator() ) { //Set the sensor name (for the time being equal to the junction name) new_sens.setName(ft_names[i]); //Set the junction name new_sens.setParentJoint(ft_names[i]); int junction_index = undirected_tree.getJunction(ft_names[i])->getJunctionIndex(); new_sens.setParentJointIndex(junction_index); KDL::CoDyCo::JunctionMap::const_iterator junct_it = undirected_tree.getJunction(ft_names[i]); int parent_index = junct_it->getParentLink()->getLinkIndex(); int child_index = junct_it->getChildLink()->getLinkIndex(); std::string parent_link_name = junct_it->getParentLink()->getName(); std::string child_link_name = junct_it->getChildLink()->getName(); if( is_measure_direction_child_to_parent[i] ) { new_sens.setAppliedWrenchLink(parent_index); } else { new_sens.setAppliedWrenchLink(child_index); } // Currently we support only the case where the ft sensor frame is equal // to the child link frame new_sens.setSecondLinkSensorTransform(child_index,iDynTree::Transform::Identity()); new_sens.setSecondLinkName(child_link_name); // Then, the parent_link_H_sensor transform is simply parent_link_H_child_link transform KDL::Frame parent_link_H_sensor = junct_it->pose(0.0,false); new_sens.setFirstLinkSensorTransform(parent_index,iDynTree::ToiDynTree(parent_link_H_sensor)); new_sens.setFirstLinkName(parent_link_name); } else { std::cerr << "[ERR] DynTree::generateSensorsTree: problem generating sensor for ft " << ft_names[i] << std::endl; assert(false); } int ret = sensors_tree.addSensor(new_sens); assert(ret == i); } return sensors_tree; }
void assertConsistency(std::string modelName) { std::cerr << "DynamicsComputationsDynTreeConsistencyUnitTest running on model " << modelName << std::endl; iDynTree::HighLevel::DynamicsComputations dynComp; iCub::iDynTree::DynTree dynTree; bool ok = dynComp.loadRobotModelFromFile(modelName); ok = ok && dynTree.loadURDFModel(modelName); ASSERT_IS_TRUE(ok); ASSERT_EQUAL_DOUBLE(dynTree.getNrOfDOFs(),dynComp.getNrOfDegreesOfFreedom()); ASSERT_EQUAL_DOUBLE(dynTree.getNrOfFrames(),dynComp.getNrOfFrames()); // Load model with the same joint ordering of the DynTree iDynTree::ModelLoader mdlLoader; ok = mdlLoader.loadModelFromFile(modelName); ASSERT_IS_TRUE(ok); std::cerr << "Loaded model " << std::endl; std::vector<std::string> consideredJoints; KDL::CoDyCo::UndirectedTree undirectedTree = dynTree.getKDLUndirectedTree(); for(int i=0; i < undirectedTree.getNrOfJunctions(); i++) { std::string jointName = undirectedTree.getJunction(i)->getName(); // If the joint belong to the new model, include it in the considered joints if( mdlLoader.model().isJointNameUsed(jointName) ) { consideredJoints.push_back(jointName); } } iDynTree::ModelLoader mdlLoaderReduced; ok = mdlLoaderReduced.loadReducedModelFromFullModel(mdlLoader.model(),consideredJoints); ASSERT_IS_TRUE(ok); iDynTree::KinDynComputations kinDynComp; ok = kinDynComp.loadRobotModel(mdlLoaderReduced.model()); ASSERT_IS_TRUE(ok); std::cerr << "Loaded model in kinDynComp" << std::endl; Vector6 baseAccKinDyn; JointDOFsDoubleArray jntAccKinDyn; setRandomState(dynComp,dynTree,kinDynComp,baseAccKinDyn,jntAccKinDyn); std::cerr << "Test transforms " << std::endl; testTransformsConsistency(dynComp,dynTree,kinDynComp); std::cerr << "Test jacob " << std::endl; testJacobianConsistency(dynComp,dynTree,kinDynComp); testRegressorConsistency(dynComp,dynTree); testCOMConsistency(dynComp,dynTree,kinDynComp); testMassMatrixConsistency(dynComp,dynTree,kinDynComp); testInverseDynamicsConsistency(dynComp,kinDynComp,baseAccKinDyn,jntAccKinDyn); }
bool simpleLeggedOdometry::init(KDL::CoDyCo::UndirectedTree & undirected_tree, const int initial_world_frame_position_index, const int initial_fixed_link_index) { if( odometry_model ) { delete odometry_model; odometry_model=0; } odometry_model = new iCub::iDynTree::DynTree(undirected_tree.getTree(),undirected_tree.getSerialization()); bool ok = reset(initial_world_frame_position_index,initial_fixed_link_index); return ok; }
bool simpleLeggedOdometry::init(KDL::CoDyCo::UndirectedTree & undirected_tree, const std::string& initial_world_frame_position, const std::string& initial_fixed_link) { int initial_world_frame_position_index = undirected_tree.getLink(initial_world_frame_position)->getLinkIndex(); int initial_fixed_link_index = undirected_tree.getLink(initial_fixed_link)->getLinkIndex();; if( initial_fixed_link_index < 0 || initial_world_frame_position_index < 0 ) { return false; } return init(undirected_tree,initial_world_frame_position_index,initial_fixed_link_index); }
/** * * @param _reverse_direction if true, reverse the direction of the regressor (root to joint instead of leaf to joint) default:false */ torqueRegressor(const KDL::CoDyCo::UndirectedTree & _undirected_tree, const iDynTree::SensorsList & _sensors_tree, const std::vector<int> & _linkIndeces2regrCols, const std::string & dof_name, const bool _reverse_direction = false, const std::vector<bool> & _activated_ft_sensors=std::vector< bool>(0), const bool _consider_ft_offset=false, const bool _verbose=true ) : p_undirected_tree(&_undirected_tree), p_sensors_tree(&_sensors_tree), linkIndeces2regrCols(_linkIndeces2regrCols), torque_dof(dof_name), reverse_direction(_reverse_direction), activated_ft_sensors(_activated_ft_sensors), consider_ft_offset(_consider_ft_offset), subtree_links_indices(0), verbose(_verbose), NrOfRealLinks_subtree(0) { assert(linkIndeces2regrCols.size() == p_undirected_tree->getNrOfLinks()); NrOfRealLinks_subtree = 0; for(int ll=0; ll < (int)linkIndeces2regrCols.size(); ll++ ) { if( linkIndeces2regrCols[ll] != -1 ) { NrOfRealLinks_subtree++; } } assert(NrOfRealLinks_subtree >= 0); assert(NrOfRealLinks_subtree <= (int)linkIndeces2regrCols.size()); }
/** * Constructor for base dynamics regressor * */ baseDynamicsRegressor(const KDL::CoDyCo::UndirectedTree & _undirected_tree, const std::vector<int> & _linkIndeces2regrCols, bool _verbose=true): p_undirected_tree(&_undirected_tree), linkIndeces2regrCols(_linkIndeces2regrCols), verbose(_verbose) { assert(linkIndeces2regrCols.size() == p_undirected_tree->getNrOfLinks()); NrOfRealLinks_subtree = 0; for(int ll=0; ll < (int)linkIndeces2regrCols.size(); ll++ ) { if( linkIndeces2regrCols[ll] != -1 ) { NrOfRealLinks_subtree++; } } assert(NrOfRealLinks_subtree >= 0); assert(NrOfRealLinks_subtree <= (int)linkIndeces2regrCols.size()); }
/** * Constructor for subtree articulated dynamics regressor * * @param _subtree_leaf_links the list of name of the leaf links of the considered subtree */ subtreeBaseDynamicsRegressor(const KDL::CoDyCo::UndirectedTree & _undirected_tree, const iDynTree::SensorsList & _sensors_tree, const std::vector<int> & _linkIndices2regrCols, std::vector< std::string> _subtree_leaf_links=std::vector< std::string>(0), const bool _consider_ft_offset=false, bool _verbose=true): p_undirected_tree(&_undirected_tree), p_sensors_tree(&_sensors_tree), linkIndices2regrCols(_linkIndices2regrCols), subtree_leaf_links(_subtree_leaf_links), consider_ft_offset(_consider_ft_offset), subtree_links_indices(0), verbose(_verbose), NrOfRealLinks_subtree(0) { assert(linkIndices2regrCols.size() == p_undirected_tree->getNrOfLinks()); NrOfRealLinks_subtree = 0; for(int ll=0; ll < (int)linkIndices2regrCols.size(); ll++ ) { if( linkIndices2regrCols[ll] != -1 ) { NrOfRealLinks_subtree++; } } assert(NrOfRealLinks_subtree >= 0); assert(NrOfRealLinks_subtree <= (int)linkIndices2regrCols.size()); }
bool checkURDF2DHForAGivenChain(const KDL::CoDyCo::UndirectedTree& undirectedTree, const std::string& base_frame, const std::string& end_effector_frame) { KDL::Chain kdl_chain; iCub::iKin::iKinLimb ikin_limb; //cerr << "urdf2dh test testing chain extraction between " << base_frame // << " and " << end_effector_frame << endl; KDL::Tree kdl_rotated_tree = undirectedTree.getTree(base_frame); bool result = kdl_rotated_tree.getChain(base_frame,end_effector_frame,kdl_chain); if( !result ) { cerr << "urdf2dh: Impossible to find " << base_frame << " or " << end_effector_frame << " in the URDF." << endl; return false; } // // Convert the chain and the limits to an iKin chain (i.e. DH parameters) // KDL::JntArray qmin(kdl_chain.getNrOfJoints()),qmax(kdl_chain.getNrOfJoints()); for(size_t i=0; i < qmin.rows(); i++ ) { qmin(i) = -10.0; qmax(i) = 10.0; } result = iKinLimbFromKDLChain(kdl_chain,ikin_limb,qmin,qmax,1000); if( !result ) { cerr << "urdf2dh: Could not export KDL::Tree to iKinChain" << endl; return false; } return checkChainsAreEqual(kdl_chain,ikin_limb); }
iDynTree::Wrench simulateFTSensorFromKinematicState(const KDL::CoDyCo::UndirectedTree & icub_undirected_tree, const KDL::JntArray & q, const KDL::JntArray & dq, const KDL::JntArray & ddq, const KDL::Twist & base_velocity, const KDL::Twist & base_acceleration, const std::string ft_sensor_name, const iDynTree::SensorsList & sensors_tree ) { // We can try to simulate the same sensor with the usual inverse dynamics KDL::CoDyCo::Traversal traversal; icub_undirected_tree.compute_traversal(traversal); std::vector<KDL::Twist> v(icub_undirected_tree.getNrOfLinks()); std::vector<KDL::Twist> a(icub_undirected_tree.getNrOfLinks()); std::vector<KDL::Wrench> f(icub_undirected_tree.getNrOfLinks()); std::vector<KDL::Wrench> f_gi(icub_undirected_tree.getNrOfLinks()); std::vector<KDL::Wrench> f_ext(icub_undirected_tree.getNrOfLinks(),KDL::Wrench::Zero()); KDL::JntArray torques(icub_undirected_tree.getNrOfDOFs()); KDL::Wrench base_force; KDL::CoDyCo::rneaKinematicLoop(icub_undirected_tree, q,dq,ddq,traversal,base_velocity,base_acceleration, v,a,f_gi); KDL::CoDyCo::rneaDynamicLoop(icub_undirected_tree,q,traversal,f_gi,f_ext,f,torques,base_force); unsigned int sensor_index; sensors_tree.getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE,ft_sensor_name,sensor_index); std::cout << ft_sensor_name << " has ft index " << sensor_index << std::endl; iDynTree::SixAxisForceTorqueSensor * p_sens = (iDynTree::SixAxisForceTorqueSensor *) sensors_tree.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,sensor_index); iDynTree::Wrench simulate_measurement; KDL::CoDyCo::Regressors::simulateMeasurement_sixAxisFTSensor(traversal,f,p_sens,simulate_measurement); return simulate_measurement; }