kdl 1

kdl_parser(ros package) provides tools to construct a KDL tree from an XML robot representation in URDF.
- kdl_parser::treeFromFile(".urdf"),直接解析的是urdf对象，对于'.xacro'文件可通过launch转化为urdf，再通过'kdl_parser::treeFromParam()'进行解析。

//comparation between ik solver
KDL::ChainIkSolverPos_NR; //duration: 0.000104653; accuracy:high
KDL::ChainIkSolverPos_LMA;//duration: 0.000001688; accuracy:low
RobotState::setFromIK;    //duration: 0.000146478; accuracy:high

//api examples
std::ifstream ifs(urdf_path);
std::string urdf_xml_string((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
urdf::ModelInterfaceSharedPtr urdf_model=urdf::parseURDF(urdf_xml_string);
if(!kdl_parser::treeFromUrdfModel(*urdf_model,tree)){
    printf("error. filed to parse urdf model.\n");
    return -1;
}
for(int i=0;i<urdf_model->getRoot()->child_links.size();i++){
    printf("link%d, name:%s\n",i,urdf_model->getRoot()->child_links[i]->name.c_str());
}

kdl ik

kdl twist:

diff()

differentiate position and orientations

// Returns a vector with the direction of the equiv. axis
// and its norm is angle
Vector Rotation::GetRot() const
{
Vector axis;
double angle;
angle = Rotation::GetRotAngle(axis,epsilon);
return axis * angle;
}

// from frames.inl
IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt) {
    Rotation R_b1_b2(R_a_b1.Inverse()*R_a_b2);
    return R_a_b1 * R_b1_b2.GetRot() / dt;
}

IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt) {
    return Twist(
            diff(F_a_b1.p,F_a_b2.p,dt),
            diff(F_a_b1.M,F_a_b2.M,dt)
            );
}

interpolation for rigid body motion

kdl dynamics

RNE: recursive newton euler