机器人动力学库Pinocchio的安装与使用

官方文档地址https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/index.html

本例程系统为Ubuntu22.04

在ROS中安装相对简单，比如我的ROS版本为humble，则直接输入

sudo apt install ros-humble-pinocchio

如果只想用Python来调用Pinocchio则只需要以下指令

conda install pinocchio -c conda-forge
#或者pip install pin也可以

如果不想用ROS，也不想直接用Python Conda，那么可以接着看下去

pinocchio使用了名为robotpkg的包管理系统来打包并处理依赖，官网如下

http://robotpkg.openrobots.org/

要使用robotpkg安装pinocchio，我们首先要添加robotpkg的软件源

首先确认安装了必要依赖

sudo apt install -qqy lsb-release curl

第二步，系统中注册robotpkg

sudo mkdir -p /etc/apt/keyrings
 curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc \
     | sudo tee /etc/apt/keyrings/robotpkg.asc

第三步，把robotpkg添加到apt源码库中

echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" \
     | sudo tee /etc/apt/sources.list.d/robotpkg.list

第四步，更新软件列表

sudo apt update

至此，我们直接使用以下指令安装pinocchio

sudo apt install -qqy robotpkg-py3*-pinocchio

包的安装位置为/opt/openrobots

最后添加环境变量，在~/.bashrc中添加如下即可

export PATH=/opt/openrobots/bin:$PATH
export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH
export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH
export PYTHONPATH=/opt/openrobots/lib/python3.10/site-packages:$PYTHONPATH # Adapt your desired python version here
export CMAKE_PREFIX_PATH=/opt/openrobots:$CMAKE_PREFIX_PATH

之后我们可以在工作空间写一个如下示例程序，并命名为overview-simple.cpp

#include "pinocchio/parsers/urdf.hpp"
 
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
 
#include <iostream>
 
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef PINOCCHIO_MODEL_DIR
  #define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif
 
int main(int argc, char ** argv)
{
  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("/example-robot-data/robots/ur_description/urdf/ur5_robot.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);
  std::cout << "q: " << q.transpose() << std::endl;
 
  // Perform the forward kinematics over the kinematic tree
  forwardKinematics(model,data,q);
 
  // Print out the placement of each joint of the kinematic tree
  for(JointIndex joint_id = 0; joint_id < (JointIndex)model.njoints; ++joint_id)
    std::cout << std::setw(24) << std::left
              << model.names[joint_id] << ": "
              << std::fixed << std::setprecision(2)
              << data.oMi[joint_id].translation().transpose()
              << std::endl;
}

之后编译，这里使用pkg-config自动获取了库文件和头文件的地址

g++ -std=c++11 overview-simple.cpp -o overview-simple $(pkg-config --cflags --libs pinocchio)

之后运行可执行文件overview-example并写上模型文件的路径，此处给出示例命令与结果，需要根据自己的模型文件路径进行调整

~/ws/pinocchio$ ./overview-simple ../pinocchio-2.7.1/models/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf 
model name: ur5
q:  4.27492 -1.32722  1.77876  3.75031  5.17291 -3.80068
universe                : 0.00 0.00 0.00
shoulder_pan_joint      : 0.00 0.00 0.09
shoulder_lift_joint     : 0.12  -0.06 0.09 
elbow_joint             : -0.03 -0.10 0.50 
wrist_1_joint           : -0.18 -0.42 0.33 
wrist_2_joint           : -0.09 -0.46 0.33 
wrist_3_joint           : -0.13 -0.53 0.38

 #注：目前robotpkg库中似乎只有amd64架构的包，这种添加robotpkg的方法，个人测试无法在rk3588和Ascend310的开发板上安装pinocchio。我最终是使用conda在orangepi上安装了pinocchio，但是似乎如此便只能使用python脚本