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;
}
Esempio n. 2
0
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);
}
Esempio n. 5
0
        /**
         *
         * @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());
 }
Esempio n. 8
0
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;
}