KDL::ChainIdSolver_RNE gcSolver计算力矩

main.cpp文件：

#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include<iostream>
#include "math.h"
int main(int argc, char const *argv[])
{
    int mNumJnts = 6;
    KDL::Chain chain;
    KDL::Tree my_tree;
    KDL::JntArray jnt_q(mNumJnts);
    KDL::JntArray jnt_qd(mNumJnts);
    KDL::JntArray jnt_qdd(mNumJnts);
    KDL::JntArray jnt_taugc(mNumJnts);

    int ret;

    std::string full_path = "xxx.urdf";//根据需要填写

    if (!kdl_parser::treeFromFile(full_path, my_tree))
    {
        std::cout<<" treeFromFile Failed"<<std::endl;
        return -1;
    }
    my_tree.getChain("base_link", "link_6", chain);//根据需要填写
    std::cout <<"Kinematic chain expects "<< chain.getNrOfJoints() <<" joints"<<std::endl;
    std::cout <<"Kinematic chain expects "<< chain.getNrOfSegments() <<" segments"<<std::endl;
    for(double angle = 0; angle < M_PI/2; angle += 0.1){
        KDL::Wrenches jnt_wrenches;
        for (int i = 0; i < mNumJnts; i++) {
            jnt_q(i) = 0;
            jnt_qd(i) = 0.0;
            jnt_qdd(i) = 0.0;
            jnt_wrenches.push_back(KDL::Wrench());
        }
        // jnt_q(1) = 0;
        // jnt_q(2) = 0;
        jnt_q(0) = angle;
        // jnt_q(4) = 0;
        // jnt_q(5) = 0;
        // jnt_q(6) = 0;

        // Compute Dynamics 
        // KDL::Vector gravity(0.0, 0.0, -9.81);
        KDL::Vector gravity(-9.81, 0.0, 0.0);
        KDL::ChainIdSolver_RNE gcSolver = KDL::ChainIdSolver_RNE(chain, gravity);
        ret = gcSolver.CartToJnt(jnt_q, jnt_qd, jnt_qdd, jnt_wrenches,jnt_taugc);
        if (ret < 0){
            std::cout<<"CartToJnt Failed"<<std::endl;
            return -1;
        }else{
            for (int i = 0; i < mNumJnts; i++) {
            std::cout <<"torque " << i <<" "<<jnt_taugc(i)<<std::endl;
            }
            std::cout <<"angle----------- " <<angle<<std::endl;
        }
    }

    return 0;
}

CMakeLists.txt文件：

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(main)

include_directories(
  /opt/ros/kinetic/include
  /usr/include
  /usr/local/include
  /usr/include/eigen3
)

add_executable (main main.cpp)

find_library(KDL_LIB orocos-kdl /opt/ros/kinetic/lib/ )
find_library(KDL_PARSER_LIB kdl_parser /opt/ros/kinetic/lib/)
target_link_libraries(main
${KDL_LIB}
${KDL_PARSER_LIB}
)