int baseDynamicsRegressor::computeRegressor(const KDL::JntArray &q,
const KDL::JntArray &q_dot,
const KDL::JntArray &q_dotdot,
const std::vector<KDL::Frame> & X_dynamic_base,
const std::vector<KDL::Twist> & v,
const std::vector<KDL::Twist> & a,
const std::vector< KDL::Wrench > & measured_wrenches,
const KDL::JntArray & measured_torques,
Eigen::MatrixXd & regressor_matrix,
Eigen::VectorXd & known_terms)
{
#ifndef NDEBUG
//std::cerr << "Called computeRegressor " << std::endl;
//std::cerr << (*p_ft_list).toString();
#endif
const KDL::CoDyCo::UndirectedTree & undirected_tree = *p_undirected_tree;
if( regressor_matrix.rows() != getNrOfOutputs() ) {
return -1;
}
//all other columns, beside the one relative to the inertial parameters of the links of the subtree, are zero
regressor_matrix.setZero();
for(int link_id =0; link_id < (int)undirected_tree.getNrOfLinks(); link_id++ ) {
if( linkIndeces2regrCols[link_id] != -1 ) {
Eigen::Matrix<double,6,10> netWrenchRegressor_i = netWrenchRegressor(v[link_id],a[link_id]);
regressor_matrix.block(0,(int)(10*linkIndeces2regrCols[link_id]),getNrOfOutputs(),10) = WrenchTransformationMatrix(X_dynamic_base[link_id])*netWrenchRegressor_i;
}
}
return 0;
}
int torqueRegressor::computeRegressor(const KDL::JntArray &q,
const KDL::JntArray &/*q_dot*/,
const KDL::JntArray &/*q_dotdot*/,
const std::vector<KDL::Frame> & X_dynamic_base,
const std::vector<KDL::Twist> & v,
const std::vector<KDL::Twist> & a,
const iDynTree::SensorsMeasurements & measured_wrenches,
const KDL::JntArray & measured_torques,
Eigen::MatrixXd & regressor_matrix_global_column_serialization,
Eigen::VectorXd & known_terms)
{
#ifndef NDEBUG
if( verbose ) std::cerr << "Called torqueRegressor::computeRegressor " << std::endl;
#endif
//const KDL::CoDyCo::UndirectedTree & = *p_undirected_tree;
//const KDL::CoDyCo::FTSensorList & ft_list = *p_ft_list;
if( regressor_local_parametrization.rows() != getNrOfOutputs() ) {
return -1;
}
/**
* \todo move this stuff in UndirectedTree
*
*/
JunctionMap::const_iterator torque_dof_it = p_undirected_tree->getJunction(torque_dof_index);
LinkMap::const_iterator parent_root_it;
if( torque_dof_it->getChildLink() == p_undirected_tree->getLink(subtree_root_link_id) ) {
parent_root_it = torque_dof_it->getParentLink();
} else {
parent_root_it = torque_dof_it->getChildLink();
}
assert(torque_dof_it->getJunctionIndex() < (int)p_undirected_tree->getNrOfDOFs());
KDL::Twist S = parent_root_it->S(p_undirected_tree->getLink(subtree_root_link_id),q(torque_dof_it->getJunctionIndex()));
//all other columns, beside the one relative to the inertial parameters of the links of the subtree, are zero
regressor_local_parametrization.setZero();
for(int i =0; i < (int)subtree_links_indices.size(); i++ ) {
int link_id = subtree_links_indices[i];
#ifndef NDEBUG
if( verbose ) std::cerr << "Adding to the torque regressor of joint " << torque_dof_it->getName() << " the regressor relative to link " << p_undirected_tree->getLink(link_id)->getName() << std::endl;
#endif
if( linkIndeces2regrCols[link_id] != -1 ) {
Eigen::Matrix<double,6,10> netWrenchRegressor_i = netWrenchRegressor(v[link_id],a[link_id]);
regressor_local_parametrization.block(0,(int)(10*linkIndeces2regrCols[link_id]),getNrOfOutputs(),10)
= toEigen(S).transpose()*WrenchTransformationMatrix(X_dynamic_base[subtree_root_link_id].Inverse()*X_dynamic_base[link_id])*netWrenchRegressor_i;
}
}
#ifndef NDEBUG
if( consider_ft_offset ) {
if( verbose ) std::cerr << "considering ft offset" << std::endl;
} else {
if( verbose ) std::cerr << "not considering ft offset" << std::endl;
}
#endif
// if the ft offset is condidered, we have to set also the columns relative to the offset
// For each subgraph, we consider the measured wrenches as the one excerted from the rest of the
// tree to the considered subtree
// So the sign of the offset should be consistent with the other place where it is defined.
// In particular, the offset of a sensor is considered
// as an addictive on the measured wrench, so f_s = f_sr + f_o, where the frame of expression
// and the sign of f_s are defined in the SensorsTree structure
for(int leaf_id = 0; leaf_id < (int) subtree_leaf_links_indeces.size(); leaf_id++ ) {
if( consider_ft_offset ) {
int leaf_link_id = subtree_leaf_links_indeces[leaf_id];
// for now we are supporting just one six axis FT sensor for link
//std::vector< FTSensor> fts_link = ft_list.getFTSensorsOnLink(leaf_link_id);
//assert(fts_link.size()==1);
//int ft_id = fts_link[0].getID();
int ft_id = getFirstFTSensorOnLink(*p_sensors_tree,leaf_link_id);
assert( ft_id >= 0 );
iDynTree::SixAxisForceTorqueSensor * sens
= (iDynTree::SixAxisForceTorqueSensor *) p_sensors_tree->getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft_id);
assert( sens->isLinkAttachedToSensor(leaf_link_id) );
#ifndef NDEBUG
if( verbose ) std::cerr << "Adding to the torque regressor of joint " << torque_dof_it->getName()
<< " the columns relative to offset of ft sensor "
<< sens->getName() << std::endl;
#endif
/** \todo find a more robust way to get columns indeces relative to a given parameters */
assert(ft_id >= 0 && ft_id < 100);
assert(10*NrOfRealLinks_subtree+6*ft_id+5 < regressor_local_parametrization.cols());
double sign;
if( sens->getAppliedWrenchLink() == leaf_link_id ) {
sign = 1.0;
} else {
sign = -1.0;
}
iDynTree::Transform leaf_link_H_sensor;
bool ok = sens->getLinkSensorTransform(leaf_link_id,leaf_link_H_sensor);
assert(ok);
regressor_local_parametrization.block(0,(int)(10*NrOfRealLinks_subtree+6*ft_id),getNrOfOutputs(),6)
= sign*toEigen(S).transpose()*regressor_local_parametrization*WrenchTransformationMatrix(X_dynamic_base[subtree_root_link_id].Inverse()*X_dynamic_base[leaf_link_id]*iDynTree::ToKDL(leaf_link_H_sensor));
}
}
//The known terms are simply all the measured wrenches acting on the subtree
// projected on the axis plus the measured torque
known_terms(0) = measured_torques(torque_dof_index);
#ifndef NDEBUG
//std::cerr << "computing kt " << std::endl;
//std::cerr << (ft_list).toString();
#endif
for(int leaf_id = 0; leaf_id < (int) subtree_leaf_links_indeces.size(); leaf_id++ ) {
int leaf_link_id = subtree_leaf_links_indeces[leaf_id];
int ft_id = getFirstFTSensorOnLink(*p_sensors_tree,leaf_link_id);
assert( ft_id >= 0 );
iDynTree::SixAxisForceTorqueSensor * sens
= (iDynTree::SixAxisForceTorqueSensor *) p_sensors_tree->getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft_id);
#ifndef NDEBUG
//std::cerr << "For leaf " << leaf_link_id << " found ft sensor " << ft_id << " that connects " << fts_link[0].getParent() << " and " << fts_link[0].getChild() << std::endl;
#endif
iDynTree::Wrench sensor_measured_wrench, link_measured_branch;
bool ok = measured_wrenches.getMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,ft_id,sensor_measured_wrench);
ok = ok && sens->getWrenchAppliedOnLink(leaf_link_id,sensor_measured_wrench,link_measured_branch);
assert(ok);
known_terms(0) += dot(S,X_dynamic_base[subtree_root_link_id].Inverse()*X_dynamic_base[leaf_link_id]*iDynTree::ToKDL(link_measured_branch));
}
#ifndef NDEBUG
/*
std::cout << "Returning from computeRegressor (subtree):" << std::endl;
std::cout << "Regressor " << std::endl;
std::cout << regressor_matrix << std::endl;
std::cout << "known terms" << std::endl;
std::cout << known_terms << std::endl;
*/
#endif
convertLocalRegressorToGlobalRegressor(regressor_local_parametrization,regressor_matrix_global_column_serialization,this->localParametersIndexToOutputParametersIndex);
return 0;
}
