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; }
void printKDLchain(std::string s,const KDL::Chain & kdlChain) { std::cout << s << std::endl; for(int p=0; p < (int)kdlChain.getNrOfSegments(); p++) { std::cout << "Segment " << p << ":" << std::endl; KDL::Segment seg = kdlChain.getSegment(p); KDL::Frame fra = seg.getFrameToTip(); std::cout << seg << std::endl; std::cout << fra << std::endl; } }
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; }
void PoseOperationTest::setUp() { KDL::Joint testJoint = KDL::Joint("TestJoint", KDL::Joint::RotZ, 1, 0, 0.01); KDL::Frame testFrame(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, -0.4, 0.0)); KDL::Segment testSegment = KDL::Segment("TestSegment", testJoint, testFrame); KDL::RotationalInertia testRotInerSeg(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); //around symmetry axis of rotation double pointMass = 0.25; //in kg KDL::RigidBodyInertia testInerSegment(pointMass, KDL::Vector(0.0, -0.4, 0.0), testRotInerSeg); testSegment.setInertia(testInerSegment); testTree.addSegment(testSegment,"root"); a_segmentState = kdle::SegmentState(); a_jointState = kdle::JointState(); }
void TransformationCalculator::update(const base::samples::Joints& joints) { base::JointState j_state; std::string j_name; std::string seg_name; KDL::Segment seg; KDL::Frame kdl_pose; std::map<std::string,base::samples::RigidBodyState>::iterator transform_it; if(!is_initialized_) throw std::runtime_error("TransformationCalculator was not initialized. Call load_robot_model first"); for(uint i=0; i<joints.size(); i++){ j_state = joints.elements[i]; j_name = joints.names[i]; if(is_invalid(j_state.position)){ LOG_ERROR("Received invalid joint position for joint %s", j_name.c_str()); continue; } try{ //Get segment seg_name = joint_name2seg_name_[j_name]; seg = get_segment_by_segment_name(seg_name); //Calculate pose kdl_pose = seg.pose(j_state.position); //Convert transform to eigen transform_it = moving_joints_transforms_.find(j_name); if(transform_it == moving_joints_transforms_.end()) throw("Unknown frame name. Should be thrown at this point, but ealier in get_segment."); convert(kdl_pose, transform_it->second); //Set timestap transform_it->second.time = joints.time; } catch(...){ LOG_WARN("Joint %s or segment %s are unknown", j_name.c_str(), seg_name.c_str()); continue; } } }
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; }