Newer
Older
#include <iostream>
#include <fstream>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <kdl/chainidsolver_vereshchagin.hpp>
#include "models.h"
std::cout << "KDL Test" << std::endl;
std::cout << " model: " << model << std::endl;
KDL::Chain chain;
kdl_parser::treeFromFile(pinocchio_benchmarks::path + model + ".urdf", tree);
tree.getChain("world_link", "lwr_arm_7_link", chain);
std::cout << " ns: " << chain.getNrOfSegments() << std::endl;
std::cout << " nj: " << chain.getNrOfJoints() << std::endl;
KDL::JntArray q(chain.getNrOfJoints());
KDL::JntArray qdot(chain.getNrOfJoints());
KDL::JntArray qddot(chain.getNrOfJoints());
KDL::JntArray tau(chain.getNrOfJoints());
KDL::Vector g_v(0, 0, -9.81);
KDL::Twist g_t(g_v, KDL::Vector::Zero());
unsigned int nr_of_constraints = 4;
KDL::Jacobian alpha(nr_of_constraints);
KDL::JntArray beta(nr_of_constraints);
//KDL::ChainIdSolver_Vereshchagin vc_solver(chain, g_t, nr_of_constraints);
//std::cout << vc_solver.CartToJnt(q, qdot, qddot, alpha, beta, f, tau) << std::endl;;
//std::cout << "HD: qddot after VC: " << qddot.data.transpose() << std::endl;
KDL::ChainIdSolver_RNE rnea_solver(chain, g_v);
std::cout << rnea_solver.CartToJnt(q, qdot, qddot, f, tau) << std::endl;;
std::cout << "ID: tau after RNEA: " << tau.data.transpose() << std::endl;
}
int main()
{
kdl_test("lwr");