pybullet API学习笔记-仿真时间、力矩控制、位置控制

bullet 仿真时间、力矩控制、位置控制

​ 参考文献： https://pybullet.org/wordpress/index.php/forum-2/

​ 参考代码： https://github.com/unitreerobotics/unitree_pybullet

• 前言

  ​ 该学习笔记主要对笔者项目所需的API进行学习记录，不会对Bullet进行系统介绍，目前介绍内容主要包括：仿真时间、力矩控制、位置控制、（状态反馈）。另外，笔者水平有限，大家有认为不对的地方，欢迎批评指正。一起学习，快乐无限～

  ​ bullet的GUI连接模式是双核的，物理仿真引擎计算占一个线程，GUI界面显示占一个线程。一个进行数值上的计算，一个将计算的结果用图形化的方式展示出来。但是物理仿真引擎计算会按照用户设置的时间进行计算，而GUI界面只会按照固定频率刷新。

通俗理解就是:

1. stepSimulation(delta_t), delta_t < 0.01666，则bullet只在后台计算物体世界坐标变化，GUI界面不显示该变化，只有在0.01666s以后才更新该变化。

2. stepSimulation(delta_t), delta_t > 0.01666)，则bullet在一个仿真时间步内会多次更新GUI界面。

注意：delta_t 的变化不会影响仿真结果，但是如果添加了控制命令，就有可能影响仿真结果了。那么接下来就看加上控制的区别。

力矩控制

• 用速度模式模拟力矩控制（模式1）

# 该控制方法比较接近真实的物理环境。
maxForce = 0
mode = p.VELOCITY_CONTROL
p.setJointMotorControl2(objUid, jointIndex, controlMode=mode, force=maxForce)

​ 1. 仿真时间设置：stepSimulation(delta)

运行到congtrol函数时会在delta时间步内都对关节施加力矩。delta越长，力矩持续时间越长。

与time.sleep(t) 连用时：仿真引擎暂停 t 秒，然后再让仿真引擎进入下一个时间步。t 变化不影响仿真结果。

​ 2. 仿真时间设置：setRealTimeSimulation()

按照RTC（Real-Time Colck）进行控制，类似与stepSimulation(delta)模式，频率为10000HZ的样子。

与time.sleep(t) 连用时：仿真引擎每 t 秒采样一次控制命令，然后按照RTC进行控制直到下一次采样。t 变化影响仿真结果。

• 力矩控制模式（模式2）

# 使用该控制模式时，控制指令相当与施加给关节的外力，只有施加外部力矩足够关节抵抗惯性矩阵产生加速度时，关节才会转动。否则关节静止。肢体重力产生的力矩并不会让关节转动。
p.setJointMotorControlArray(quadruped, jointIds, p.TORQUE_CONTROL, forces=torque_value)

​ 1. 仿真时间设置：stepSimulation() (当控推荐使用)

同上。运行到congtrol函数时：会在delta时间步内都对关节施加力矩。delta越长，力矩持续时间越长。

同上。与time.sleep(t) 连用时：仿真引擎暂停 t 秒，然后再让仿真引擎进入下一个时间步。t 变化不影响仿真结果。

​ 2. 仿真时间设置：setRealTimeSimulation()

单独使用时：同上。按照RTC（Real-Time Colck）进行控制，类似与stepSimulation(delta)模式，频率为10000HZ的样子。

与time.sleep(t) 连用时：仿真引擎每 t 秒采样一次控制命令，然后按照RTC进行控制直到下一次采样。

位置控制

p.setJointMotorControl2(quadruped, jointIds[j], p.POSITION_CONTROL, targetPos, force=maxForce)

​ 1. 仿真时间设置：stepSimulation()

即使当前时间步内没有到达需要的位置，也会继续执行下一步仿真。如果在该时间步内达到位置，则等待时间步结束，然后开始下一个时间步仿真。

与time.sleep(t) 连用时：仿真引擎暂停 t 秒，然后再让仿真引擎进入下一个时间步。t 变化不影响仿真结果。

​ 2. 仿真时间设置：setRealTimeSimulation()

单独使用时：不建议使用，该模式要求尽可能快的到达指定位置。如果没有到达，则继续开始下一个指令

与time.sleep(t) 连用时：仿真引擎每 t 秒采样一次控制命令，然后按照RTC进行控制直到下一次采样。

仿真时间

• stepSimulation()

1. 单独使用

该API用来完成一步仿真，默认仿真步长为 1/ 240s。只有运行该函数时，仿真引擎才会运行。

力矩控制模式：会在整个仿真时间步都施加相应的力矩。仿真时间步越长，力矩持续时间越长。

位置控制模式：即使当前时间步内没有到达需要的位置，也会继续执行下一步仿真。如果在该时间步内达到位置，则等待时间步结束，开始下一个时间步仿真。

2. 与*p.setTimeStep(1/240)*连用 :

用来设置仿真时间步长，即设置仿真引擎在 1/240s 内运行一次。

3. 与*time.sleep(10)*连用 :

该函数与 stepSimulation() API一起连用时，无论是力控模式还是位控模式都不影响仿真结果, 只是方便观察

作用：仿真引擎暂停 10s 然后再让仿真引擎进入下一个时间步。

• setRealTimeSimulation()

setRealTimeSimulation() 对 DIRECT 模式没有影响。

​ 1. 单独使用

力矩控制模式1：推荐使用。物理引擎会尽快的采样控制命令，然后一直执行直到下一次采样。

位置控制模式：不建议使用，该模式要求尽可能快的到达指定位置。但是仿真引擎达不到那么快的响应。

​ 2. 与 time.sleep(10) 连用

该函数会影响仿真结果，在该10秒内仿真引擎会继续运行。

力矩控制模式1(p.VELOCITY_CONTROL)：仿真引擎每 10秒 采样一次控制命令，然后一直执行该命令直到下一次采样。

力矩控制模式2(p.TORQUE_CONTROL) : 关节控制指令会暂停10秒，10秒后再执行下一个控制指令。

位置控制模式：同力矩控制模式2。

• 注意:

不能将 p.setRealTimeSimulation() 和 p.stepSimulation() 一同使用

一同使用时，仅运行p.stepSimulation()的相应模式，不会运行p.setRealTimeSimulation() 。

状态反馈

• getBasePositionAndOrientation

作用：获得笛卡尔世界坐标系下的机身当前位置和姿态，其中姿态为四元数的表达形式。
输入参数：机身ID
输出参数：位置向量[x,y,z], 姿态向量[x, y, z, w]

• getEulerFromQuaternion

作用：将四元数格式转化为欧拉角格式
输入参数：四元数格式[x, y, z, w]
输出参数：欧拉角RPY格式[roll, pitch, yaw]

• invertTransform

作用：

• multiplyTransforms

作用：转换一种坐标系到另一种坐标系系统
输入参数：A的位姿，B的位姿（位姿代表位置和姿态）
输出参数：位置向量，姿态向量[x, y, z, w]

状态估计器用到的API

• getBaseVelocity

作用：获得笛卡尔世界坐标系下的机身的线速度和角速度
输入参数：机身ID
输出参数：线速度[x, y, z], 角速度[wx, wy, wz]。

摆动相轨迹用到的API