当前位置：首页 > news >正文

Pinocchio中data、model接口介绍

news 来源：原创 2025/5/21 17:33:42

视频讲解：

今天介绍下 pinocchio 中 data、model 常用接口

model 用于存储机器人模型的静态信息，如关节结构、惯性参数等

data 用于存储机器人在仿真过程中的动态信息，如关节位姿、雅可比矩阵、惯性矩阵等

model.nq 关节数量

model.nv 速度向量的维度

print(f"Number of configuration variables (nq): {model.nq}")
print(f"Number of velocity variables (nv): {model.nv}")

model.names 包含所有关节名称的列表

for i, joint in enumerate(model.joints):
    print(f"Joint {i}:")
    print(f"  Name: {model.names[i]}")

lowerPositionLimit 关节位置的下限

upperPositionLimit 上限，用于限制关节的运动范围

lower_limits = model.lowerPositionLimit
upper_limits = model.upperPositionLimit
print("Lower limits:", lower_limits)
print("Upper limits:", upper_limits)

model.inertias 每个刚体的惯性参数，如质量、质心位置和惯性张量

for i, inertia in enumerate(model.inertias):
    print(f"Inertia {i}:")
    print(f"  Mass: {inertia.mass}")
    print(f"  Center of Mass: {inertia.lever}")
    print(f"  Inertia Matrix:\n{inertia.inertia}")

model.jointPlacements 每个关节相对于其父关节的初始位置和姿态

for placement in model.jointPlacements:
    print(f"Translation: {placement.translation}")
    print(f"Rotation Matrix:\n{placement.rotation}")

以panda机械臂为例，完整代码

import pinocchio as pin
import numpy as np

# 加载 URDF 文件
urdf_path = "/home/dar/MuJoCoBin/mujoco-learning/franka_panda_description/robots/panda_arm.urdf"
model = pin.buildModelFromUrdf(urdf_path)
data = model.createData()

# 获取关节限位
lower_limits = model.lowerPositionLimit
upper_limits = model.upperPositionLimit
# 打印关节限位
print("Lower limits:", lower_limits)
print("Upper limits:", upper_limits)

# 打印配置向量和速度向量的维度
print(f"Number of configuration variables (nq): {model.nq}")
print(f"Number of velocity variables (nv): {model.nv}")

# 计算正运动学
q = np.zeros(model.nq)  # 初始关节配置
data = model.createData()
pin.framesForwardKinematics(model, data, q)

# 打印 joints 信息
for i, joint in enumerate(model.joints):
    print(f"Joint {i}:")
    print(f"  Name: {model.names[i]}")

# 打印 jointPlacements 信息
print("\nJoint Placements Information:")
for i, placement in enumerate(model.jointPlacements):
    print(f"Joint Placement {i}:")
    print(f"  Translation: {placement.translation}")
    print(f"  Rotation Matrix:\n{placement.rotation}")

# 打印 inertias 信息
print("\nInertias Information:")
for i, inertia in enumerate(model.inertias):
    print(f"Inertia {i}:")
    print(f"  Mass: {inertia.mass}")
    print(f"  Center of Mass: {inertia.lever}")
    print(f"  Inertia Matrix:\n{inertia.inertia}")

结果输出

Lower limits: [-2.8973 -1.7628 -2.8973 -3.0718 -2.8973 -0.0175 -2.8973]
Upper limits: [ 2.8973  1.7628  2.8973 -0.0698  2.8973  3.7525  2.8973]
Number of configuration variables (nq): 7
Number of velocity variables (nv): 7
Joint 0:
  Name: universe
Joint 1:
  Name: panda_joint1
Joint 2:
  Name: panda_joint2
Joint 3:
  Name: panda_joint3
Joint 4:
  Name: panda_joint4
Joint 5:
  Name: panda_joint5
Joint 6:
  Name: panda_joint6
Joint 7:
  Name: panda_joint7

Joint Placements Information:
Joint Placement 0:
  Translation: [0. 0. 0.]
  Rotation Matrix:
[[1. 0. 0.]
 [0. 1. 0.]
 [0. 0. 1.]]
Joint Placement 1:
  Translation: [0.    0.    0.333]
  Rotation Matrix:
[[1. 0. 0.]
 [0. 1. 0.]
 [0. 0. 1.]]
Joint Placement 2:
  Translation: [0. 0. 0.]
  Rotation Matrix:
[[ 1.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  4.89663865e-12  1.00000000e+00]
 [ 0.00000000e+00 -1.00000000e+00  4.89663865e-12]]
Joint Placement 3:
  Translation: [ 0.    -0.316  0.   ]
  Rotation Matrix:
[[ 1.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  4.89663865e-12 -1.00000000e+00]
 [ 0.00000000e+00  1.00000000e+00  4.89663865e-12]]
Joint Placement 4:
  Translation: [0.0825 0.     0.    ]
  Rotation Matrix:
[[ 1.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  4.89663865e-12 -1.00000000e+00]
 [ 0.00000000e+00  1.00000000e+00  4.89663865e-12]]
Joint Placement 5:
  Translation: [-0.0825  0.384   0.    ]
  Rotation Matrix:
[[ 1.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  4.89663865e-12  1.00000000e+00]
 [ 0.00000000e+00 -1.00000000e+00  4.89663865e-12]]
Joint Placement 6:
  Translation: [0. 0. 0.]
  Rotation Matrix:
[[ 1.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  4.89663865e-12 -1.00000000e+00]
 [ 0.00000000e+00  1.00000000e+00  4.89663865e-12]]
Joint Placement 7:
  Translation: [0.088 0.    0.   ]
  Rotation Matrix:
[[ 1.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  4.89663865e-12 -1.00000000e+00]
 [ 0.00000000e+00  1.00000000e+00  4.89663865e-12]]

Inertias Information:
Inertia 0:
  Mass: 0.0
  Center of Mass: [0. 0. 0.]
  Inertia Matrix:
[[0. 0. 0.]
 [0. 0. 0.]
 [0. 0. 0.]]
Inertia 1:
  Mass: 4.970684
  Center of Mass: [ 0.003875  0.002081 -0.175   ]
  Inertia Matrix:
[[ 7.0337e-01 -1.3900e-04  6.7720e-03]
 [-1.3900e-04  7.0661e-01  1.9169e-02]
 [ 6.7720e-03  1.9169e-02  9.1170e-03]]
Inertia 2:
  Mass: 0.646926
  Center of Mass: [-0.003141 -0.02872   0.003495]
  Inertia Matrix:
[[ 0.007962 -0.003925  0.010254]
 [-0.003925  0.02811   0.000704]
 [ 0.010254  0.000704  0.025995]]
Inertia 3:
  Mass: 3.228604
  Center of Mass: [ 0.027518  0.039252 -0.066502]
  Inertia Matrix:
[[ 0.037242 -0.004761 -0.011396]
 [-0.004761  0.036155 -0.012805]
 [-0.011396 -0.012805  0.01083 ]]
Inertia 4:
  Mass: 3.587895
  Center of Mass: [-0.05317   0.104419  0.027454]
  Inertia Matrix:
[[ 0.025853  0.007796 -0.001332]
 [ 0.007796  0.019552  0.008641]
 [-0.001332  0.008641  0.028323]]
Inertia 5:
  Mass: 1.225946
  Center of Mass: [-0.011953  0.041065 -0.038437]
  Inertia Matrix:
[[ 0.035549 -0.002117 -0.004037]
 [-0.002117  0.029474  0.000229]
 [-0.004037  0.000229  0.008627]]
Inertia 6:
  Mass: 1.666555
  Center of Mass: [ 0.060149 -0.014117 -0.010517]
  Inertia Matrix:
[[ 0.001964  0.000109 -0.001158]
 [ 0.000109  0.004354  0.000341]
 [-0.001158  0.000341  0.005433]]
Inertia 7:
  Mass: 0.735522
  Center of Mass: [ 0.010517 -0.004252  0.061597]
  Inertia Matrix:
[[ 0.013516 -0.000428 -0.001196]
 [-0.000428  0.011027 -0.000741]
 [-0.001196 -0.000741  0.005815]]