Esempio n. 1
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());
        }
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;
}
 /**
  * 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());
 }