/**
*
* @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());
}
