void dynamicsRegressorLoop(const UndirectedTree & ,
const KDL::JntArray &q,
const Traversal & traversal,
const std::vector<Frame>& X_b,
const std::vector<Twist>& v,
const std::vector<Twist>& a,
Eigen::MatrixXd & dynamics_regressor)
{
dynamics_regressor.setZero();
Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;
// Store the base_world translational transform in world orientation
KDL::Frame world_base_X_world_world = KDL::Frame(-(X_b[traversal.getBaseLink()->getLinkIndex()].p));
for(int l =(int)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) {
LinkMap::const_iterator link = traversal.getOrderedLink(l);
int i = link->getLinkIndex();
//Each link affects the dynamics of the joints from itself to the base
netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);
//Base dynamics
// The base dynamics is expressed with the orientation of the world but
// with respect to the base origin
dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(world_base_X_world_world*X_b[i])*netWrenchRegressor_i;
//dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;
LinkMap::const_iterator child_link = link;
LinkMap::const_iterator parent_link=traversal.getParentLink(link);
while( child_link != traversal.getOrderedLink(0) ) {
if( child_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
#ifndef NDEBUG
//std::cerr << "Calculating regressor columns for link " << link->getName() << " and joint " << child_link->getAdjacentJoint(parent_link)->getName() << std::endl;
#endif
int dof_index = child_link->getAdjacentJoint(parent_link)->getDOFIndex();
int child_index = child_link->getLinkIndex();
Frame X_j_i = X_b[child_index].Inverse()*X_b[i];
dynamics_regressor.block(6+dof_index,10*i,1,10) =
toEigen(parent_link->S(child_link,q(dof_index))).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
}
child_link = parent_link;
#ifndef NDEBUG
//std::cout << "Getting parent link of link of index " << child_link->getName() << " " << child_link->getLinkIndex() << std::endl;
//std::cout << "Current base " << traversal.order[0]->getName() << " " << traversal.order[0]->getLinkIndex() << std::endl;
#endif
parent_link = traversal.getParentLink(child_link);
}
}
}
int TreeInertialParameters::dynamicsRegressor( const JntArray &q,
const JntArray &q_dot,
const JntArray &q_dotdot,
const Twist& base_velocity,
const Twist& base_acceleration,
Eigen::MatrixXd & dynamics_regressor)
{
if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || dynamics_regressor.cols()!=10*ns || dynamics_regressor.rows()!=(6+nj))
return -1;
unsigned int i;
unsigned int j=0;
//Kinematic propagation copied by RNE
for(unsigned int l = 0; l < recursion_order.size(); l++ ) {
unsigned int curr_index = recursion_order[l];
const Segment& seg = seg_vector[curr_index]->second.segment;
const Joint& jnt = seg.getJoint();
double q_,qdot_,qdotdot_;
if(jnt.getType() !=Joint::None){
int idx = link2joint[curr_index];
q_=q(idx);
qdot_=q_dot(idx);
qdotdot_=q_dotdot(idx);
}else
q_=qdot_=qdotdot_=0.0;
Frame& eX = X[curr_index];
Twist& eS = S[curr_index];
Twist& ev = v[curr_index];
Twist& ea = a[curr_index];
Wrench& ef = f[curr_index];
Frame& eX_b = X_b[curr_index];
assert(X.size() == ns);
//Calculate segment properties: X,S,vj,cj
X[curr_index] = seg.pose(q_);
//eX=seg.pose(q_);
//Remark this is the inverse of the
//frame for transformations from
//the parent to the current coord frame
//Transform velocity and unit velocity to segment frame
Twist vj=eX.M.Inverse(seg.twist(q_,qdot_));
eS=eX.M.Inverse(seg.twist(q_,1.0));
//We can take cj=0, see remark section 3.5, page 55 since the unit velocity vector S of our joints is always time constant
//calculate velocity and acceleration of the segment (in segment coordinates)
int parent_index = lambda[curr_index];
if( parent_index == -1 ) {
eX_b = eX;
} else {
eX_b = X_b[parent_index]*eX;
}
Twist parent_a, parent_v;
if( parent_index == -1 ) {
parent_a = base_acceleration;
parent_v = base_velocity;
} else {
parent_a = a[parent_index];
parent_v = v[parent_index];
}
ev=eX.Inverse(parent_v)+vj;
ea=eX.Inverse(parent_a)+eS*qdotdot_+ev*vj;
}
//end kinematic phase
dynamics_regressor.setZero();
//just for design, then the loop can be put together
Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;
for(i=0;i<ns;i++) {
netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);
dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;
Frame X_j_i;
for(j=0;j<ns;j++) {
X_j_i = X_b[j].Inverse()*X_b[i];
if( seg_vector[j]->second.segment.getJoint().getType() != Joint::None ) {
if( indicator_function[i][link2joint[j]] ) {
dynamics_regressor.block(6+link2joint[j],10*i,1,10) =
toEigen(S[j]).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
}
}
}
}
return 0;
}
void dynamicsRegressorFixedBaseLoop(const UndirectedTree & ,
const KDL::JntArray &q,
const Traversal & traversal,
const std::vector<Frame>& X_b,
const std::vector<Twist>& v,
const std::vector<Twist>& a,
Eigen::MatrixXd & dynamics_regressor)
{
dynamics_regressor.setZero();
Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;
for(int l =(int)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) {
LinkMap::const_iterator link = traversal.getOrderedLink(l);
int i = link->getLinkIndex();
//Each link affects the dynamics of the joints from itself to the base
netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);
//dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;
LinkMap::const_iterator child_link = link;
LinkMap::const_iterator parent_link=traversal.getParentLink(link);
while( child_link != traversal.getOrderedLink(0) ) {
if( child_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
int dof_index = child_link->getAdjacentJoint(parent_link)->getDOFIndex();
int child_index = child_link->getLinkIndex();
Frame X_j_i = X_b[child_index].Inverse()*X_b[i];
dynamics_regressor.block(dof_index,10*i,1,10) =
toEigen(parent_link->S(child_link,q(dof_index))).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
}
child_link = parent_link;
parent_link = traversal.getParentLink(child_link);
}
}
}
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;
}
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;
}
