bool kdl_urdf_tools::initialize_kinematics_from_urdf(
const std::string &robot_description,
const std::string &root_link,
const std::string &tip_link,
unsigned int &n_dof,
KDL::Chain &kdl_chain,
KDL::Tree &kdl_tree,
urdf::Model &urdf_model)
{
if(robot_description.length() == 0) {
ROS_ERROR("URDF string is empty.");
return false;
}
// Construct an URDF model from the xml string
urdf_model.initString(robot_description);
// Get a KDL tree from the robot URDF
if (!kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
// Populate the KDL chain
if(!kdl_tree.getChain(root_link, tip_link, kdl_chain))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_link<<" --> "<<tip_link);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin();
it != segment_map.end();
it++ )
{
ROS_ERROR_STREAM( " "<<(*it).first);
}
return false;
}
// Store the number of degrees of freedom of the chain
n_dof = kdl_chain.getNrOfJoints();
return true;
}
TorqueEstimationTree::TorqueEstimationTree(KDL::Tree& icub_kdl,
std::vector< kdl_format_io::FTSensorData > ft_sensors,
std::vector< std::string > ft_serialization,
std::string fixed_link, unsigned int verbose)
{
yarp::sig::Vector q_max(icub_kdl.getNrOfJoints(),1000.0);
yarp::sig::Vector q_min(icub_kdl.getNrOfJoints(),-1000.0);
KDL::CoDyCo::TreeSerialization serial(icub_kdl);
std::vector<std::string> dof_serialization;
for(int i = 0; i < serial.getNrOfDOFs(); i++ )
{
dof_serialization.push_back(serial.getDOFName(i));
}
TorqueEstimationConstructor(icub_kdl,ft_sensors,dof_serialization,ft_serialization,q_min,q_max,fixed_link,verbose);
}
void testFwdKinConsistency(std::string modelFilePath)
{
std::cout << "Testing " << modelFilePath << std::endl;
// Load iDynTree model and compute a default traversal
ModelLoader loader;
loader.loadModelFromFile(modelFilePath);
iDynTree::Model model = loader.model();
iDynTree::Traversal traversal;
model.computeFullTreeTraversal(traversal);
// Load KDL tree
KDL::Tree tree;
treeFromUrdfFile(modelFilePath,tree);
KDL::CoDyCo::UndirectedTree undirected_tree(tree);
KDL::CoDyCo::Traversal kdl_traversal;
undirected_tree.compute_traversal(kdl_traversal);
// A KDL::Tree only supports 0 and 1 DOFs joints, and
// KDL::Tree::getNrOfJoints does not count the 0 dof joints, so
// it is actually equivalent to iDynTree::Model::getNrOfDOFs
ASSERT_EQUAL_DOUBLE(tree.getNrOfJoints(),model.getNrOfDOFs());
// Input : joint positions and base position with respect to world
iDynTree::FreeFloatingPos baseJntPos(model);
iDynTree::FreeFloatingVel baseJntVel(model);
iDynTree::FreeFloatingAcc baseJntAcc(model);
baseJntPos.worldBasePos() = getRandomTransform();
baseJntVel.baseVel() = getRandomTwist();
baseJntAcc.baseAcc() = getRandomTwist();
for(unsigned int jnt=0; jnt < baseJntPos.getNrOfPosCoords(); jnt++)
{
baseJntPos.jointPos()(jnt) = getRandomDouble();
baseJntVel.jointVel()(jnt) = getRandomDouble();
baseJntAcc.jointAcc()(jnt) = getRandomDouble();
}
// Build a map between KDL joints and iDynTree joints because we are not sure
// that the joint indices will match
std::vector<int> kdl2idyntree_joints;
kdl2idyntree_joints.resize(undirected_tree.getNrOfDOFs());
for(unsigned int dofIndex=0; dofIndex < undirected_tree.getNrOfDOFs(); dofIndex++)
{
std::string dofName = undirected_tree.getJunction(dofIndex)->getName();
int idyntreeJointIndex = model.getJointIndex(dofName);
kdl2idyntree_joints[dofIndex] = idyntreeJointIndex;
}
KDL::JntArray jntKDL(undirected_tree.getNrOfDOFs());
KDL::JntArray jntVelKDL(undirected_tree.getNrOfDOFs());
KDL::JntArray jntAccKDL(undirected_tree.getNrOfDOFs());
KDL::Frame worldBaseKDL;
KDL::Twist baseVelKDL;
KDL::Twist baseAccKDL;
ToKDL(baseJntPos,worldBaseKDL,jntKDL,kdl2idyntree_joints);
ToKDL(baseJntVel,baseVelKDL,jntVelKDL,kdl2idyntree_joints);
ToKDL(baseJntAcc,baseAccKDL,jntAccKDL,kdl2idyntree_joints);
// Output : link (for iDynTree) or frame positions with respect to world
std::vector<KDL::Frame> framePositions(undirected_tree.getNrOfLinks());
iDynTree::LinkPositions linkPositions(model);
// Build a map between KDL links and iDynTree links because we are not sure
// that the link indices will match
std::vector<int> idynTree2KDL_links;
idynTree2KDL_links.resize(model.getNrOfLinks());
for(unsigned int linkIndex=0; linkIndex < model.getNrOfLinks(); linkIndex++)
{
std::string linkName = model.getLinkName(linkIndex);
int kdlLinkIndex = undirected_tree.getLink(linkName)->getLinkIndex();
idynTree2KDL_links[linkIndex] = kdlLinkIndex;
}
// Compute position forward kinematics with old KDL code and with the new iDynTree code
KDL::CoDyCo::getFramesLoop(undirected_tree,
jntKDL,
kdl_traversal,
framePositions,
worldBaseKDL);
ForwardPositionKinematics(model,traversal,baseJntPos,linkPositions);
// Check results
for(unsigned int travEl = 0; travEl < traversal.getNrOfVisitedLinks(); travEl++)
{
LinkIndex linkIndex = traversal.getLink(travEl)->getIndex();
ASSERT_EQUAL_TRANSFORM_TOL(linkPositions(linkIndex),ToiDynTree(framePositions[idynTree2KDL_links[linkIndex]]),1e-1);
}
// Compution velocity & acceleration kinematics
std::vector<KDL::Twist> kdlLinkVel(undirected_tree.getNrOfLinks());
std::vector<KDL::Twist> kdlLinkAcc(undirected_tree.getNrOfLinks());
KDL::CoDyCo::rneaKinematicLoop(undirected_tree,jntKDL,jntVelKDL,jntAccKDL,kdl_traversal,
baseVelKDL,baseAccKDL,kdlLinkVel,kdlLinkAcc);
iDynTree::LinkVelArray linksVels(model);
iDynTree::LinkAccArray linksAcc(model);
ForwardVelAccKinematics(model,traversal,baseJntPos,baseJntVel,baseJntAcc,linksVels,linksAcc);
// Check results
for(unsigned int travEl = 0; travEl < traversal.getNrOfVisitedLinks(); travEl++)
{
LinkIndex linkIndex = traversal.getLink(travEl)->getIndex();
ASSERT_EQUAL_VECTOR(linksVels(linkIndex).asVector(),
ToiDynTree(kdlLinkVel[idynTree2KDL_links[linkIndex]]).asVector());
ASSERT_EQUAL_VECTOR(linksAcc(linkIndex).asVector(),
ToiDynTree(kdlLinkAcc[idynTree2KDL_links[linkIndex]]).asVector());
}
// Compute position, velocity and acceleration
LinkPositions linksPos_check(model);
iDynTree::LinkVelArray linksVels_check(model);
iDynTree::LinkAccArray linksAcc_check(model);
ForwardPosVelAccKinematics(model,traversal,baseJntPos, baseJntVel, baseJntAcc,
linksPos_check,linksVels_check,linksAcc_check);
for(unsigned int travEl = 0; travEl < traversal.getNrOfVisitedLinks(); travEl++)
{
LinkIndex linkIndex = traversal.getLink(travEl)->getIndex();
ASSERT_EQUAL_TRANSFORM(linkPositions(linkIndex),
linksPos_check(linkIndex));
ASSERT_EQUAL_VECTOR(linksVels(linkIndex).asVector(),
linksVels_check(linkIndex).asVector());
ASSERT_EQUAL_VECTOR(linksAcc(linkIndex).asVector(),
linksAcc(linkIndex).asVector());
}
// Compute torques (i.e. inverse dynamics)
LinkNetExternalWrenches fExt(model);
LinkInternalWrenches f(model);
FreeFloatingGeneralizedTorques baseWrenchJointTorques(model);
KDL::JntArray jntTorques(model.getNrOfDOFs());
KDL::Wrench baseWrench;
std::vector<KDL::Wrench> fExtKDL(undirected_tree.getNrOfLinks());
std::vector<KDL::Wrench> fKDL(undirected_tree.getNrOfLinks());
// First set to zero all the fExtKDL, fKDL wrenches : then
// the following loop will set the one that correspond to "real"
// iDynTree links to the same value we used in iDynTree
for(unsigned int linkKDL = 0; linkKDL < undirected_tree.getNrOfLinks(); linkKDL++)
{
fKDL[linkKDL] = KDL::Wrench::Zero();
fExtKDL[linkKDL] = KDL::Wrench::Zero();
}
for(unsigned int link = 0; link < model.getNrOfLinks(); link++ )
{
fExt(link) = getRandomWrench();
fExtKDL[idynTree2KDL_links[link]] = ToKDL(fExt(link));
fKDL[idynTree2KDL_links[link]] = KDL::Wrench::Zero();
}
bool ok = RNEADynamicPhase(model,traversal,
baseJntPos.jointPos(),
linksVels,linksAcc,fExt,f,baseWrenchJointTorques);
int retVal = KDL::CoDyCo::rneaDynamicLoop(undirected_tree,jntKDL,kdl_traversal,
kdlLinkVel,kdlLinkAcc,
fExtKDL,fKDL,jntTorques,baseWrench);
ASSERT_EQUAL_DOUBLE(ok,true);
ASSERT_EQUAL_DOUBLE(retVal,0);
/***
* This check is commented out because KDL::CoDyCo::rneaDynamicLoop returned
* a reverse baseWrench.. still need to be investigated.
* (The - is necessary for consistency with the mass matrix..)
*
*/
//ASSERT_EQUAL_VECTOR(baseWrenchJointTorques.baseWrench().asVector(),
// ToiDynTree(baseWrench).asVector());
for(int link = 0; link < model.getNrOfLinks(); link++ )
{
ASSERT_EQUAL_VECTOR(f(link).asVector(),ToiDynTree(fKDL[idynTree2KDL_links[link]]).asVector());
}
for(int dof = 0; dof < model.getNrOfDOFs(); dof++ )
{
ASSERT_EQUAL_DOUBLE(jntTorques(dof),baseWrenchJointTorques.jointTorques()(kdl2idyntree_joints[dof]));
}
// Check mass matrix
iDynTree::FreeFloatingMassMatrix massMatrix(model);
iDynTree::LinkInertias crbis(model);
ok = CompositeRigidBodyAlgorithm(model,traversal,
baseJntPos.jointPos(),
crbis,massMatrix);
KDL::CoDyCo::FloatingJntSpaceInertiaMatrix massMatrixKDL(undirected_tree.getNrOfDOFs()+6);
std::vector<KDL::RigidBodyInertia> Ic(undirected_tree.getNrOfLinks());
retVal = KDL::CoDyCo::crba_floating_base_loop(undirected_tree,kdl_traversal,jntKDL,Ic,massMatrixKDL);
ASSERT_IS_TRUE(ok);
ASSERT_EQUAL_DOUBLE_TOL(retVal,0,1e-8);
// Check composite rigid body inertias
for(int link = 0; link < model.getNrOfLinks(); link++ )
{
ASSERT_EQUAL_MATRIX(crbis(link).asMatrix(),ToiDynTree(Ic[idynTree2KDL_links[link]]).asMatrix());
}
std::cerr << "iDynTree " << massMatrix.toString() << std::endl;
std::cerr << "massMatrix " << massMatrixKDL.data << std::endl;
// Check CRBA algorithm
for(int ii=0; ii < model.getNrOfDOFs()+6; ii++ )
{
for( int jj=0; jj < model.getNrOfDOFs()+6; jj++ )
{
int idyntreeII = kdl2idyntreeFloatingDOFMapping(ii,kdl2idyntree_joints);
int idyntreeJJ = kdl2idyntreeFloatingDOFMapping(jj,kdl2idyntree_joints);
ASSERT_EQUAL_DOUBLE_TOL(massMatrix(idyntreeII,idyntreeJJ),massMatrixKDL(ii,jj),1e-8);
}
}
return;
}
bool Kinematics::build_chain(FINGERS f, const KDL::Tree &kdl_tree_){
std::string root_name, tip_name;
se_finger finger_info;
std::string finger_name;
try{
finger_info = root_tip.at(f);
root_name = finger_info.root_name;
tip_name = finger_info.tip_name;
}catch(const std::out_of_range& oor){
ROS_ERROR("Kinematics::build_chain no such finger type");
return false;
}
// joint_limits& joint_limits_ = finger_joint_limits.at(f);
KDL::Chain& kdl_chain_ = kdl_chains[f];
finger_name = f_enum2str.at(f);
// Populate the KDL chain
if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_name<<" --> "<<tip_name);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree_.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin(); it != segment_map.end(); it++ )
ROS_ERROR_STREAM( " "<<(*it).first);
return false;
}
ROS_INFO("=== Loading %s KChain ===",finger_name.c_str());
ROS_INFO("root: %s\t tip: %s",root_name.c_str(),tip_name.c_str());
ROS_INFO("Number of segments: %d",kdl_chain_.getNrOfSegments());
ROS_INFO("Number of joints in chain: %d",kdl_chain_.getNrOfJoints());
// Parsing joint limits from urdf model
/* boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint_;
joint_limits_.min.resize(kdl_chain_.getNrOfJoints());
joint_limits_.max.resize(kdl_chain_.getNrOfJoints());
joint_limits_.center.resize(kdl_chain_.getNrOfJoints());*/
/*int index;
for (int i = 0; i < kdl_chain_.getNrOfJoints() && link_; i++)
{
joint_ = model.getJoint(link_->parent_joint->name);
index = kdl_chain_.getNrOfJoints() - i - 1;
joint_limits_.min(index) = joint_->limits->lower;
joint_limits_.max(index) = joint_->limits->upper;
joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2;
link_ = model.getLink(link_->getParent()->name);
}
*/
/* std::cout<< "segment names" << std::endl;
int index;
for(std::size_t i = 0; i < kdl_chain_.getNrOfJoints();i++){
// std::cout<< kdl_chain_.segments[i].getName() << " " << kdl_chain_.segments[i].getJoint().getName() << " " << kdl_chain_.segments[i].getJoint().getTypeName() << std::endl;
joint_ = model.getJoint(kdl_chain_.segments[i].getJoint().getName());
index = i;
switch(joint_->type){
case urdf::Joint::REVOLUTE:
{
joint_limits_.min(i) = joint_->limits->lower;
joint_limits_.max(i) = joint_->limits->upper;
joint_limits_.center(i) = (joint_limits_.min(i) + joint_limits_.max(i))/2;
ROS_INFO("joint name: %s \t type: %d \t %f %f",joint_->name.c_str(),joint_->type,joint_->limits->lower,joint_->limits->upper);
break;
}
case urdf::Joint::FIXED:
{
ROS_INFO("joint name: %s \t type: %d",joint_->name.c_str(),joint_->type);
break;
}
default:
{
std::cerr<< "Kinematics::build_chain no such joint type: " << joint_->type << " implemented" << std::endl;
break;
}
}
}*/
}
void drawMyTree(KDL::Tree& a_tree)
{
//graphviz stuff
/****************************************/
Agraph_t *g;
GVC_t *gvc;
/* set up a graphviz context */
gvc = gvContext();
/* Create a simple digraph */
g = agopen("robot-structure", AGDIGRAPH);
//create vector to hold nodes
std::vector<Agnode_t*> nodeVector;
nodeVector.resize(a_tree.getSegments().size());
// printf("size of segments in tree map %d\n", a_tree.getSegments().size());
// printf("size of segments in tree %d\n", a_tree.getNrOfSegments());
//create vector to hold edges
std::vector<Agedge_t*> edgeVector;
edgeVector.resize(a_tree.getNrOfJoints() + 1);
int jointIndex = a_tree.getNrOfJoints() + 1;
// printf("size of joint array %d %d\n", jointIndex, a_tree.getNrOfJoints());
int segmentIndex = 0;
// fill in the node vector by iterating over tree segments
for (SegmentMap::const_iterator iter = a_tree.getSegments().begin(); iter != a_tree.getSegments().end(); ++iter)
{
//it would have been very useful if one could access list of joints of a tree
//list of segments is already possible
int stringLength = iter->second.segment.getName().size();
char name[stringLength + 1];
strcpy(name, iter->second.segment.getName().c_str());
//q_nr returned is the same value for the root and the its child. this is a bug
nodeVector[iter->second.q_nr] = agnode(g, name);
agsafeset(nodeVector[iter->second.q_nr], "color", "red", "");
agsafeset(nodeVector[iter->second.q_nr], "shape", "box", "");
std::cout << "index parent " << iter->second.q_nr << std::endl;
std::cout << "name parent " << iter->second.segment.getName() << std::endl;
std::cout << "joint name parent " << iter->second.segment.getJoint().getName() << std::endl;
std::cout << "joint type parent " << iter->second.segment.getJoint().getType() << std::endl;
// if (iter->second.segment.getJoint().getType() == Joint::None) //equals to joint type None
// {
// int stringLength = iter->second.segment.getJoint().getName().size();
// char name[stringLength + 1];
// strcpy(name, iter->second.segment.getJoint().getName().c_str());
// edgeVector[iter->second.q_nr] = agedge(g, nodeVector[iter->second.q_nr], nodeVector[iter->second.q_nr]);
// agsafeset(edgeVector[iter->second.q_nr], "label", name, "");
// }
if (segmentIndex < a_tree.getSegments().size())
segmentIndex++;
}
//fill in edge vector by iterating over joints in the tree
for (SegmentMap::const_iterator iter = a_tree.getSegments().begin(); iter != a_tree.getSegments().end(); ++iter)
{
//TODO: Fix node-edge connection relation
int stringLength = iter->second.segment.getJoint().getName().size();
// std::cout << "Joint name " << iter->second.segment.getJoint().getName() << std::endl;
char name[stringLength + 1];
strcpy(name, iter->second.segment.getJoint().getName().c_str());
for (std::vector<KDL::SegmentMap::const_iterator>::const_iterator childIter = iter->second.children.begin(); childIter != iter->second.children.end(); childIter++)
{
edgeVector[iter->second.q_nr] = agedge(g, nodeVector[iter->second.q_nr], nodeVector[(*childIter)->second.q_nr]);
agsafeset(edgeVector[iter->second.q_nr], "label", name, "");
}
// if (jointIndex != 0)
// {
// edgeVector[jointIndex] = agedge(g, nodeVector[segmentIndex], nodeVector[jointIndex]);
// agsafeset(edgeVector[jointIndex], "label", name, "");
// }
}
/* Compute a layout using layout engine from command line args */
// gvLayoutJobs(gvc, g);
gvLayout(gvc, g, "dot");
/* Write the graph according to -T and -o options */
//gvRenderJobs(gvc, g);
gvRenderFilename(gvc, g, "ps", "test-invdynamics2.ps");
/* Free layout data */
gvFreeLayout(gvc, g);
/* Free graph structures */
agclose(g);
gvFreeContext(gvc);
/* close output file, free context, and return number of errors */
return;
}
bool TreeKinematics::readJoints(urdf::Model &robot_model,
KDL::Tree &kdl_tree,
std::string &tree_root_name,
unsigned int &nr_of_jnts,
KDL::JntArray &joint_min,
KDL::JntArray &joint_max,
KDL::JntArray &joint_vel_max)
{
KDL::SegmentMap segmentMap;
segmentMap = kdl_tree.getSegments();
tree_root_name = kdl_tree.getRootSegment()->second.segment.getName();
nr_of_jnts = kdl_tree.getNrOfJoints();
ROS_DEBUG( "the tree's number of joints: [%d]", nr_of_jnts );
joint_min.resize(nr_of_jnts);
joint_max.resize(nr_of_jnts);
joint_vel_max.resize(nr_of_jnts);
info_.joint_names.resize(nr_of_jnts);
info_.limits.resize(nr_of_jnts);
// The following walks through all tree segments, extracts their joints except joints of KDL::None and extracts
// the information about min/max position and max velocity of the joints not of type urdf::Joint::UNKNOWN or
// urdf::Joint::FIXED
ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None.");
boost::shared_ptr<const urdf::Joint> joint;
for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it)
{
if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
{
// check, if joint can be found in the URDF model of the robot
joint = robot_model.getJoint(seg_it->second.segment.getJoint().getName().c_str());
if (!joint)
{
ROS_FATAL("Joint '%s' has not been found in the URDF robot model!", joint->name.c_str());
return false;
}
// extract joint information
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
{
ROS_DEBUG( "getting information about joint: [%s]", joint->name.c_str() );
double lower = 0.0, upper = 0.0, vel_limit = 0.0;
unsigned int has_pos_limits = 0, has_vel_limits = 0;
if ( joint->type != urdf::Joint::CONTINUOUS )
{
ROS_DEBUG("joint is not continuous.");
lower = joint->limits->lower;
upper = joint->limits->upper;
has_pos_limits = 1;
if (joint->limits->velocity)
{
has_vel_limits = 1;
vel_limit = joint->limits->velocity;
ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
}
else
{
has_vel_limits = 0;
vel_limit = 0.0;
ROS_DEBUG("joint has no velocity limit.");
}
}
else
{
ROS_DEBUG("joint is continuous.");
lower = -M_PI;
upper = M_PI;
has_pos_limits = 0;
if(joint->limits && joint->limits->velocity)
{
has_vel_limits = 1;
vel_limit = joint->limits->velocity;
ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
}
else
{
has_vel_limits = 0;
vel_limit = 0.0;
ROS_DEBUG("joint has no velocity limit.");
}
}
joint_min(seg_it->second.q_nr) = lower;
joint_max(seg_it->second.q_nr) = upper;
joint_vel_max(seg_it->second.q_nr) = vel_limit;
ROS_DEBUG("pos_min = %f, pos_max = %f, vel_max = %f", lower, upper, vel_limit);
info_.joint_names[seg_it->second.q_nr] = joint->name;
info_.limits[seg_it->second.q_nr].joint_name = joint->name;
info_.limits[seg_it->second.q_nr].has_position_limits = has_pos_limits;
info_.limits[seg_it->second.q_nr].min_position = lower;
info_.limits[seg_it->second.q_nr].max_position = upper;
info_.limits[seg_it->second.q_nr].has_velocity_limits = has_vel_limits;
info_.limits[seg_it->second.q_nr].max_velocity = vel_limit;
}
}
}
return true;
}
void IkSolver::init(const KDL::Tree& tree,
const std::vector<std::string>& endpoint_names,
const std::vector<EndpointCoupling>& endpoint_couplings)
{
// Preconditions
assert(endpoint_names.size() == endpoint_couplings.size() && "endpoints and coupling vectors size mismatch");
// TODO: Verify that all endpoints are contained in tree
// Enpoint names vector
endpoint_names_ = endpoint_names;
// Problem size
const size_t q_dim = tree.getNrOfJoints(); // Joint space dimension
size_t x_dim = 0; // Task space dimension, value assigned below
// Populate coupled directions vector
coupled_dirs_.resize(endpoint_names_.size());
for (size_t i = 0; i < coupled_dirs_.size(); ++i)
{
if ((endpoint_couplings[i] & Position) == Position)
{
coupled_dirs_[i].push_back(0);
coupled_dirs_[i].push_back(1);
coupled_dirs_[i].push_back(2);
x_dim += 3;
}
if ((endpoint_couplings[i] & Orientation) == Orientation)
{
coupled_dirs_[i].push_back(3);
coupled_dirs_[i].push_back(4);
coupled_dirs_[i].push_back(5);
x_dim += 3;
}
}
// Initialize kinematics solvers
fk_solver_.reset(new FkSolver(tree));
jac_solver_.reset(new JacSolver(tree));
inverter_.reset(new Inverter(x_dim, q_dim));
// Matrix inversion parameters TODO: Expose!
inverter_->setLsInverseThreshold(1e-5); // NOTE: Magic values
inverter_->setDlsInverseThreshold(1e-4);
inverter_->setMaxDamping(0.05);
// Default values of position solver parameters
delta_twist_max_ = Eigen::NumTraits<double>::highest();
delta_joint_pos_max_ = Eigen::NumTraits<double>::highest();
velik_gain_ = 1.0;
eps_ = Eigen::NumTraits<double>::epsilon();
max_iter_ = 1;
// Populate map from joint names to KDL tree indices
joint_name_to_idx_.clear();
const KDL::SegmentMap& tree_segments = tree.getSegments();
for (KDL::SegmentMap::const_iterator it = tree_segments.begin(); it != tree_segments.end(); ++it)
{
const KDL::Joint& joint = it->second.segment.getJoint();
if (joint.getType() != KDL::Joint::None)
{
joint_name_to_idx_[joint.getName()] = it->second.q_nr;
}
}
// Joint space weights updater
limits_avoider_.reset(new JointPositionLimitsAvoider(q_dim));
limits_avoider_->setSmoothing(0.8); // NOTE: Magic value
// Preallocate IK resources
delta_twist_ = VectorXd::Zero(x_dim);
delta_q_ = VectorXd::Zero(q_dim);
q_tmp_ = VectorXd::Zero(q_dim);
jacobian_ = Eigen::MatrixXd(x_dim, q_dim);
jacobian_tmp_ = KDL::Jacobian(q_dim);
Wx_ = VectorXd::Ones(x_dim);
q_posture_ = KDL::JntArray(q_dim);
nullspace_projector_ = Eigen::MatrixXd(q_dim, q_dim);
identity_qdim_ = Eigen::MatrixXd::Identity(q_dim, q_dim);
}
