devbox
IT小白
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

Pybullet&Pinocchio(python robot)-note_动力学库pinocchio的 python例程

作者：IT小白 | 2024-06-30 15:49:50

赞

踩

动力学库pinocchio的 python例程

机器人算法前置工作：

PyBullet 基于开源物理引擎 bullet 开发，封装成了 Python 的一个模块，用于机器人仿真和学习

pybullet中文手册（一） - 灰信网（软件开发博客聚合） (freesion.com)

Pinocchio是常用的机器人动力学库

pinocchio: Overview (laas.fr)

1.pybullet

安装：pip install pybullet

流程：

引入模块

初始化pybullet的环境

创建环境和机器人

设置物理引擎参数

运行主循环

结束pybullet环境 

最简单的例子引入：

  1. import pybullet as p
  2. import time
  3. import pybullet_data
  4.  
  5. # 连接物理引擎
  6. physicsCilent = p.connect(p.GUI)
  7.  
  8. # 添加资源路径
  9. p.setAdditionalSearchPath(pybullet_data.getDataPath())
  10.  
  11. # 设置环境重力加速度
  12. p.setGravity(0, 0, -10)
  13.  
  14. # 加载URDF模型，此处是加载蓝白相间的陆地
  15. planeId = p.loadURDF("plane.urdf")
  16.  
  17. # 加载机器人，并设置加载的机器人的位姿
  18. startPos = [0, 0, 1]
  19. startOrientation = p.getQuaternionFromEuler([0, 0, 0])
  20. boxId = p.loadURDF("r2d2.urdf", startPos, startOrientation)
  21.  
  22. # 按照位置和朝向重置机器人的位姿，由于我们之前已经初始化了机器人，所以此处加不加这句话没什么影响
  23. p.resetBasePositionAndOrientation(boxId, startPos, startOrientation)
  24.  
  25. # 开始一千次迭代，也就是一千次交互，每次交互后停顿1/240
  26. for i in range(1000):
  27.     p.stepSimulation()
  28.     time.sleep(1 / 240)
  29.  
  30. # 获取位置与方向四元数
  31. cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
  32. print("-" * 20)
  33. print(f"机器人的位置坐标为:{cubePos}\n机器人的朝向四元数为:{cubeOrn}")
  34. print("-" * 20)
  35.  
  36. # 断开连接
  37. p.disconnect()

解释：

导入 pybullet 模块后，要做的第一件事是“连接”到物理引擎：pybullet 内置了物理引擎的两种连接方式：DIRECT和GUI。无论哪种连接，在 pybullet 中都执行相同物理模拟和渲染。差别在于有无可视化的窗口。（connect方法）

GUI 连接：

physicsClient = p.connect(p.GUI)

DIRECT 连接：

physicsClient = p.connect(p.DIRECT)

disconnect断开连接

使用disconnect()函数可以指定需要关闭的物理服务器，默认参数是0，也就是第一个创建的服务器的ID。

默认情况下，连接到物理引擎后，并没有没有启用重力。因此我们需要单独设置：

setGravity能帮我们在沿x,y,z轴方向上设置重力。（单位是国际标准单位制下的单位。该函数没有返回值）

LOADURDF加载模型（一般情况下pybullet自带了文件）：loadURDF 将向物理服务器发送命令，以从通用机器人描述文件（URDF）加载物理模型。

  1. # 加载URDF模型，此处是加载蓝白相间的陆地
  2. planeId = p.loadURDF("plane.urdf")
  3.  
  4. ......
  5. boxId = p.loadURDF("r2d2.urdf", startPos, startOrientation)

参数fileName代表需要加载的urdf模型的绝对路径，如果你将urdf文件放在了一个文件夹下，可以使用setAdditionalSearchPath()方法来添加loadURDF的搜索路径。

使用stepSimulation进行模拟

使用stepSimulation函数可以看成是进行一次迭代步

  1. for i in range(1000):
  2.     p.stepSimulation()
  3.     time.sleep(0.1)

使用setRealTimeSimulation进行模拟

除了使用stepSimualtion进行正向动力学进行步进模拟，我们还可以使用setRealTimeSimulation函数，直接将物理引擎渲染的时间和RTC(real time clock)同步，这样做，就不需要使用stepSimualtion显式地执行模拟步了

  1. import pybullet as p
  2. import time
  3. import pybullet_data
  4.  
  5. # 连接物理引擎
  6. physicsCilent = p.connect(p.GUI)
  7.  
  8. # 渲染逻辑
  9. p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
  10. p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
  11.  
  12. # 添加资源路径
  13. p.setAdditionalSearchPath(pybullet_data.getDataPath())
  14.  
  15. # 设置环境重力加速度
  16. p.setGravity(0, 0, -10)
  17.  
  18. # 加载URDF模型，此处是加载蓝白相间的陆地
  19. planeId = p.loadURDF("plane.urdf")
  20.  
  21. # 加载机器人，并设置加载的机器人的位姿
  22. startPos = [0, 0, 1]
  23. startOrientation = p.getQuaternionFromEuler([0, 0, 0])
  24. boxId = p.loadURDF("r2d2.urdf", startPos, startOrientation)
  25.  
  26. # 按照位置和朝向重置机器人的位姿，由于我们之前已经初始化了机器人，所以此处加不加这句话没什么影响
  27. p.resetBasePositionAndOrientation(boxId, startPos, startOrientation)
  28. p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
  29.  
  30. p.setRealTimeSimulation(1)
  31.  
  32. while True:
  33.     pass
  34.  
  35. # 断开连接
  36. p.disconnect()

查看机器人的信息

pybullet提供了一堆函数来查询指定机器人的信息，乃至指定机器人指定关节的信息

比如getBasePositionAndOrientation接受机器人ID，返回两个列表，第一个代表基座的位置，第二个代表方向四元数，还有类似的函数，都以get打头，基本都是一看就知道啥意思

还有一些函数是用来获取机器人关节信息：

getNumJoints()接受两个参数，其中第一个参数为机器人ID，第二个参数可选，一般不需要选，因为只有多平台开发时才用得到。该函数返回该ID机器人的关节数量。

getJointInfo()接受三个参数，其中第三个参数和上面那个函数的第二个参数一样，有关平台，可以不填，前两个参数为需要查看的机器人的ID和需要查看的该机器人的关节序号。该函数返回一个tuple，该tuple的长度为17

以如下代码为例：

  1. import pybullet as p
  2. import pybullet_data
  3. from time import sleep
  4.  
  5. # 链接物理引擎
  6. p.connect(p.GUI)
  7.  
  8. # 设置模型加载路径
  9. datapath = pybullet_data.getDataPath()
  10. p.setAdditionalSearchPath(datapath)
  11.  
  12. # 加载模型
  13. robot_id = p.loadURDF("r2d2.urdf", [0, 0, 0.5])
  14.  
  15. # 输出基本信息
  16. joint_num = p.getNumJoints(robot_id)
  17. print("r2d2的节点数量为：", joint_num)
  18.  
  19. print("r2d2的信息：")
  20. for joint_index in range(joint_num):
  21.     info_tuple = p.getJointInfo(robot_id, joint_index)
  22.     print(f"关节序号：{info_tuple[0]}\n\
  23.             关节名称：{info_tuple[1]}\n\
  24.             关节类型：{info_tuple[2]}\n\
  25.             机器人第一个位置的变量索引：{info_tuple[3]}\n\
  26.             机器人第一个速度的变量索引：{info_tuple[4]}\n\
  27.             保留参数：{info_tuple[5]}\n\
  28.             关节的阻尼大小：{info_tuple[6]}\n\
  29.             关节的摩擦系数：{info_tuple[7]}\n\
  30.             slider和revolute(hinge)类型的位移最小值：{info_tuple[8]}\n\
  31.             slider和revolute(hinge)类型的位移最大值：{info_tuple[9]}\n\
  32.             关节驱动的最大值：{info_tuple[10]}\n\
  33.             关节的最大速度：{info_tuple[11]}\n\
  34.             节点名称：{info_tuple[12]}\n\
  35.             局部框架中的关节轴系：{info_tuple[13]}\n\
  36.             父节点frame的关节位置：{info_tuple[14]}\n\
  37.             父节点frame的关节方向：{info_tuple[15]}\n\
  38.             父节点的索引，若是基座返回-1：{info_tuple[16]}\n\n")

输出结果：

  1. r2d2的节点数量为： 15
  2. r2d2的信息：
  3. 关节序号：0
  4.             关节名称：b'base_to_right_leg'
  5.             关节类型：4
  6.             机器人第一个位置的变量索引：-1
  7.             机器人第一个速度的变量索引：-1
  8.             保留参数：0
  9.             关节的阻尼大小：0.0
  10.             关节的摩擦系数：0.0
  11.             slider和revolute(hinge)类型的位移最小值：0.0
  12.             slider和revolute(hinge)类型的位移最大值：-1.0
  13.             关节驱动的最大值：0.0
  14.             关节的最大速度：0.0
  15.             节点名称：b'right_leg'
  16.             局部框架中的关节轴系：(0.0, 0.0, 0.0)
  17.             父节点frame的关节位置：(0.22, 0.0, 0.25)
  18.             父节点frame的关节方向：(0.0, -0.7070904020014416, 0.0, 0.7071231599922604)
  19.             父节点的索引，若是基座返回-1：-1
  20.  
  21.  
  22. 关节序号：1
  23.             关节名称：b'right_base_joint'
  24.             关节类型：4
  25.             机器人第一个位置的变量索引：-1
  26.             机器人第一个速度的变量索引：-1
  27.             保留参数：0
  28.             关节的阻尼大小：0.0
  29.             关节的摩擦系数：0.0
  30.             slider和revolute(hinge)类型的位移最小值：0.0
  31.             slider和revolute(hinge)类型的位移最大值：-1.0
  32.             关节驱动的最大值：0.0
  33.             关节的最大速度：0.0
  34.             节点名称：b'right_base'
  35.             局部框架中的关节轴系：(0.0, 0.0, 0.0)
  36.             父节点frame的关节位置：(0.2999999996780742, 0.0, -1.3898038463944216e-05)
  37.             父节点frame的关节方向：(0.0, 0.7070904020014416, 0.0, 0.7071231599922604)
  38.             父节点的索引，若是基座返回-1：0
  39.  
  40.  
  41. 关节序号：2
  42.             关节名称：b'right_front_wheel_joint'
  43.             关节类型：0
  44.             机器人第一个位置的变量索引：7
  45.             机器人第一个速度的变量索引：6
  46.             保留参数：1
  47.             关节的阻尼大小：0.0
  48.             关节的摩擦系数：0.0
  49.             slider和revolute(hinge)类型的位移最小值：0.0
  50.             slider和revolute(hinge)类型的位移最大值：-1.0
  51.             关节驱动的最大值：100.0
  52.             关节的最大速度：100.0
  53.             节点名称：b'right_front_wheel'
  54.             局部框架中的关节轴系：(0.0, 0.0, 1.0)
  55.             父节点frame的关节位置：(0.0, 0.133333333333, -0.085)
  56.             父节点frame的关节方向：(0.0, -0.7070904020014416, 0.0, 0.7071231599922604)
  57.             父节点的索引，若是基座返回-1：1
  58.  
  59.  
  60. 关节序号：3
  61.             关节名称：b'right_back_wheel_joint'
  62.             关节类型：0
  63.             机器人第一个位置的变量索引：8
  64.             机器人第一个速度的变量索引：7
  65.             保留参数：1
  66.             关节的阻尼大小：0.0
  67.             关节的摩擦系数：0.0
  68.             slider和revolute(hinge)类型的位移最小值：0.0
  69.             slider和revolute(hinge)类型的位移最大值：-1.0
  70.             关节驱动的最大值：100.0
  71.             关节的最大速度：100.0
  72.             节点名称：b'right_back_wheel'
  73.             局部框架中的关节轴系：(0.0, 0.0, 1.0)
  74.             父节点frame的关节位置：(0.0, -0.133333333333, -0.085)
  75.             父节点frame的关节方向：(0.0, -0.7070904020014416, 0.0, 0.7071231599922604)
  76.             父节点的索引，若是基座返回-1：1
  77.  
  78.  
  79. 关节序号：4
  80.             关节名称：b'base_to_left_leg'
  81.             关节类型：4
  82.             机器人第一个位置的变量索引：-1
  83.             机器人第一个速度的变量索引：-1
  84.             保留参数：0
  85.             关节的阻尼大小：0.0
  86.             关节的摩擦系数：0.0
  87.             slider和revolute(hinge)类型的位移最小值：0.0
  88.             slider和revolute(hinge)类型的位移最大值：-1.0
  89.             关节驱动的最大值：0.0
  90.             关节的最大速度：0.0
  91.             节点名称：b'left_leg'
  92.             局部框架中的关节轴系：(0.0, 0.0, 0.0)
  93.             父节点frame的关节位置：(-0.22, 0.0, 0.25)
  94.             父节点frame的关节方向：(0.0, -0.7070904020014416, 0.0, 0.7071231599922604)
  95.             父节点的索引，若是基座返回-1：-1
  96.  
  97.  
  98. 关节序号：5
  99.             关节名称：b'left_base_joint'
  100.             关节类型：4
  101.             机器人第一个位置的变量索引：-1
  102.             机器人第一个速度的变量索引：-1
  103.             保留参数：0
  104.             关节的阻尼大小：0.0
  105.             关节的摩擦系数：0.0
  106.             slider和revolute(hinge)类型的位移最小值：0.0
  107.             slider和revolute(hinge)类型的位移最大值：-1.0
  108.             关节驱动的最大值：0.0
  109.             关节的最大速度：0.0
  110.             节点名称：b'left_base'
  111.             局部框架中的关节轴系：(0.0, 0.0, 0.0)
  112.             父节点frame的关节位置：(0.2999999996780742, 0.0, -1.3898038463944216e-05)
  113.             父节点frame的关节方向：(0.0, 0.7070904020014416, 0.0, 0.7071231599922604)
  114.             父节点的索引，若是基座返回-1：4
  115.  
  116.  
  117. 关节序号：6
  118.             关节名称：b'left_front_wheel_joint'
  119.             关节类型：0
  120.             机器人第一个位置的变量索引：9
  121.             机器人第一个速度的变量索引：8
  122.             保留参数：1
  123.             关节的阻尼大小：0.0
  124.             关节的摩擦系数：0.0
  125.             slider和revolute(hinge)类型的位移最小值：0.0
  126.             slider和revolute(hinge)类型的位移最大值：-1.0
  127.             关节驱动的最大值：100.0
  128.             关节的最大速度：100.0
  129.             节点名称：b'left_front_wheel'
  130.             局部框架中的关节轴系：(0.0, 0.0, 1.0)
  131.             父节点frame的关节位置：(0.0, 0.133333333333, -0.085)
  132.             父节点frame的关节方向：(0.0, -0.7070904020014416, 0.0, 0.7071231599922604)
  133.             父节点的索引，若是基座返回-1：5
  134.  
  135.  
  136. 关节序号：7
  137.             关节名称：b'left_back_wheel_joint'
  138.             关节类型：0
  139.             机器人第一个位置的变量索引：10
  140.             机器人第一个速度的变量索引：9
  141.             保留参数：1
  142.             关节的阻尼大小：0.0
  143.             关节的摩擦系数：0.0
  144.             slider和revolute(hinge)类型的位移最小值：0.0
  145.             slider和revolute(hinge)类型的位移最大值：-1.0
  146.             关节驱动的最大值：100.0
  147.             关节的最大速度：100.0
  148.             节点名称：b'left_back_wheel'
  149.             局部框架中的关节轴系：(0.0, 0.0, 1.0)
  150.             父节点frame的关节位置：(0.0, -0.133333333333, -0.085)
  151.             父节点frame的关节方向：(0.0, -0.7070904020014416, 0.0, 0.7071231599922604)
  152.             父节点的索引，若是基座返回-1：5
  153.  
  154.  
  155. 关节序号：8
  156.             关节名称：b'gripper_extension'
  157.             关节类型：1
  158.             机器人第一个位置的变量索引：11
  159.             机器人第一个速度的变量索引：10
  160.             保留参数：1
  161.             关节的阻尼大小：0.0
  162.             关节的摩擦系数：0.0
  163.             slider和revolute(hinge)类型的位移最小值：-0.38
  164.             slider和revolute(hinge)类型的位移最大值：0.0
  165.             关节驱动的最大值：1000.0
  166.             关节的最大速度：0.5
  167.             节点名称：b'gripper_pole'
  168.             局部框架中的关节轴系：(1.0, 0.0, 0.0)
  169.             父节点frame的关节位置：(0.0, 0.19, 0.2)
  170.             父节点frame的关节方向：(0.0, 0.0, -0.7070904020014416, 0.7071231599922604)
  171.             父节点的索引，若是基座返回-1：-1
  172.  
  173.  
  174. 关节序号：9
  175.             关节名称：b'left_gripper_joint'
  176.             关节类型：0
  177.             机器人第一个位置的变量索引：12
  178.             机器人第一个速度的变量索引：11
  179.             保留参数：1
  180.             关节的阻尼大小：0.0
  181.             关节的摩擦系数：0.0
  182.             slider和revolute(hinge)类型的位移最小值：0.0
  183.             slider和revolute(hinge)类型的位移最大值：0.548
  184.             关节驱动的最大值：1000.0
  185.             关节的最大速度：0.5
  186.             节点名称：b'left_gripper'
  187.             局部框架中的关节轴系：(0.0, 0.0, 1.0)
  188.             父节点frame的关节位置：(0.2, 0.02, 0.0)
  189.             父节点frame的关节方向：(0.0, 0.0, 0.0, 1.0)
  190.             父节点的索引，若是基座返回-1：8
  191.  
  192.  
  193. 关节序号：10
  194.             关节名称：b'left_tip_joint'
  195.             关节类型：4
  196.             机器人第一个位置的变量索引：-1
  197.             机器人第一个速度的变量索引：-1
  198.             保留参数：0
  199.             关节的阻尼大小：0.0
  200.             关节的摩擦系数：0.0
  201.             slider和revolute(hinge)类型的位移最小值：0.0
  202.             slider和revolute(hinge)类型的位移最大值：-1.0
  203.             关节驱动的最大值：0.0
  204.             关节的最大速度：0.0
  205.             节点名称：b'left_tip'
  206.             局部框架中的关节轴系：(0.0, 0.0, 0.0)
  207.             父节点frame的关节位置：(0.0, 0.0, 0.0)
  208.             父节点frame的关节方向：(0.0, 0.0, 0.0, 1.0)
  209.             父节点的索引，若是基座返回-1：9
  210.  
  211.  
  212. 关节序号：11
  213.             关节名称：b'right_gripper_joint'
  214.             关节类型：0
  215.             机器人第一个位置的变量索引：13
  216.             机器人第一个速度的变量索引：12
  217.             保留参数：1
  218.             关节的阻尼大小：0.0
  219.             关节的摩擦系数：0.0
  220.             slider和revolute(hinge)类型的位移最小值：0.0
  221.             slider和revolute(hinge)类型的位移最大值：0.548
  222.             关节驱动的最大值：1000.0
  223.             关节的最大速度：0.5
  224.             节点名称：b'right_gripper'
  225.             局部框架中的关节轴系：(0.0, 0.0, -1.0)
  226.             父节点frame的关节位置：(0.2, -0.02, 0.0)
  227.             父节点frame的关节方向：(0.0, 0.0, 0.0, 1.0)
  228.             父节点的索引，若是基座返回-1：8
  229.  
  230.  
  231. 关节序号：12
  232.             关节名称：b'right_tip_joint'
  233.             关节类型：4
  234.             机器人第一个位置的变量索引：-1
  235.             机器人第一个速度的变量索引：-1
  236.             保留参数：0
  237.             关节的阻尼大小：0.0
  238.             关节的摩擦系数：0.0
  239.             slider和revolute(hinge)类型的位移最小值：0.0
  240.             slider和revolute(hinge)类型的位移最大值：-1.0
  241.             关节驱动的最大值：0.0
  242.             关节的最大速度：0.0
  243.             节点名称：b'right_tip'
  244.             局部框架中的关节轴系：(0.0, 0.0, 0.0)
  245.             父节点frame的关节位置：(0.0, 0.0, 0.0)
  246.             父节点frame的关节方向：(0.0, 0.0, 0.0, 1.0)
  247.             父节点的索引，若是基座返回-1：11
  248.  
  249.  
  250. 关节序号：13
  251.             关节名称：b'head_swivel'
  252.             关节类型：0
  253.             机器人第一个位置的变量索引：14
  254.             机器人第一个速度的变量索引：13
  255.             保留参数：1
  256.             关节的阻尼大小：0.0
  257.             关节的摩擦系数：0.0
  258.             slider和revolute(hinge)类型的位移最小值：-0.38
  259.             slider和revolute(hinge)类型的位移最大值：0.0
  260.             关节驱动的最大值：1000.0
  261.             关节的最大速度：0.5
  262.             节点名称：b'head'
  263.             局部框架中的关节轴系：(0.0, 0.0, 1.0)
  264.             父节点frame的关节位置：(0.0, 0.0, 0.3)
  265.             父节点frame的关节方向：(0.0, 0.0, 0.0, 1.0)
  266.             父节点的索引，若是基座返回-1：-1
  267.  
  268.  
  269. 关节序号：14
  270.             关节名称：b'tobox'
  271.             关节类型：4
  272.             机器人第一个位置的变量索引：-1
  273.             机器人第一个速度的变量索引：-1
  274.             保留参数：0
  275.             关节的阻尼大小：0.0
  276.             关节的摩擦系数：0.0
  277.             slider和revolute(hinge)类型的位移最小值：0.0
  278.             slider和revolute(hinge)类型的位移最大值：-1.0
  279.             关节驱动的最大值：0.0
  280.             关节的最大速度：0.0
  281.             节点名称：b'box'
  282.             局部框架中的关节轴系：(0.0, 0.0, 0.0)
  283.             父节点frame的关节位置：(0.0, 0.1214, 0.1214)
  284.             父节点frame的关节方向：(0.0, 0.0, 0.0, 1.0)
  285.             父节点的索引，若是基座返回-1：13

机器人控制，相机追踪

让模型动起来

urdf文件（常见的机器人模型储存在计算机的数据结构）：

URDF文件生成的机器人模型，主体称为base（基座），其余的分支称为link，base与link，link与link之间通过joint（关节）连接。因此，整个URDF模型可以描述为一颗多叉树，树的根节点为base。根据树的一些知识，我们也知道树是不能成环的，因此，URDF描述的机器人多叉树中，组件与组件之间的连接方式不能形成环，否则pybullet解析URDF文件时会报错

以这个urdf为例，中间蓝色的圆柱体是它的base，其余所有你能看到的组件都是link，然后link与link之间，base与link之间都是通过joint连接的，只不过这个地方我们看不出来，所有的joint都在右上角展示出来了。而我们能够控制的，也就是这些关节，我们能怎么控制这些关节，关节提供给用户的控制量都是在URDF中定义好的

URDF中的joint是通过joint标签来定义的，在joint的标签属性中，除了定义joint的名字外，还需要给出这个关节的类型

比如连接head与base的joint中，type为“continuous”，说明这个关节可以在连续空间中转动：

比如连接base和机器人右腿的joint，定义如下，可以看到type为fixed，说明这个joint不能转动，是固定死的：

因此对于URDF模型文件生成的机器人来说，我们只能通过控制这些type不为fixed的joint来控制机器人。

那么为了控制一个机器人，我们就得知道关于机器关节的信息，基于这些信息，我们才能知道一个机器人有什么地方可以被我们控制以及怎么控制

获取关节信息，得到可控制关节的索引及信息：

主要是getNumJoints()和getJointInfo()这两个函数， getNumJoints()接受机器人的ID，返回该机器人的关节数量，通常遍历机器人所有关节时会用到；getJointInfo()接受机器人的ID以及关节的索引值，用来查询指定机器人指定关节的信息，通过这些信息，我们可以知道哪些关节是我们想要的

以这个输出结果为例：

  1. r2d2的节点数量为： 15
  2. r2d2的信息：
  3. 关节序号：0
  4.             关节名称：b'base_to_right_leg'
  5.             关节类型：4
  6.             机器人第一个位置的变量索引：-1
  7.             机器人第一个速度的变量索引：-1
  8.             保留参数：0
  9.             关节的阻尼大小：0.0
  10.             关节的摩擦系数：0.0
  11.             slider和revolute(hinge)类型的位移最小值：0.0
  12.             slider和revolute(hinge)类型的位移最大值：-1.0
  13.             关节驱动的最大值：0.0
  14.             关节的最大速度：0.0
  15.             节点名称：b'right_leg'
  16.             局部框架中的关节轴系：(0.0, 0.0, 0.0)
  17.             父节点frame的关节位置：(0.22, 0.0, 0.25)
  18.             父节点frame的关节方向：(0.0, -0.7070904020014416, 0.0, 0.7071231599922604)
  19.             父节点的索引，若是基座返回-1：-1
  20.  
  21.  
  22. 关节序号：1
  23.             关节名称：b'right_base_joint'
  24.             关节类型：4
  25.             机器人第一个位置的变量索引：-1
  26.             机器人第一个速度的变量索引：-1
  27.             保留参数：0
  28.             关节的阻尼大小：0.0
  29.             关节的摩擦系数：0.0
  30.             slider和revolute(hinge)类型的位移最小值：0.0
  31.             slider和revolute(hinge)类型的位移最大值：-1.0
  32.             关节驱动的最大值：0.0
  33.             关节的最大速度：0.0
  34.             节点名称：b'right_base'
  35.             局部框架中的关节轴系：(0.0, 0.0, 0.0)
  36.             父节点frame的关节位置：(0.2999999996780742, 0.0, -1.3898038463944216e-05)
  37.             父节点frame的关节方向：(0.0, 0.7070904020014416, 0.0, 0.7071231599922604)
  38.             父节点的索引，若是基座返回-1：0
  39.  
  40.  
  41. 关节序号：2
  42.             关节名称：b'right_front_wheel_joint'
  43.             关节类型：0
  44.             机器人第一个位置的变量索引：7
  45.             机器人第一个速度的变量索引：6
  46.             保留参数：1
  47.             关节的阻尼大小：0.0
  48.             关节的摩擦系数：0.0
  49.             slider和revolute(hinge)类型的位移最小值：0.0
  50.             slider和revolute(hinge)类型的位移最大值：-1.0
  51.             关节驱动的最大值：100.0
  52.             关节的最大速度：100.0
  53.             节点名称：b'right_front_wheel'
  54.             局部框架中的关节轴系：(0.0, 0.0, 1.0)
  55.             父节点frame的关节位置：(0.0, 0.133333333333, -0.085)
  56.             父节点frame的关节方向：(0.0, -0.7070904020014416, 0.0, 0.7071231599922604)
  57.             父节点的索引，若是基座返回-1：1

根据“关节类型”这一项，可以判断改关节是不是可以操控的。于是乎，我们先试着通过代码打印r2d2的可移动的关节，首先创建环境加载模型：

  1. import pybullet as p
  2. import pybullet_data
  3. import time 
  4. from pprint import pprint
  5.  
  6. # 连接物理引擎
  7. use_gui = True
  8. if use_gui:
  9.     serve_id = p.connect(p.GUI)
  10. else:
  11.     serve_id = p.connect(p.DIRECT)
  12.  
  13. # 添加资源路径
  14. p.setAdditionalSearchPath(pybullet_data.getDataPath())
  15.  
  16. # 配置渲染机制
  17. p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
  18. p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
  19. p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
  20.  
  21. # 设置重力，加载模型
  22. p.setGravity(0, 0, -10)
  23. _ = p.loadURDF("plane.urdf", useMaximalCoordinates=True)
  24. robot_id = p.loadURDF("r2d2.urdf", useMaximalCoordinates=True)

加载完后，我们来打印一下可以使用的关节：（不可用关节的关节类型都是4，也就是常量p.JOINT_FIXED，所以可以根据这个信息来筛选）

  1. available_joints_indexes = [i for i in range(p.getNumJoints(robot_id)) if p.getJointInfo(robot_id, i)[2] != p.JOINT_FIXED]
  2.  
  3. pprint([p.getJointInfo(robot_id, i)[1] for i in available_joints_indexes])

输出结果：

  1. [b'right_front_wheel_joint',
  2.  b'right_back_wheel_joint',
  3.  b'left_front_wheel_joint',
  4.  b'left_back_wheel_joint',
  5.  b'gripper_extension',
  6.  b'left_gripper_joint',
  7.  b'right_gripper_joint',
  8.  b'head_swivel']

我们通过程序得到了可控制关节的相关信息，下面就是操控它们了

操控机器人：setJointMotorControl2与setJointMotorControlArray

这两个的用法基本一样，不同的是前者调用一次只能设置一台关节电机的参数，后者调用一次则可以设置一组关节电机的参数

setJointMotorControl2有三个必选参数和若干可选参数，三个必选参数为需要控制的机器人的序号bodyUniqueId，需要控制的关节索引jointIndex，还有控制模式controlMode。控制模式controlMode的可填值为POSITION_CONTROL, VELOCITY_CONTROL, TORQUE_CONTROL and PD_CONTROL

setJointMotorControlArray类似，只不过把对应的参数换成同类型的列表就行

仍然之前那个机器人例子为例，使用上述函数来让它动起来。需要注意的是，移动它只需要四个轮子，所以我们需要先挑选出轮子与基座的关节索引，这个很简单，只需要一句话

wheel_joints_indexes = [i for i in available_joints_indexes if "wheel" in str(p.getJointInfo(robot_id, i)[1])]

开启渲染后，使用setJointMotorControlArray在每次迭代步一次性设置四个轮子的关节马达的值：

  1. # 预备工作结束，重新开启渲染
  2. p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
  3. # 关闭实时模拟步
  4. p.setRealTimeSimulation(0)
  5.  
  6. target_v = 10                   # 电机达到的预定角速度（rad/s）
  7. max_force = 10                  # 电机能够提供的力，这个值决定了机器人运动时的加速度，太快会翻车哟，单位N
  8.  
  9. for i in range(1000):
  10.     p.stepSimulation()
  11.     p.setJointMotorControlArray(
  12.         bodyUniqueId=robot_id,
  13.         jointIndices=wheel_joints_indexes,
  14.         controlMode=p.VELOCITY_CONTROL,
  15.         targetVelocities=[target_v for _ in wheel_joints_indexes],
  16.         forces=[max_force for _ in wheel_joints_indexes]
  17.     )
  18.     time.sleep(1 / 240)         # 模拟器一秒模拟迭代240步
  19.  
  20. # 断开连接
  21. p.disconnect(serve_id)

将指定机器人的指定关节设为禁用（将这个马达的force设为0）：

  1. maxForce = 0
  2. mode = p.VELOCITY_CONTROL
  3. p.setJointMotorControl2(objUid, jointIndex,
  4.  	controlMode=mode, force=maxForce)

相机跟踪：

让我们的机器人一直在画面中心，这个时候，就需要调用resetDebugVisualizerCamera函数来设置当前相机的位置和朝向，那么我只需要在每次迭代步结束后，调用这个函数来调整相机位置

在设置相机前，我们还得知道机器人现在的位置，我们可以通过getJointState(s)来查询机器人指定关节的信息，当然这个信息里面就包含着我们想要的位置信息（三维坐标）：

如果只是想要获得机器人在世界坐标系中的位置坐标的话，直接通getBasePositionAndOrientation就可以获取机器人的位置和位姿四元数了。

那么接下来，我们只需要通过上述这个函数查询机器人base的位置信息，再用这个位置信息去更新相机的位置，迭代循环更改为：

  1. for i in range(1000):
  2.     p.stepSimulation()
  3.     p.setJointMotorControlArray(
  4.         bodyUniqueId=robot_id,
  5.         jointIndices=wheel_joints_indexes,
  6.         controlMode=p.VELOCITY_CONTROL,
  7.         targetVelocities=[target_v for _ in wheel_joints_indexes],
  8.         forces=[max_force for _ in wheel_joints_indexes]
  9.     )
  10.     location, _ = p.getBasePositionAndOrientation(robot_id)
  11.     p.resetDebugVisualizerCamera(
  12.         cameraDistance=3,
  13.         cameraYaw=110,
  14.         cameraPitch=-30,
  15.         cameraTargetPosition=location
  16.     )
  17.     time.sleep(1 / 240)         # 模拟器一秒模拟迭代240步

可以看到，机器人已经时刻处于镜头中央了

2.Pinocchio

总览一下实现的主要算法：正向运动学（forward kinematics）：给定机器人构型，计算每个关节的空间位置并将其存储为坐标变换。如果给定速度或加速度，将计算每个关节的空间速度（以局部坐标系表示）。运动学雅可比矩阵（kinematic jacobian）：在机械臂运动学中用来计算机械臂末端执行器的速度与各个关节运动速度之间的关系。逆动力学（inverse dynamics）：采用Recursive Newton-Euler Algorithm (RNEA) 计算逆动力学。即给定所需的机器人构型、速度、加速度，计算并存储执行该运动所需的扭矩。关节空间惯性矩阵（Joint space inertia matrix）：采用Compostie rigid body algortihm (CRBA)算法，计算关节空间惯性矩阵。前向动力学（forward dynamics）：采用Articulated Body Algorithm（ABA）计算无约束的前向动力学。即给定机器人构型、速度、扭矩和外力的情况下，可以计算出由此产生的关节加速度。其他算法。

最简单的例子引入，计算机器人逆动力学：

from future import print_function  # 导入print_function函数
import pinocchio  # 导入Pinocchio库，用于机器人动力学建模和分析

model = pinocchio.buildSampleModelManipulator()  # 创建一个带有操纵器的机器人模型示例
data = model.createData()  # 创建数据结构，用于后续的数值计算


q = pinocchio.neutral(model)  # 计算机器人的中性配置，即所有关节都处于机械中性位置的配置
v = pinocchio.utils.zero(model.nv)  # 创建一个长度为model.nv的零向量，model.nv是机器人的自由度数量
a = pinocchio.utils.zero(model.nv)  # 创建另一个长度为model.nv的零向量

tau = pinocchio.rnea(model, data, q, v, a)  # 使用RNEA方法计算关节力（tau）
print('tau = ', tau.T)  # 打印关节力tau，并将其转换为NumPy数组并进行转置（tau.T）

该程序加载机器人模型，创建用于算法缓冲的数据结构，并运行递归牛顿-欧拉算法 （RNEA） 来计算机器人逆动力学。

第一段定义了模型，在 Python 中，模型是通过调用单个命令创建的。模型类包含机器人的常量参数（质量、段长度、主体名称等），即算法无法修改的参数。由于大多数算法需要额外的空间来存储内部值，因此我们还必须分配一个专用于模型的数据结构。

逆动力学计算所需的扭矩，以跟踪由关节配置、速度和加速度定义的轨迹。在第二段中，我们定义了关节配置，速度和加速度。速度和加速度是纯向量，可以初始化为任何值，它们的大小对应于机器人的瞬时自由度数。

还有一个使用Pinocchio库计算机器人动力学的示例：

这个脚本的主要功能如下：1.计算7自由度操纵器（以Kuka iiwa7为例）的逆动力学，包括：每个关节的总扭矩； Gravity contribution G(q)； 每个关节的惯性矩阵 M； 每个关节的科里奥利矩阵 C

2. 验证 dM/dt - 2*C 的反对称性质

使用：将dy.py文件和iiwa7_description.urdf文件放在同一个文件夹中，使用Python运行脚本，最后你可以得到你的模型中的M、C和G

注意：如果你更改了urdf文件使用了你的模型，还应根据你的模型改变q, qdot,qddot的大小

dy,py脚本如下：A_Example_for_Calculating_Robot_Dynamics_Using_Pinocchio_Library/dy.py at main · bhtxy0525/A_Example_for_Calculating_Robot_Dynamics_Using_Pinocchio_Library (github.com)

3.pinocchio与pybullet

0.回顾pinocchio运动学计算及参数更新机制：

导入URDF模型，创建数据模型。注意，model为刚体的模型结构，data存储了刚体系统的数据结构，当调用forwardKinematics()函数计算正向运动学时，函数根据当前关节位置、速度和加速度，更新data中存储的关节位置、空间速度和空间加速度

  1. import pinocchio  # 导入Pinocchio库
  2. from sys import argv  # 从sys模块导入argv列表，用于获取命令行参数
  3. from os.path import dirname, join, abspath  # 从os.path模块导入dirname, join, abspath函数，用于操作文件路径
  4.  
  5. # 这里的路径指向Pinocchio源代码，但你可以在这里定义你自己的目录。
  6. pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
  7.  
  8. # 应该在这里设置你自己的URDF文件，或者直接作为此例子的参数传递。
  9. urdf_filename = pinocchio_model_dir + '/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf' if len(argv)<2 else argv[1]
  10.  
  11. # 从URDF文件中加载模型
  12. model = pinocchio.buildModelFromUrdf(urdf_filename)
  13. print('模型名称: ' + model.name)
  14.  
  15. # 创建算法所需的data对象
  16. data = model.createData()
  17.  
  18. # 随机采样一个配置
  19. q = pinocchio.randomConfiguration(model)
  20. print('配置q: %s' % q.T)
  21.  
  22. # 在运动学树中进行正向运动学
  23. pinocchio.forwardKinematics(model, data, q)
  24.  
  25. # 打印出运动学树中每个关节的位置
  26. for name, oMi in zip(model.names, data.oMi):
  27.     print(("{:<24} : {: .2f} {: .2f} {: .2f}"
  28.            .format(name, *oMi.translation.T.flat)))

1.联调

仿真在 PyBullet中进行，动力学库 Pinocchio 用于更新机器人的动态项，如惯性矩阵和雅可比矩阵

关键函数是根据bullet中状态更新pinocchio参数：

  1. def get_state_update_pinocchio(self):
  2. """从pybullet获取状态并更新pinocchio机器人内部。
  3. 此函数从pybullet模拟器中获取状态，并将
  4. 运动学、雅可比矩阵、质心矩等正向传递给pinocchio机器人
  5. （关于计算的量，请参见forward_pinocchio的详细信息）。"""
  6. q, dq, ef = self.get_state()  # 从self对象中获取状态，通常包含关节位置q、关节速度dq和末端执行器位置ef
  7. self.update_pinocchio(q, dq)  # 更新pinocchio机器人内部状态，通常包括运动学、雅可比矩阵和质心矩的计算
  8. return q, dq, ef  # 返回更新后的关节位置q、关节速度dq和末端执行器位置ef

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/IT小白/article/detail/773184
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号