Beispiel #1
0
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;
}
Beispiel #2
0
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;
}
Beispiel #3
0
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;
    }
}
Beispiel #4
0
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;
    }
Beispiel #8
0
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;
}
Beispiel #9
0
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;
}