KinematicKDL.hpp

Go to the documentation of this file.

#ifndef ODOMETRY_KINEMATIC_KDL_HPP
#define ODOMETRY_KINEMATIC_KDL_HPP

/* Base includes */
#include <assert.h> 
#include <sstream>
#include <string> 
#include <base/Time.hpp>
#include <base/Eigen.hpp>
#include <base-logging/Logging.hpp>

#include <Eigen/Core>

/* KDL Parser */
#include <kdl_parser/kdl_parser.hpp>

#include <kdl/frames_io.hpp> 
#include <kdl/treefksolver.hpp> 
#include <kdl/treefksolverpos_recursive.hpp> 
#include <kdl/chainjnttojacsolver.hpp> 
#include <kdl/treejnttojacsolver.hpp> 
namespace threed_odometry
{

    class KinematicKDL
    {

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW //Structures having Eigen members

    protected:
        KDL::Tree tree; 
        std::string model_name; 
        std::vector<KDL::Chain> ichains; 
        std::vector< std::vector<std::string> > ichains_joint_names; 
        std::vector<std::string> contact_point_segments; 
        std::vector<std::string> contact_angle_segments; 
        int number_robot_joints, number_slip_joints, number_contact_joints; 
    public:
        unsigned int model_dof; 
    public:
        KinematicKDL (const std::string &urdf_file, const std::vector<std::string> &_contact_points,
                const std::vector<std::string> &contact_angles, const int _number_robot_joints, const int _number_slip_joints, const int _number_contact_joints);
        ~KinematicKDL();

        std::string getName();
        void fkSolver(const std::vector<double> &positions, const std::vector<std::string> &tip_names, std::vector<Eigen::Affine3d> &fkTrans, std::vector<base::Matrix6d> &fkCov);
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> jacobianSolver(const std::vector<std::string> &names, const std::vector<double> &positions);
        void unpackJoints(const std::vector<std::string>& joint_names, const std::vector<double>& joint_positions,
                                const std::vector<std::string>& involved_joints, KDL::JntArray& joint_array);
        void organizeJacobian(const int chainidx, const std::vector<std::string> &joint_names, const std::vector<std::string> &involved_joints,
                                    const Eigen::Matrix <double, Eigen::Dynamic, Eigen::Dynamic> &jacobian, Eigen::Matrix <double, Eigen::Dynamic, Eigen::Dynamic> &J);

    };

    static void printLink(const KDL::SegmentMap::const_iterator& link, const std::string& prefix)
    {
        std::cout << prefix << "- Segment " << link->second.segment.getName() << " [Joint "<< link->second.segment.getJoint().getName()<<
            "] origin axis: "<<link->second.segment.getJoint().JointOrigin()<<" rotating axis: "<<link->second.segment.getJoint().JointAxis()<<" has " << link->second.children.size() << " children" << std::endl;
        for (unsigned int i=0; i<link->second.children.size(); ++i)
            printLink(link->second.children[i], prefix + "  ");
    };

    static std::string getContactPoint (const KDL::SegmentMap::const_iterator& link, const int childrenIdx)
    {
        if (link->second.children.size() == 0)
            return link->second.segment.getName();

        std::cout<<link->second.segment.getName() << " has "<<link->second.children.size()<<" children\n";
        return getContactPoint(link->second.children[childrenIdx], 0);
    };

};

#endif