动力学库-pinocchio的C++例程

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include <iostream>
#include <ros/ros.h>
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Dense"
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef PINOCCHIO_MODEL_DIR
  #define PINOCCHIO_MODEL_DIR "/home/xxx/workspace/xxx/src/xxxxx_urdf"
#endif
int main(int argc, char **argv)
{
   
  ros::init(argc, argv, "Pinocchio_test");
  ros::NodeHandle nh;
  using namespace pinocchio;

  // You should change here to set up your own URDF file or just pass it as an argument of this example.
  const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/urdf/xxx_urdf.urdf") : argv[1];

  // Load the urdf model
  Model model;
  pinocchio::urdf::buildModel(urdf_filename,model);
  std::cout << "model name: " << model.name << std::endl;

  // Create data required by the algorithms
  Data data(model);

  // Sample a random configuration
//  Eigen::VectorXd q= randomConfiguration(model);
  Eigen::VectorXd q(model.nq) ,v(model.nv),a(model.nv),tau(model.nq);
  Eigen::Matrix<double,6,4> J1,J2,dJ;//the model has 4 joints
  q<<0,0,0,0;
  v<<0,0.1,0.1,0;
  a<<0,0,0,0;
  J1<<0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
  J2=J1;
  dJ=J1;
  std::cout << "q: " << q.transpose() << std::endl;
  std::cout << "v: " << v.t