int getFrameLoop(const UndirectedTree & undirected_tree,
const KDL::JntArray &q,
const Traversal & traversal,
const int proximal_link_index,
const int distal_link_index,
Frame & frame_proximal_distal)
{
LinkMap::const_iterator distal_it = undirected_tree.getLink(distal_link_index);
LinkMap::const_iterator proximal_it = undirected_tree.getLink(proximal_link_index);
Frame currentFrame;
Frame resultFrame = Frame::Identity();
for(LinkMap::const_iterator link=distal_it; link != proximal_it; link = traversal.getParentLink(link) ) {
LinkMap::const_iterator parent_link = traversal.getParentLink(link);
assert( parent_link != undirected_tree.getInvalidLinkIterator() );
double joint_position;
if( link->getAdjacentJoint(parent_link)->getJoint().getType() != Joint::None ) {
joint_position = q((link->getAdjacentJoint(parent_link))->getDOFIndex());
} else {
joint_position =0;
}
currentFrame = link->pose(parent_link,
joint_position);
resultFrame = currentFrame*resultFrame;
}
frame_proximal_distal = resultFrame;
return 0;
}
void inertialParametersVectorLoopFakeLinks(const UndirectedTree & undirected_tree,
Eigen::VectorXd & parameters_vector,
std::vector< std::string > fake_links_names)
{
int real_index_loop = 0;
for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
if( std::find(fake_links_names.begin(), fake_links_names.end(), undirected_tree.getLink(i)->getName()) == fake_links_names.end() ) {
parameters_vector.segment(real_index_loop*10,10) = Vectorize(undirected_tree.getLink(i)->getInertia());
real_index_loop++;
}
}
}
void inertialParametersVectorToUndirectedTreeLoop(const Eigen::VectorXd & parameters_vector,
UndirectedTree & undirected_tree)
{
for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
undirected_tree.getLink(i,false)->setInertia(deVectorize(parameters_vector.segment(i*10,10)));
}
}
void inertialParametersVectorLoop(const UndirectedTree & undirected_tree,
Eigen::VectorXd & parameters_vector)
{
for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
parameters_vector.segment(i*10,10) = Vectorize(undirected_tree.getLink(i)->getInertia());
}
}
void getFloatingBaseJacobianLoop(const UndirectedTree & undirected_tree,
const GeneralizedJntPositions &q,
const Traversal & traversal,
const int link_index,
Jacobian & jac)
{
Frame T_total = Frame::Identity(); //The transformation between link_index frame and current_link frame
assert(link_index < (int)undirected_tree.getNrOfLinks());
KDL::CoDyCo::LinkMap::const_iterator current_link;
current_link = undirected_tree.getLink(link_index);
//All the columns not modified are zero
SetToZero(jac);
KDL::CoDyCo::LinkMap::const_iterator parent_link=traversal.getParentLink(current_link);
while(current_link != traversal.getBaseLink()) {
double joint_pos = 0.0;
if( current_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
KDL::Twist jac_col;
int dof_index = current_link->getAdjacentJoint(parent_link)->getDOFIndex();
joint_pos = q.jnt_pos(dof_index);
KDL::Twist S_current_parent = parent_link->S(current_link,joint_pos);
jac_col = T_total*S_current_parent;
assert(6+dof_index < (int)jac.columns());
assert( dof_index < (int)undirected_tree.getNrOfDOFs() );
jac.setColumn(6+dof_index,jac_col);
}
KDL::Frame X_current_parent = parent_link->pose(current_link,joint_pos);
T_total = T_total*X_current_parent;
current_link = parent_link;
parent_link = traversal.getParentLink(current_link);
}
//Setting the floating part of the Jacobian
T_total = T_total*KDL::Frame(q.base_pos.M.Inverse());
jac.data.block(0,0,6,6) = TwistTransformationMatrix(T_total);
jac.changeBase(T_total.M.Inverse());
}
void getRelativeJacobianLoop(const UndirectedTree & undirected_tree,
const KDL::JntArray &q,
const Traversal & traversal,
const int link_index,
Jacobian & jac)
{
Frame T_total = Frame::Identity(); //The transformation between link_index frame and current_link frame
KDL::CoDyCo::LinkMap::const_iterator current_link;
current_link = undirected_tree.getLink(link_index);
//All the columns not modified are zero
SetToZero(jac);
KDL::CoDyCo::LinkMap::const_iterator parent_link=traversal.getParentLink(current_link);
while(current_link != traversal.getBaseLink()) {
double joint_pos = 0.0;
if( current_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
KDL::Twist jac_col;
int dof_index = current_link->getAdjacentJoint(parent_link)->getDOFIndex();
joint_pos = q(dof_index);
KDL::Twist S_current_parent = parent_link->S(current_link,joint_pos);
jac_col = T_total*S_current_parent;
jac.setColumn(dof_index,jac_col);
}
KDL::Frame X_current_parent = parent_link->pose(current_link,joint_pos);
T_total = T_total*X_current_parent;
current_link = parent_link;
parent_link = traversal.getParentLink(current_link);
}
}