Pinocchio十二讲（课程）

第一讲：Pinocchio简介以及安装

1.1、简介

pinocchio是一个机器人动力学的库，主要参考了Roy Featherstone大佬出版的Rigid Body Dynamics Algorithm。

A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives

!!! hint - 课程内容，暂不对外开放

1.2、Python版pinocchio库的安装

pinocchio侧重的动力学计算，不是仿真，利用meshcat是可以进行可视化的，但是和仿真还是有区别的

IDE：PyCharm 2024.3 (Community Edition) macOS 15.2

利用Pycharm新建工程，利用Conda新建虚拟环境（Learn_Pino_env）

直接在虚拟环境Learn_Pino_env中，利用conda命令安装conda install pinocchio -c conda-forge。

利用conda list <package_name>查看安装的库的信息

注意：在macOS中pip命令python -m pip install pin安装会不成功，因为Github上说明了pip安装方法只能在Linux中使用（实测，确实linux中利用pip安装是可以的，不过千万不要pip install pinocchio，否则会有问题），应该是：pip install pin。

然后就可以新建Python文件，导入pinocchio库，愉快的使用啦

但是，如果出现不可以正常索引，自动补全，通过urdf构建模型，这个model点索引没有正常显示，如下所示。

别急别急～

解决办法：pinocchio可能是动态库，使用类型注解就可以正常索引了

model: pin.Model = pin.buildModelFromUrdf(urdf_path)

类型注释（Python的一种语法）的说明

如果data = model.createData()之后，data也是不能自动索引补全，类似之前操作就可以data: pin.Data = model.createData()。

1.3、demo

测试代码：链接

import pinocchio

model = pinocchio.buildSampleModelManipulator()
data = model.createData()

q = pinocchio.neutral(model)
v = pinocchio.utils.zero(model.nv)
a = pinocchio.utils.zero(model.nv)

tau = pinocchio.rnea(model, data, q, v, a)
print("tau = ", tau.T)

demo：链接

git clone --recursive https://github.com/stack-of-tasks/pinocchio.git，clone下pinoochio的源码库。--recursive作用：会克隆主仓库及其子模块（仓库中包含对其他库的引用）并初始化。目前更推荐使用git clone --recurse-submodules。

更改一下model的路径，第7行

from pathlib import Path
from sys import argv
 
import pinocchio
 
# This path refers to Pinocchio source code but you can define your own directory here.
pinocchio_model_dir = Path(__file__).parent.parent / "pinocchio" /"models"
 
# You should change here to set up your own URDF file or just pass it as an argument of
# this example.
urdf_filename = (
    pinocchio_model_dir / "example-robot-data/robots/ur_description/urdf/ur5_robot.urdf"
    if len(argv) < 2
    else argv[1]
)
 
# Load the urdf model
model = pinocchio.buildModelFromUrdf(urdf_filename)
print("model name: " + model.name)
 
# Create data required by the algorithms
data = model.createData()
 
# Sample a random configuration
q = pinocchio.randomConfiguration(model)
print(f"q: {q.T}")
 
# Perform the forward kinematics over the kinematic tree
pinocchio.forwardKinematics(model, data, q)
 
# Print out the placement of each joint of the kinematic tree
for name, oMi in zip(model.names, data.oMi):
    print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat))

第二讲：Pinocchio整体架构

2.1、pinocchio可以实现的功能

可视化的功能（区别于simulation）

2.2、几点说明

A、First joint（universe）

一般关节数比自由度度多一个，因为First joint 是universe，其他一切的参考。
但是如果有Planar、Floating，就不一定了

B、pinocchio中支持的关节类型

链接。简单介绍。

Revolute joints, rotating around a fixed axis, either one of or a custom one;
Prismatic joints, translating along any fixed axis, as in the revolute case;
Spherical joints, free rotations in the 3D space;
Translation joints, for free translations in the 3D space;
Planar joints, for free movements in the 2D space;
Free-floating joints, for free movements in the 3D space. Planar and free-floating joints are meant to be employed as the basis of kinematic tree of mobile robots (humanoids, automated vehicles, or objects in manipulation planning).

Remark: In the URDF format, a joint of type fixed can be defined. However, a fixed joint is not really a joint because it cannot move. For efficiency reasons, it is therefore treated as operational frame of the model.

在urdf文件中定义fixed joint，此时不能使用model.getJointId和pin.getJointJacobian等，把固定关节视作frame，所以需要使用model.getFrameId和pin.getFrameJacobian，github的回复说明。

C、Planar、Free-floating类型关节的输入参数

（planar joint）利用planar类型关节建立urdf得到的模型，这个关节具有三个自由度，但是利用q0 = pin.neutral(model)得到的q0，这个关节有4个量。前两个是x、y值，第3、4个量是绕z轴旋转角度的余弦和正弦，所以，planar joint的输入参数是

# 输入的是x，y的位移、z轴转动角度的cos、sin。
# theta = -np.pi / 3
q = np.array([1, 1, np.cos(theta), np.sin(theta)])

（free-floating joint）的输入参数是xyzw

q = np.array([trans[0], trans[1], trans[2], quat.x, quat.y, quat.z, quat.w])

或者

pin.JointModelFreeFlyer() # q是7个量:前3位置;后4是四元数（x,y,z,w）
一般也不按照上面的直接在代码中添加，都是直接在urdf文件中定义floating类型的关节

在pinocchio中，参考，continue joint（ros中支持的关节类型）的输入参数是

continuous类型的关节输入是：cos(theta),sin(theta)

Pinocchio和mujoco中的四元数的顺序输入反过来的。在mujoco中，free joint的输入参数是wxyz：
joint_angles = [trans[0], trans[1], trans[2], quat.w, quat.x, quat.y, quat.z]
在Eigen中，四元数的顺序（更离谱）。四元数Quaterniond构造函数的参数为(w, x, y, z)，但在存储或调用::coeffs()返回其系数时使用(x, y, z, w)顺序。
Eigen::Quaterniond q(1.0, 0.0, 0.0, 0.0); //赋值
//Eigen中四元数赋值的顺序，实数w在首；
//但是实际上它的内部存储顺序是[x y z w]。实际上后面输出系数的时候也是按内部存储顺序输出

第三讲：可视化操作（✅）

理解、会用就行，pinocchio本身也不是做可视化的。

3.1、GepettoViewer

需要提前安装GepettoViewer。

conda install gepetto-viewer gepetto-viewer-corba -c conda-forge；

并且在Terminal启动gepetto-gui。

代码链接。

==以后在代码中直接添加的可视化代码==

根据需要，将想要加载的urdf文件修改正确的名字与路径。一般是直接加载pinocchio中的urdf文件。2024.11.29

from pathlib import Path
import sys
import pinocchio as pin
import numpy as np
from pinocchio.visualize import GepettoVisualizer

pinocchio_model_dir = Path(__file__).parent.parent / 'pinocchio' /"models"
model_path = pinocchio_model_dir / "example-robot-data/robots"
mesh_dir = pinocchio_model_dir
urdf_filename = "panda.urdf"
urdf_model_path = model_path / "panda_description/urdf" / urdf_filename
model, collision_model, visual_model = pin.buildModelsFromUrdf(
    urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()
)

# # 如果urdf文件中的mesh是绝对路径，下面方法更简单一些
# urdf_path = "../urdf/Franka_urdf/fr3_with_hand.urdf" # 正确的urdf路径
# model: pin.Model = pin.buildModelFromUrdf(urdf_path)
# visual_model = pin.buildGeomFromUrdf(model, urdf_path, pin.GeometryType.VISUAL)
# collision_model = pin.buildGeomFromUrdf(model, urdf_path, pin.GeometryType.COLLISION)

viz = GepettoVisualizer(model, collision_model, visual_model)

# Initialize the viewer.
try:
    viz.initViewer()
except ImportError as err:
    print(
        "Error while initializing the viewer. "
        "It seems you should install gepetto-viewer"
    )
    print(err)
    sys.exit(0)

try:
    viz.loadViewerModel("pinocchio")
except AttributeError as err:
    print(
        "Error while loading the viewer model. "
        "It seems you should start gepetto-viewer"
    )
    print(err)
    sys.exit(0)

# Display a robot configuration.
q0 = pin.neutral(model)
viz.display(q0)

3.2、Meshcat

==以后在代码中直接添加的可视化代码==

根据需要，将想要加载的urdf文件修改正确的名字与路径。一般是直接加载pinocchio中的urdf文件。2024.11.29

import pinocchio as pin
from pathlib import Path
import sys
from pinocchio.visualize import MeshcatVisualizer

pinocchio_model_dir = Path(__file__).parent.parent / 'pinocchio' /"models"
model_path = pinocchio_model_dir / "example-robot-data/robots"
mesh_dir = pinocchio_model_dir
urdf_filename = "panda.urdf"
urdf_model_path = model_path / "panda_description/urdf" / urdf_filename
model, collision_model, visual_model = pin.buildModelsFromUrdf(
    urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()
)

# # 如果urdf文件中的mesh是绝对路径，下面方法更简单一些
# urdf_path = "../urdf/Franka_urdf/fr3_with_hand.urdf" # 正确的urdf路径
# model: pin.Model = pin.buildModelFromUrdf(urdf_path)
# visual_model = pin.buildGeomFromUrdf(model, urdf_path, pin.GeometryType.VISUAL)
# collision_model = pin.buildGeomFromUrdf(model, urdf_path, pin.GeometryType.COLLISION)

try:
    viz = MeshcatVisualizer(model, collision_model, visual_model)
    viz.initViewer(open=True)
except ImportError as err:
    print(
        "Error while initializing the viewer. "
        "It seems you should install Python meshcat"
    )
    print(err)
    sys.exit(0)

# Load the robot in the viewer.
viz.loadViewerModel()

# Display a robot configuration.
q0 = pin.neutral(model)
viz.display(q0)
viz.displayVisuals(True)

input("Press Enter to continue...") # 没有这句话，显示就不对，很奇怪

第四讲：常用API

4.1、usual api

# 创建model、data
model:pin.Model = pin.buildModelFromUrdf(urdf_filename)
data:pin.Data = model.createData()
# 下面也是一种创建三种模型的方法，但是这个方法创建的model和model:pin.Model = pin.buildModelFromUrdf(urdf_filename)有点不太一样
model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()
# 还有下面两种，2024.12.14
pin.buildModelFromXML()
pin.buildModelFromMJCF()                                                   
                                                               
# 获得模型的自由度数
model.nq
model.nv
# nq和nv可能会不一致，对于floating关节类型，就会不一致。

# 得到关节的id
Joint_id = model.getJointId("xxx") 
# 得到frame的id，也可以是固定关节（认为是frame）
Frame_id = model.getFrameId("xxx")
# 得到body的id
body_id= model.getBodyId("xxx")

# 生成自然状态下
q0 = pin.neutral(model) 
# 生成随机状态的关节角度
q_random = pin.randomConfiguration(model) 
# 获得全0的速度、加速度
v = pinocchio.utils.zero(model.nv)
a = pinocchio.utils.zero(model.nv)

# 计算正运动学
pin.forwardKinematics(model, data, q) # 优先用这个
# pin.forwardKinematics(model, data, q, v)
# pin.forwardKinematics(model, data, q, v, a)

# Updates the position of each frame contained in the model.
pin.updateFramePlacements(model, data) # 优先用这个
# Updates the placement of the given frame.
pin.updateFramePlacements(model, data, frame_id)

pin.framesForwardKinematics

joint_pose:pin.pinocchio_pywrap_default.SE3 = data.oMi[joint_id]
frame_pose:pin.pinocchio_pywrap_default.SE3 = data.oMf[frame_id] 
# 上述的pose类型是：pinocchio.pinocchio_pywrap_default.SE3
# 可用joint_pose.rotation得到旋转部分,joint_pose.translation得到平移部分
final_joint_pose = data.oMi[-1] # 最后一个关节的pose

# 计算、获的joint的jacobian
J = pin.computeJointJacobian(model,data,q,joint_id)


# 计算、获得frame的jacobian
# Computes the Jacobian of a specific Frame expressed in the LOCAL frame coordinate system.
J = computeFrameJacobian(model,data,q,frame_id)
# in the desired reference_frame given as argument.
J = computeFrameJacobian(model,data,q,frame_id,reference_frame)

pin.computeJointJacobians(model,data)
pin.computeJointJacobians(model,data,q) # 优先用这个
Jacobian = data.J
J = pin.getFrameJacobian(model,data,frame_id,reference_frame)
J = pin.getJointJacobian(model,data,joint_id,reference_frame)

# 计算动力学
pin.forwardDynamics()

pin.crba()

# 计算动力学
tau = pin.rnea(model, data, q, v, a)

pin.computeCollision()
pin.computeCollisions()

pin.updateGeometryPlacements()
pin.updateFramePlacement()
pin.updateFramePlacements()
pin.updateGlobalPlacements()

pin.computeAllTerms
pin.ContactCholeskyDecomposition
pin.integrate
pin.SE3.Identity()

data.com[0]

4.2、Reference Frame

pin.LOCAL_WORLD_ALIGNED
pin.WORLD
pin.LOCAL

pin.LOCAL_WORLD_ALIGNED表示，想要计算ID相对world坐标系

pin.LOCAL表示，想要计算ID相对自己当下局部坐标系（不再是0 joint）

第五讲：正逆运动学等基本函数使用

以Franka为例。利用pinocchio源码库中的代码：/pinocchio/models/example-robot-data/robots/panda_description/urdf/panda.urdf，利用mechcat进行可视化

获取JointId、FrameId

# 得到第7个关节的id
Joint7 = model.getJointId("fr3_joint7") 
# 第8个固定关节（认为是frame）的id
end_frame = model.getFrameId("fr3_joint8")  
# 得到末端tcp的位置，也是frame
tcp_frame_id = model.getFrameId("fr3_hand_tcp_joint")

5.1、FK

正运动学

# 假设franka：7+2个自由度
theta = -np.pi / 3
q0 = np.array([1, 1, np.cos(theta), np.sin(theta),
               0, 0, 0, -np.pi / 2, 0, np.pi / 2, np.pi / 4,
               0, 0])
# 得到关节的id
Joint7_joint_id = model.getJointId("fr3_joint7")
# 得到一个固定关节（认为是frame）的id
Joint8_frame_id = model.getFrameId("fr3_joint8")
  
pin.forwardKinematics(model, data, q0)
pose = data.oMi[-1]  # 最后一个关节,可以把-1换成某个关节的Id，如Joint7_joint_id
print("pose:", pose)

注意：获取Frame（如Joint8_frame_id）的位姿，还需要在正运动学之后加上pin.updateFramePlacements(model, data)，并且用data.oMf[Joint8_frame_id]获取。

也可以按照下面方式显示，定义model、data、计算正运动学forwardKinematics(model, data, q0)，Then

# Print out the placement of each joint of the kinematic tree
for name, oMi in zip(model.names, data.oMi):
    # *oMi.translation.T.flat展开位姿矩阵的平移部分（即关节的世界坐标位置）
    print(("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat)))

结果：（ur5，随机关节角度)

5.2、IK

逆动力学,有一个inverse kinematics(clik)

This example shows how to position the end effector of a manipulator robot to a given pose (position and orientation). The example employs a simple Jacobian-based iterative algorithm, which is called closed-loop inverse kinematics (CLIK).

第六讲：微分运动学等函数使用

6.1、理论分析

6.2、Jacobian代码讲解

计算雅可比矩阵

# 计算所有关节的雅可比矩阵，并且更新模型数据data结构中与每个关节相关的雅可比矩阵信息。in the world frame
pin.computeJointJacobians(model, data, q_random) # 必须先有这个，才能get
print("date.J: \n", data.J)  # data.J默认是最后一个关节相对于0关节的Jacobian。
# The result is accessible through data.J. 
# 这个函数也自动计算了forwardKinematics
# pin.forwardKinematics(model, data, q_random)，这个就不用写了

Joint7_jacobian = pin.getJointJacobian(model, data, Joint7, pin.WORLD)
end_jacobian = pin.getFrameJacobian(model, data, end_frame, pin.WORLD)
# 刚才的data.J和Joint7的jacobian结果是一样的
# 想要get，必须有前面的pin.computeJointJacobians，否则不报错，但结果都是0

思考：为啥Joint7_jacobian和end_jacobian也一样？？？

next
```
J7_jac = pin.computeJointJacobian(model, data, q, J7_id) 
```
只有一种使用方法，输入也没有ReferenceFrame，in the local frame of the joint
```
end_jac = pin.computeFrameJacobian(model, data, q, end_frame_id)
end_jac = pin.computeFrameJacobian(model, data, q, end_frame_id, reference_frame)
```
两种用法：

1、四个参数输入时：Computes the Jacobian of a specific Frame expressed in the LOCAL frame coordinate system.

2、五个参数输入时：在上面基础上，加上一个reference_frame的输入：in the desired reference_frame given as argument.

总结：上述这两个直接计算，相当于下面两个的组合：但是需要注意坐标系的问题。

computeJointJacobians + getJointJacobian

computeJointJacobians + getFrameJacobian

第七讲：动力学等函数使用

7.1、ID（inverse dynamics）

位置、速度、加速度、（外力）$\to$力矩

7.2、FD（forward dynamics）

力矩、位置、速度$\to$加速度

动力学参数获取（）

正逆动力学

逆动力学

tau = pinocchio.rnea(model, data, q, v, a)
print('tau = ', tau.T)

第八讲：(高级) urdf分析

前言：可能pycharm中不支持urdf的高亮显示，需要根据如下步骤，添加*.urdf即可

8.1、urdf介绍

介绍：todo

可以利用urdfpy，帮助理解urdf文件。安装：pip install urdfpy

<?xml version="1.0" ?>

<robot name="fr3">

  <link name="world"/>

  <link name="base">
    <inertial>
      <origin xyz="-0.010475 0 -0.05266" rpy="0 0 0" />
      <mass value="96" />
      <inertia ixx="5.08001" ixy="0.00112" ixz="0.17291" iyy="9.68153" iyz="0.00137" izz="10.41607" />
    </inertial>
    <visual>
      <geometry>
        <!-- <box size="0.5 0.5 0.3"/> -->
      </geometry>
    </visual>
    <collision>
      <geometry>
        <!-- <box size="0.5 0.5 0.3"/> -->
      </geometry>
    </collision>
  </link>

  <joint name="base_planar_joint" type="floating">
    <origin xyz="0 0 0.30305" rpy="0 0 0"/>
    <parent link="world"/>
    <child link="base"/>
  </joint>

  <link name="fr3_link0">
    <inertial>
      <origin xyz="-0.041018 -0.00014 0.049974" rpy="0 0 0" />
      <mass value="0.629769" />
      <inertia ixx="0.00315" ixy="8.2904E-07" ixz="0.00015" iyy="0.00388" iyz="8.2299E-06" izz="0.004285" />
    </inertial>
    <visual>
      <geometry>
        <mesh filename="xxxxxx/meshes/visual/link0.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="xxxxxx/meshes/collision/link0.stl"/>
      </geometry>
    </collision>
  </link>

  <joint name="base_link0_joint" type="fixed">
    <origin xyz="-0.175 0 0.06" rpy="0 0 0"/>
    <parent link="base"/>
    <child link="fr3_link0"/>
  </joint> 
  ...

8.2、urdf中坐标系关系

link

对于link中的inertial中的origin、mass、inertia这三个属性

origin可以理解为质心的位置，这个质心位置是相对于连杆的原点（如果是导入的模型，一般有一个原点坐标系，类似于在这个原点坐标系下开始建立模型；如果是利用box标签，生成简单的几何图形，对于box，对于长方体的中心。）

mass是质量

inertia是在质心坐标系下计算的惯性矩阵

来源：https://wiki.ros.org/urdf/XML/link

joint

对于joint标签，主要是origin、parent、child等；

origin是父连杆到子连杆的变化（the joint is located at the origin of the child link）。关节坐标和子连杆的原点重合。

来源：https://wiki.ros.org/urdf/XML/joint

8.3、urdf实例解析

第九讲：(高级) mobile manipulator的建模

9.1、关节类型说明与使用

1、在Pinocchio中有相应类型的关节，参考Supported kinematic models。

==Planer joint==，for free translations in the 2D space。

==Free-floating joints==，for free movements in the 3D space, Planar and free-floating joints are meant to be employed as the basis of kinematic tree of mobile robots(humanoids,automated vehicles,or objects in manipulation planning).
2、在urdf文件中，也有对应的关节类型，Planar类型 / floating类型，参考。

==planar== — this joint allows motion in a plane perpendicular to the axis

==floating== — this joint allows motion for all 6 degrees of freedom.

9.2、planar joint的MM的建模分析

9.3、floating joint的MM建模分析

第十讲：(高级) pinocchio和CoppeliaSim联合调试

10.1、在coppeliasim中建立MM平台

结合之前的防倾覆的，搞一下coppeliasim和python的远程调试。搞一下工程，放在第九讲中。

10.2、利用pinocchio建模与控制（python版）

第十一讲：(高级)

11.1

11.2

第十二讲：(高级) demo讲解

12.1

12.2

VSCode上用C++库（deprecated？）

注：参考xcode那个md文章，

做不做再说。2024.11.20

新建VSCode工程：新建一个文件夹，用vscode打开，然后在配置c_cpp_properties.json文件，如果没有这个文件，利用下面方法生成。

触发c_cpp_properties.json的生成：

通过打开一个C或C++文件（比如你的main.cpp）。
然后，打开命令面板（Cmd+Shift+P on macOS, Ctrl+Shift+P on Windows/Linux）。
输入并选择“C/C++: Edit Configurations (UI)”命令。

修改内容

重点修改了c_cpp_properties.json、tasks.json这两个文件，具体见VSCode中的First_Project_Learn_Pin工程。==（后面还得继续修改）==

编译调试

修改完配置文件以后，在main中编写对应的代码。

Shift+Cmd+B：编译、运行（尽量不要点击IDE的运行按钮）

C++ 索引自动补全问题（`Eigen`）

在VSCode中使用Eigen库，在.vscode中设置了配置环境

但是在main.cpp中，如果不使用命名空间，使用完全限定名，索引不出来我想要的接口

:smile:相反，使用命令空间，在main中就可以了（就有VectorXd了），很奇怪？

Franka的urdf文件
Franka的urdf文件（带夹爪）
MR2000+Franka的urdf文件
建立一个planar关节 + Franka的urdf机器人模型（利用pinocchio计算所有需要的模型信息）
- 目前的urdf文件，导入CoppeliaSim中，第五轴显示不太对感觉？但是在pinocchio中显示好像就可以？（==解决：CoppeliaSim中的关节限位问题==）已经将mac上目前所有的urdf文件都进行了修改。
- 目前存在一个可视化的问题，桌面上修改的urdf文件，显示出现了问题，不知道原因？代码、命令、注释、路径基本都排查了，不过感觉还是有问题，可能还是urdf文件的问题。2024.4.28
- [ ]
基于MR2000，建立Free-floating关节，+Franka的urdf模型。

可视化基本流程

Haitao Liu