예제 #1
0
    int JointInvertPolarity(const KDL::Joint & old_joint, const KDL::Frame & old_f_tip, KDL::Joint & new_joint, KDL::Frame & new_f_tip)
    {
 
        switch(old_joint.getType()) 
        {
            case Joint::RotAxis:
            case Joint::RotX:
            case Joint::RotY:
            case Joint::RotZ:
                //\todo document this
                new_f_tip = KDL::Frame(old_f_tip.M.Inverse(),-(old_f_tip.M.Inverse()*old_joint.JointOrigin()));
                new_joint = Joint(old_joint.getName(),-(old_f_tip.M.Inverse()*old_f_tip.p), -(old_f_tip.M.Inverse()*old_joint.JointAxis()), Joint::RotAxis);
            break;
            case Joint::TransAxis:
            case Joint::TransX:
            case Joint::TransY:
            case Joint::TransZ:
                new_f_tip = KDL::Frame(old_f_tip.M.Inverse(),-(old_f_tip.M.Inverse()*old_f_tip.p));
                new_joint = Joint(old_joint.getName(),-(old_f_tip.M.Inverse()*old_joint.JointOrigin()), -(old_f_tip.M.Inverse()*old_joint.JointAxis()), Joint::TransAxis);
            break;
            case Joint::None:
            default:
                new_f_tip = KDL::Frame(old_f_tip.M.Inverse(),-(old_f_tip.M.Inverse()*old_f_tip.p));
                new_joint = Joint(old_joint.getName(), Joint::None);
            break;
        }
        #ifndef NDEBUG
        std::cerr << "Exiting joint polarity inversion, checking all is working" << std::endl;
        double q = 0.83;
        std::cerr << old_joint.pose(q)*old_f_tip*new_joint.pose(q)*new_f_tip << std::endl;
        #endif 
        return 0;
    }
예제 #2
0
KDL::Frame getH_new_old(KDL::Joint jnt, KDL::Frame frameToTip)
{
    KDL::Frame H_new_old;
    if( (jnt.JointOrigin()-frameToTip.p).Norm() < 1e-12 || jnt.getType() == KDL::Joint::None ) {
        //No need of changing link frame
        H_new_old = KDL::Frame::Identity();
    } else {
        std::cerr << "[WARN] the reference frame of link connected to joint " << jnt.getName()  << "  has to be shifted to comply to URDF constraints" << std::endl;
        //H_new_old = (KDL::Frame(jnt.JointOrigin())*KDL::Frame(frameToTip.M)).Inverse()*frameToTip;
        H_new_old = KDL::Frame(frameToTip.M.Inverse()*(frameToTip.p-jnt.JointOrigin()));
    }
    return H_new_old;
}
예제 #3
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;
}
예제 #4
0
    static inline YAML::Node extract(const std::string& start_link, const std::string& end_link, const KDL::Chain& chain)
    {
        std::string var_suffix = "_var";
        std::string frame_suffix = "_frame";
        std::string expression_name = "fk";

        std::vector<YAML::Node> input_vars;
        std::vector<YAML::Node> joint_frames;
        YAML::Node frame_mul;
        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);

        int input_var_index = 0;
        for (std::vector<KDL::Segment>::const_iterator it = chain.segments.begin(); it != chain.segments.end(); ++it)
        {
            KDL::Joint joint = it->getJoint();

            std::string var_name = joint.getName() + var_suffix;
            std::string frame_name = joint.getName() + frame_suffix;

            YAML::Node joint_frame;
            if (joint.getType() != KDL::Joint::None)
            {
                // Set input variable definition
                YAML::Node input_var;
                input_var[joint.getName() + var_suffix]["input-var"] = input_var_index;
                input_var_index++;
                input_vars.push_back(input_var);

                // Set joint transform
                YAML::Node translation;
                YAML::Node rotation;

                // Set joint axis
                YAML::Node joint_axis = get_vector3(joint.JointAxis());
                if (joint.getType() == KDL::Joint::TransAxis)
                {
                    YAML::Node scale_vec;
                    scale_vec["scale-vector"].push_back(var_name);
                    scale_vec["scale-vector"].push_back(joint_axis);
                    translation["vector-add"].push_back(scale_vec);
                    translation["vector-add"].push_back(get_vector3(joint.JointOrigin()));
                    rotation = axis_angle_null;
                }
                else if (joint.getType() == KDL::Joint::RotAxis)
                {
                    translation = get_vector3(joint.JointOrigin());
                    rotation["axis-angle"].push_back(joint_axis);
                    rotation["axis-angle"].push_back(var_name);
                }

                // Create frame and add to list
                YAML::Node joint_transform;
                joint_transform["frame"].push_back(rotation);
                joint_transform["frame"].push_back(translation);
                joint_frame[frame_name]["frame-mul"].push_back(joint_transform);
            }

            std::vector<YAML::Node> f_tip_nodes = get_frame_tip_nodes(*it);
            for (std::vector<YAML::Node>::iterator f_tip_it = f_tip_nodes.begin(); f_tip_it != f_tip_nodes.end(); ++f_tip_it)
            {
                joint_frame[frame_name]["frame-mul"].push_back(*f_tip_it);
            }

            if (joint_frame.size() > 0)
            {
                joint_frames.push_back(joint_frame);
                frame_mul.push_back(frame_name);
            }
        }

        // Merge nodes
        YAML::Node node;
        for (std::vector<YAML::Node>::iterator it = input_vars.begin(); it != input_vars.end(); ++it)
        {
            node.push_back(*it);
        }
        for (std::vector<YAML::Node>::iterator it = joint_frames.begin(); it != joint_frames.end(); ++it)
        {
            node.push_back(*it);
        }
        YAML::Node fk_def;
        fk_def[expression_name]["frame-mul"] = frame_mul;
        node.push_back(fk_def);

        return node;
    }