安装

conda create -n pinocchio python=3.10
conda install pinocchio -c conda-forge
pip install meshcat
conda install matplotlib

必要的库

import pinocchio as pin
from pinocchio.utils import *

机器人模型

pinocchio通常只处理开环机器人。要用pinocchio处理闭环机器人，以及多机器人协作时的运动学，可以参考该链接

读取urdf文件

# 定义初始关节角姿态
initialJointConfig = np.array([0, 0, 0, 1, 1, 1,])

# 定义要锁死的关节
jointsToLock = ["wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]
 
# 获得要锁死的关节ID
jointsToLockIDs = []
for jn in jointsToLock:
    if model.existJointName(jn):
        jointsToLockIDs.append(model.getJointId(jn))
    else:
        print("Warning: joint " + str(jn) + " does not belong to the model!")

# 从urdf文件中读取机器人模型
model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_filename, [base_path])

# 构建一个降阶机器人模型，只包含某些关节，jointsToLockIDs为关节ID列表
model_reduced, visual_model_reduced = pin.buildReducedModel(
    model, visual_model, jointsToLockIDs, initialJointConfig
)

# 利用RobotWrapper高级封装类构建机器人对象，其中不仅有model还有data
robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, [base_path])
model = robot.model
data = robot.data

# 基于RobotWrapper高级封装类构建reduced模型
reduced_robot = robot.buildReducedRobot(
    list_of_joints_to_lock=mixed_jointsToLockIDs,
    reference_configuration=initialJointConfig,
)

查看URDF文件

sudo apt install liburdfdom-tools
check_urdf urdf_filename
urdf_to_graphiz urdf_filename

李群李代数

李群进行对数映射，就会变成李代数。

李代数进行指数映射，就会变成李群。

李代数是一个线性向量空间，李群是一个矩阵空间

姿态，SE3，特殊欧几里德群

R = eye(3)
p = zero(3)
M0 = pin.SE3(R, p) # 直接给定旋转矩阵和平移向量来定义SE3矩阵
M1 = pin.SE3.Random() # 随机生成一个SE3矩阵
M1.translation = p # 单独设置平移向量
M1.rotation = R # 单独设置旋转矩阵

速度，se3，六维向量，李代数

v = zero(3)
w = zero(3)
nu0 = pin.Motion(v, w)
nu1 = pin.Motion.Random()
nu1.linear = v
nu1.angular = w

力，se3,六维向量，李代数

f = zero(3)
tau = zero(3)
phi0 = pin.Force(f, tau)
phi1 = pin.Force.Random()
phi1.linear = f
phi1.angular = tau

Inverse Kinematics (轨迹)

原理讲解

这里主要有两种方案

方案1

第一种方案是将轨迹分割成许多个点，然后让机器人依次对每个点进行单点IK求解，这里需要嵌套循环。这里需要指出的是，最好应该是先计算好关节轨迹，然后再运动。不要实时计算。

参考代码片段如下

# 设置effector目标位置和姿态，并进行IK求解，注意速度要保持均匀

t = 0.0
dt = 0.01
qlist = []
ypos_v = 0.5
ypos_des = 0.3

it_num_max = math.floor(ypos_des / (ypos_v * dt))

for it_num in range(it_num_max):
    tic = time.time() # 计时开始
    ypos = it_num*ypos_v*dt
    
    T_t = pin.SE3(np.eye(3), np.array([0.5, ypos, 0.0])) # 确定目标位置和姿态
    homo_matrix = T_t.homogeneous
    ############################################################
    # 创建一个box位于目标位置，方便查看
    box_name = "world/box"
    box_size = [0.04, 0.02, 0.01]

    viz.viewer[box_name].set_object(
        meshcat.geometry.Box(box_size),
        meshcat.geometry.MeshLambertMaterial(color=0x6622cc,reflectivity=0.4)
    )

    viz.viewer[box_name].set_transform(
        homo_matrix
    )


    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # 开始进行逆运动学求解
    i = 0
    
    while True:
        T_bt = robot.data.oMi[idx].actInv(T_t)
        J_b = pin.computeJointJacobian(robot.model, robot.data, q, idx)
        J_l = -pin.Jlog6(T_bt.inverse())

        # 接下来会这里会利用J_error v^{*} = -e(q)来求v^{*}
        J_error = J_l.dot(J_b)
        e_q = pin.log(T_bt).vector # 定义的误差向量

        # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
        v_star = pinv(J_error).dot(-e_q) # 此处是求解的关键，这里直接用pinv，没有借用damping项
        # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
        q = pin.integrate(robot.model, q, v_star*dt_ik)
        
        pin.forwardKinematics(robot.model, robot.data, q) # 更新位置，这个一定不能忘


        if np.linalg.norm(e_q) < 1e-4:
            success = True
            break
        if i>IT_MAX:
            success = False
            break
        i += 1

    qlist.append(q)# 记录每次收敛得到的q
    if success:
        print("Convergence achieved!")
    else:
        print("Convergence failed!")

    
    toc = time.time()
    elapsed = toc - tic
    dt_sleep = max(0, dt - (elapsed))
    time.sleep(dt_sleep)

# <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
vqmax = 0.8
dt_inter_pos = 0.01
qlist_inter = InterPos(q0, qlist, vqmax, dt_inter_pos)


for q in qlist_inter:
    tic = time.time() # 计时开始
    pin.forwardKinematics(robot.model, robot.data, q) # 更新位置，这个一定不能忘
    viz.display(q)
    toc = time.time()
    elapsed = toc - tic
    dt_sleep = max(0, dt_inter_pos - (elapsed))
    time.sleep(dt_sleep)

方案2

第二种是：

Direct and Inverse Dynamics

安装二次规划库

pip install quadprog

使用例程

#Solve a strictly convex quadratic program
#Minimize     1/2 x^T G x - a^T x
#Subject to   C.T x >= b
x, f, xu, iterations, lagrangian, iact = solve_qp(G, a, C=None, b=None, meq=0, factorized=False)

Direct Dynamics

参考代码片段如下：

import math
import time

dt = 0.001

starttime = time.time()
for i in range(10000):

    # 设置阻尼力矩
    t_q = -0.5*vq
    # t_q += 10*math.sin(time.time()-starttime)


    # 机器人动力学方程为Ma_q + b = \tau _q, 此处b包含了Coriolis, centrifugal, gravity
    # rnea是recursive Newton Euler Algorithm的简写，即递归牛顿欧拉算法
    b = pin.rnea(robot.model, robot.data, q, vq, aq)


    # crba是专门计算机器人质量矩阵M的函数
    M = pin.crba(robot.model, robot.data, q)

    # 求aq， 不要用np.linalg.solve， 经常会不收敛
    aq = pin.aba(robot.model, robot.data, q, vq, t_q)
    vq += aq*dt
    # 求解新位置
    q = pin.integrate(robot.model, q, vq*dt)
    # 更新位置，这个一定不能忘
    pin.forwardKinematics(robot.model, robot.data, q)
    
    # 更新显示
    viz.display(q)