Skip to content
Snippets Groups Projects
kdl-test.cpp 1.54 KiB
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>
void kdl_test(std::string model)
  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);
Guilhem Saurel's avatar
Guilhem Saurel committed
  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());
Guilhem Saurel's avatar
Guilhem Saurel committed
  KDL::Wrenches f(chain.getNrOfSegments());

  KDL::Vector g_v(0, 0, -9.81);
  KDL::Twist g_t(g_v, KDL::Vector::Zero());

  unsigned int nr_of_constraints = 4;
Guilhem Saurel's avatar
Guilhem Saurel committed
  KDL::Jacobian alpha(nr_of_constraints);
  KDL::JntArray beta(nr_of_constraints);
Guilhem Saurel's avatar
Guilhem Saurel committed
  //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);
Guilhem Saurel's avatar
Guilhem Saurel committed
  std::cout << rnea_solver.CartToJnt(q, qdot, qddot, f, tau) << std::endl;;
  std::cout << "ID: tau after RNEA: " << tau.data.transpose() << std::endl;