bool framesFromKDLTree(const KDL::Tree& tree,
std::vector<std::string>& framesNames,
std::vector<std::string>& parentLinkNames)
{
framesNames.clear();
parentLinkNames.clear();
KDL::SegmentMap::iterator seg;
KDL::SegmentMap segs;
KDL::SegmentMap::const_iterator root_seg;
root_seg = tree.getRootSegment();
segs = tree.getSegments();
for( seg = segs.begin(); seg != segs.end(); seg++ )
{
if( GetTreeElementChildren(seg->second).size() == 0 &&
GetTreeElementSegment(seg->second).getJoint().getType() == KDL::Joint::None &&
GetTreeElementSegment(seg->second).getInertia().getMass() == 0.0 )
{
std::string frameName = GetTreeElementSegment(seg->second).getName();
std::string parentLinkName = GetTreeElementSegment(GetTreeElementParent(seg->second)->second).getName();
framesNames.push_back(frameName);
parentLinkNames.push_back(parentLinkName);
//also check parent
KDL::Segment parent = GetTreeElementSegment(GetTreeElementParent(seg->second)->second);
if (parent.getJoint().getType() == KDL::Joint::None &&
parent.getInertia().getMass() == 0.0)
{
framesNames.push_back(parentLinkName);
}
}
}
return true;
}
sensor_msgs::JointState init_message(const KDL::Chain &chain) {
sensor_msgs::JointState msg;
for (unsigned int i=0; i < chain.getNrOfSegments(); ++i) {
KDL::Segment segment = chain.getSegment(i);
KDL::Joint joint = segment.getJoint();
if (joint.getType() == KDL::Joint::JointType::None) continue;
msg.name.push_back(joint.getName());
msg.position.push_back(0);
}
return msg;
}
int main()
{
KDLCollada kdlCollada;
vector <KDL::Chain> kinematicsModels;
const string filename = "puma.dae"; // loading collada kinematics model and converting it to kdl serial chain
if (!kdlCollada.load(COLLADA_MODELS_PATH + filename, kinematicsModels))
{
cout << "Failed to import " << filename;
return 0;
}
cout << "Imported " << kinematicsModels.size() << " kinematics chains" << endl;
for (unsigned int i = 0; i < kinematicsModels.size(); i++) // parsing output kdl serail chain
{
KDL::Chain chain = kinematicsModels[i];
cout << "Chain " << i << " has " << chain.getNrOfSegments() << " segments" << endl;
for (unsigned int u = 0; u < chain.getNrOfSegments(); u++)
{
KDL::Segment segment = chain.segments[u];
string segmentName = segment.getName();
cout << "Segment " << segmentName << " :" <<endl;
KDL::Frame f_tip = segment.getFrameToTip();
KDL::Vector rotAxis = f_tip.M.GetRot();
double rotAngle = f_tip.M.GetRotAngle(rotAxis);
KDL::Vector trans = f_tip.p;
cout << " frame: rotation " << rotAxis.x() << " " << rotAxis.y() << " " << rotAxis.z() << " " << rotAngle * 180 / M_PI << endl;
cout << " frame: translation " << trans.x() << " " << trans.y() << " " << trans.z() << endl;
KDL::RigidBodyInertia inertia = segment.getInertia();
KDL::Joint joint = segment.getJoint();
string jointName = joint.getName();
string jointType = joint.getTypeName();
KDL::Vector jointAxis = joint.JointAxis();
KDL::Vector jointOrigin = joint.JointOrigin();
cout << " joint name: " << jointName << endl;
cout << " type: " << jointType << endl;
cout << " axis: " << jointAxis.x() << " " <<jointAxis.y() << " " << jointAxis.z() << endl;
cout << " origin: " << jointOrigin.x() << " " << jointOrigin.y() << " " << jointOrigin.z() << endl;
}
}
return 0;
}
static inline std::vector<YAML::Node> get_frame_tip_nodes(const KDL::Segment& seg)
{
std::vector<YAML::Node> frames;
KDL::Frame frame = seg.getFrameToTip() * seg.getJoint().pose(0).Inverse();
// Set xyz
if(!KDL::Equal(frame.p, KDL::Vector::Zero()))
{
YAML::Node xyz_frame;
YAML::Node axis_angle_null;
axis_angle_null["axis-angle"].push_back(get_vector3(1, 0, 0));
axis_angle_null["axis-angle"].push_back(0);
xyz_frame["frame"].push_back(axis_angle_null);
xyz_frame["frame"].push_back(get_vector3(frame.p));
frames.push_back(xyz_frame);
}
// Set rpy
if(!KDL::Equal(frame.M, KDL::Rotation::Identity()))
{
std::vector<double> rpy(3);
frame.M.GetRPY(rpy[0],rpy[1],rpy[2]);
for (std::vector<double>::size_type i = rpy.size() - 1;
i != (std::vector<double>::size_type) -1; i--)
{
if(rpy[i] != 0)
{
YAML::Node rpy_frame;
YAML::Node axis_angle;
KDL::Vector axis;
axis[i] = 1;
axis_angle["axis-angle"].push_back(get_vector3(axis));
axis_angle["axis-angle"].push_back(rpy[i]);
rpy_frame["frame"].push_back(axis_angle);
rpy_frame["frame"].push_back(get_vector3(0, 0, 0));
frames.push_back(rpy_frame);
}
}
}
return frames;
}
bool addBaseTransformation(const KDL::Chain & old_chain, KDL::Chain & new_chain, KDL::Frame H_new_old)
{
new_chain = KDL::Chain();
for(unsigned int i=0; i<old_chain.getNrOfSegments(); i++) {
KDL::Segment segm;
segm = old_chain.getSegment(i);
//if is not the first segment add normally the segment
if( i != 0 ) {
new_chain.addSegment(segm);
} else {
//otherwise modify the segment before adding it
KDL::Segment new_segm;
KDL::Joint new_joint, old_joint;
old_joint = segm.getJoint();
KDL::Joint::JointType new_type;
switch(old_joint.getType()) {
case KDL::Joint::RotAxis:
case KDL::Joint::RotX:
case KDL::Joint::RotY:
case KDL::Joint::RotZ:
new_type = KDL::Joint::RotAxis;
break;
case KDL::Joint::TransAxis:
case KDL::Joint::TransX:
case KDL::Joint::TransY:
case KDL::Joint::TransZ:
new_type = KDL::Joint::TransAxis;
break;
case KDL::Joint::None:
default:
new_type = KDL::Joint::None;
}
//check !
new_joint = KDL::Joint(old_joint.getName(),H_new_old*old_joint.JointOrigin(),H_new_old.M*old_joint.JointAxis(),new_type);
new_segm = KDL::Segment(segm.getName(),new_joint,H_new_old*segm.getFrameToTip(),segm.getInertia());
new_chain.addSegment(new_segm);
}
}
return true;
}
KDL::Frame getH_new_old(KDL::Segment seg)
{
KDL::Joint jnt = seg.getJoint();
KDL::Frame frameToTip = seg.getFrameToTip();
return getH_new_old(jnt,frameToTip);
}
void TransformationCalculator::load_robot_model(std::string filepath, bool init_invalid){
clear_all();
bool success=false;
success = kdl_parser::treeFromFile(filepath, kdl_tree_);
if(!success){
is_initialized_ = false;
throw("Could not load Model file");
}
//Extract names of all joints defined in urdf file.
//These are the names that are expected to be referred to in update function.
KDL::SegmentMap map = kdl_tree_.getSegments();
std::string root_name = kdl_tree_.getRootSegment()->first;
KDL::Joint joint;
KDL::Segment segment;
for(KDL::SegmentMap::const_iterator it = map.begin(); it!=map.end(); ++it){
#ifdef KDL_USE_NEW_TREE_INTERFACE
segment = it->second->segment;
#else
segment = it->second.segment;
#endif
joint = segment.getJoint();
std::string j_name = joint.getName();
if(j_name == "NoName" || j_name == ""){
if(segment.getName() != root_name){
LOG_ERROR("Segment %s has an unnamed joint. This is not okay, but will skip it for now.", segment.getName().c_str());
}
LOG_DEBUG("Skipping NonName joint of root segment");
continue;
}
if(is_fixed(joint)){
if(std::find(static_joint_names_.begin(), static_joint_names_.end(), j_name) == static_joint_names_.end()){
static_segment_names_.push_back(segment.getName());
static_joint_names_.push_back(j_name);
joint_name2seg_name_[j_name] = segment.getName();
all_joint_names_.push_back(j_name);
}
}
if(is_valid_joint(joint)){
if(std::find(moving_joint_names_.begin(), moving_joint_names_.end(), j_name) == moving_joint_names_.end()){
moving_joint_names_.push_back(j_name);
joint_name2seg_name_[j_name] = segment.getName();
all_joint_names_.push_back(j_name);
}
}
}
//link_names_ = extract_keys(map);
//Populate transforms map with identity transforms
for(std::vector<std::string>::iterator it = moving_joint_names_.begin();
it != moving_joint_names_.end(); ++it)
{
std::string j_name = *it;
std::string seg_name = joint_name2seg_name_[j_name];
moving_joints_transforms_[j_name] = base::samples::RigidBodyState(true);
if(!init_invalid){
//initUnknow sets trasform to identity. If we donÄt want invalidating, set to identity.
moving_joints_transforms_[j_name].initUnknown();
}
moving_joints_transforms_[j_name].sourceFrame = seg_name;
#ifdef KDL_USE_NEW_TREE_INTERFACE
moving_joints_transforms_[j_name].targetFrame = get_tree_element(seg_name).parent->second->segment.getName();
#else
moving_joints_transforms_[j_name].targetFrame = get_tree_element(seg_name).parent->second.segment.getName();
#endif
}
//Populate static transforms vector
for(uint i=0; i<static_joint_names_.size(); i++){
std::string seg_name = static_segment_names_[i];
KDL::TreeElement tree_elem = get_tree_element(seg_name);
KDL::Frame kdl_transform = tree_elem.segment.pose(0);
std::string j_name = static_joint_names_[i];
base::samples::RigidBodyState rbs;
convert(kdl_transform, rbs);
//Skip root
if(tree_elem.segment.getName() == kdl_tree_.getRootSegment()->first){
continue;
}
#ifdef KDL_USE_NEW_TREE_INTERFACE
rbs.sourceFrame = tree_elem.parent->second->segment.getName();
#else
rbs.sourceFrame = tree_elem.parent->second.segment.getName();
#endif
rbs.targetFrame = tree_elem.segment.getName();
static_joints_transforms_[j_name] = rbs;
}
is_initialized_ = true;
}