PyBullet (二) 机器人手臂的简单移动（有需要更新的内容）

申明：本人今年博一（地点英国），项目是与机器人有关的内容。承接上文，mini-project（由于这周要做另一个mini-project2，所以现在才完成系列文章）。

系列文章目录：
上一篇：
PyBullet (一) 在Ubuntu20.04.1中安装PyBullet（VMware Workstation 16.x Pro）
下一篇：
PyBullet (三) 将圆柱体看作机器人的加速和减速运动机

虚拟机：VMware Workstation 16.x Pro
系统：Ubuntu20.04.1（官方下载链接）
PyBullet：（官方链接）（官方指南）

1. 整体思路

首先要创建整个机器人运行的环境。由于环境是自己创建的，所以知道环境中所有物体的位置。知道物体位置之后设定目标点，然后将目标点传入机器人手臂中。接着机器人手臂就可以朝着目标点移动了。

2. 代码解析

2.1 代码分步

2.1.1 库

import pybullet as p
import time
import pybullet_data
import numpy as np

最基础的导入包啊，库啊

2.1.2 连接物理模拟

physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version，"连接 "物理模拟。
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally

导入PyBullet模块后，首先要做的是 "连接 "物理模拟。PyBullet是围绕客户端-服务器驱动的API设计的，客户端发送命令，物理服务器返回状态。PyBullet有一些内置的物理服务器。DIRECT和GUI。GUI和DIRECT连接都会在与PyBullet相同的过程中执行物理模拟和渲染。

2.1.3 重力设置

p.setGravity(0,0,-9.81)

设置重力系数分别是：x方向（+：x轴正半轴；-：x轴负半轴）；y方向（+：y轴正半轴；-：y轴负半轴）；z方向（+：z轴正半轴；-：z轴负半轴）。

2.1.4 模型加载（一）

planeId = p.loadURDF("plane.urdf")
robotId = p.loadSDF("/home/xzs/PyBullet_Practice/bullet3-master/data/kuka_iiwa/kuka_with_gripper.sdf")

a.加载平面环境

plane.urdf中的内容：

<?xml version="1.0" ?>
<robot name="plane">
  <link name="planeLink">
  <contact>
      <lateral_friction value="1"/>
  </contact>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
       <mass value=".0"/>
       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
				<mesh filename="plane.obj" scale="1 1 1"/>
      </geometry>
       <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -5"/>
      <geometry>
	 	<box size="30 30 10"/>
      </geometry>
    </collision>
  </link>
</robot>

b.加载机器人

kuka_with_gripper.sdf中的内容：
（由于太长可以自行到GitHub上下载：下载链接）

有很多不同的加载模型的代码：loadURDF, loadSDF, loadMJCF

loadURDF

必需/可选参数	参数名字	参数类型	介绍
必需	fileName	string	物理服务器文件系统中URDF文件的相对或绝对路径。
可选	basePosition	vec3	在世界空间坐标[X,Y,Z]的指定位置创建对象的基座。注意，这个位置是URDF链接位置。如果惯性框架是非零，这与质量中心位置不同。使用resetBasePositionAndOrientation来设置质量中心的位置/方向。
可选	baseOrientation	vec4	以世界空间四元数[X,Y,Z,W]的形式在指定的方向上创建对象的基座。参见basePosition的说明。
可选	useMaximalCoordinates	int	实验性的。默认情况下，URDF文件中的关节是用缩减坐标法创建的：joints 使用 Featherstone 铰接体算法（ABA，Bullet 2.x 中的 btMultiBody）进行模拟。useMaximalCoordinates选项将为每个环节创建一个6自由度的刚体，这些刚体之间的约束被用来模拟关节。
可选	useFixedBase	int	强制加载对象的基座为静态。
可选	flags	int	以下标志可以使用位性OR组合: URDF_MERGE_FIXED_LINKS: 这将从URDF文件中删除固定链接，并合并产生的链接。这对性能很有好处，因为各种算法（关节体算法、正向运动学等）的关节数量都是线性复杂度，包括固定关节。URDF_USE_INERTIA_FROM_FILE: 默认情况下，Bullet 根据碰撞形状的质量和体积重新计算惯性张量。如果你能提供更精确的惯性张量，请使用这个标志。 URDF_USE_SELF_COLLISION：默认情况下，Bullet 是不允许自碰撞的。这个标志可以让你启用它。你可以使用以下标志自定义自碰撞行为: URDF_USE_SELF_COLLISION_INCLUDE_PARENT将启用子代和父代之间的碰撞，默认为禁用。需要和URDF_USE_SELF_COLLISION标志一起使用。URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS 将丢弃一个子链接和它的任何祖先（父母，父母的父母，直到基数）之间的自碰撞。需要配合使用URDF_USE_SELF_COLLISION。URDF_USE_IMPLICIT_CYLINDER, 将使用一个平滑的隐式圆柱体。默认情况下，Bullet 会将圆柱体处理成一个凸形的船体。URDF_ENABLE_SLEEPING, 将允许在一个物体一段时间没有移动后禁用模拟。与活动体的交互将重新开启模拟。URDF_INITIALIZE_SAT_FEATURES，将为凸形创建三角形网格。这将改善可视化，并允许使用分离轴测试（SAT）来代替GJK/EPA。需要使用setPhysicsEngineParameter启用SAT。URDF_USE_MATERIAL_COLORS_FROM_MTL,将。使用Wavefront OBJ文件中的RGB颜色，而非 的URDF文件。URDF_ENABLE_CACHED_GRAPHICS_SHAPES，将使缓存和重复使用图形形状。这将提高具有类似图形资产的文件的加载性能。URDF_MAINTAIN_LINK_ORDER，将尝试保持URDF文件中的链接顺序。比如说在URDF文件中，顺序是。ParentLink0, ChildLink1 (连接到ParentLink0), ChildLink2 (连接到ParentLink0). 如果没有这个标志，顺序可能是P0、C2、C1。
可选	globalScaling	float	globalScaling将对URDF模型应用一个缩放因子。
可选	physicsClientId	int	如果你连接到多个服务器，你可以选择一个。

loadURDF返回一个主体唯一id，一个非负的整数值。如果URDF文件不能被加载，这个整数将是负值，而不是有效的主体唯一id。

loadSDF, loadMJCF

必需/可选参数	参数名字	参数类型	介绍
必需	fileName	string	物理服务器文件系统中URDF文件的相对或绝对路径。
可选	useMaximalCoordinates	int	实验性的。详见loadURDF。
可选	globalScaling	float	globalScaling支持SDF和URDF，不支持MJCF。每一个对象都将使用这个缩放因子进行缩放（包括链接、链接框架、关节附件和线性关节限制）。这对质量没有影响，只对几何体有影响。如果需要的话，请使用changeDynamics来改变质量。
可选	physicsClientId	int	如果你连接到多个服务器，你可以选择一个。

loadBullet、loadSDF和loadMJCF将返回一个对象唯一id的数组。

参数名字	参数类型	介绍
objectUniqueIds	list of int	列表中包括每个加载对象的唯一ID。

一般返回的列表的第一个元素是加载对象的唯一ID

2.1.5 设置基础参数（一）

robotStartPos = [0,0,0]#机器人的初始位置
cylinderStartPos = [1,0,0.3]#圆柱体的初始位置（在环境中我将一个圆柱体设置为平台（一个台子而已，可以是任何东西））
boxStartPos = [1,0,0.6 + 0.05 + 0.01]#箱子的基础位置（我在台子上放了一个箱子当作目标，为以后机器人的抓取做准备）
robotStartOrientation = p.getQuaternionFromEuler([0,0,0])#（机器人）会转换成一个四元数，可以理解成能够控制模型在空间中绕x，y，z轴旋转的参数。（参数是角度。e.g. [3.14,0,0] == [pai,0,0];[1.57,0,0] == [pai/2,0,0]。参数的正负表示旋转的不同方向。）
cylinderStartOrientation = p.getQuaternionFromEuler([0,0,0])#（圆柱体）同上
boxStartOrientation = p.getQuaternionFromEuler([0,0,0])#（箱子）同上

p.resetBasePositionAndOrientation(robotId[0],robotStartPos,robotStartOrientation)#按照以上的参数重新设置机器人的位置。

resetBasePositionAndOrientation

必需/可选参数	参数名字	参数类型	介绍
必需	bodyUniqueId	int	加载模型的唯一ID。（loadURDF, loadSDF, loadMJCF返回的ID）
必需	posObj	vec3	在世界空间坐标[X,Y,Z]中的指定位置重置对象的底座。
必需	ornObj	vec4	以指定的世界空间四元数[X,Y,Z,W]重置对象的基座。
可选	physicsClientId	int	如果你连接到多个服务器，你可以选择一个。

2.1.6 模型加载（二）

cylinderId = p.loadURDF("/home/xzs/PyBullet_Practice/bullet3-master/data/cylinder1.urdf",cylinderStartPos,cylinderStartOrientation)
boxId = p.loadURDF("/home/xzs/PyBullet_Practice/bullet3-master/data/cube1.urdf",boxStartPos,boxStartOrientation)

cylinder1.urdf：

<?xml version="1.0"?>
 <robot name="myfirst">
 
   <material name="blue">
     <color rgba="0 1 1 1"/>
   </material>
   
   <link name="base_link">
     <visual>
       <geometry>
         <cylinder length="0.6" radius="0.2"/>
       </geometry>
       <material name="blue"/>
     </visual>
     <collision>
       <geometry>
         <cylinder length="0.6" radius="0.2"/>
       </geometry>
     </collision>
     <inertial>
       <mass value="1"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
   
 </robot>

cube1.urdf：

<?xml version="1.0"?>
 <robot name="myfirst">
   <material name="blue">
     <color rgba="0 0 1 1"/>
   </material>
   <link name="right_leg">
     <visual>
       <geometry>
         <box size="0.1 0.1 0.1"/>
       </geometry>
       <material name="blue"/>
     </visual>
     <collision>
       <geometry>
         <box size="0.1 0.1 0.1"/>
       </geometry>
     </collision>
     <inertial>
       <mass value="1"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
 </robot>

2.1.7 设置基础参数（二）

robot7StartPos = [0,0,1.2]#机器人第7个结点的初始位置
robotEndPos = [0.75,0,0.625]#机器人第7个结点的结束位置
robotEndOrientation = p.getQuaternionFromEuler([1.57,0,1.57])#同上
startPos_array = np.array(robot7StartPos)#将参数转换成array然后才能进行“+，-，*，/”运算
endPos_array = np.array(robotEndPos)#同上
stepNum = 5#我将移动过程分成了5步
step_array = (endPos_array - startPos_array)/stepNum#每一步走的长度（其实就是每一步x,y,z坐标的变化量）

2.1.8 移动开始

for j in range(stepNum):#循环次数
    print(j,"step")
    robotStepPos = list(step_array + startPos_array)#转换回list
    targetPositionsJoints = p.calculateInverseKinematics(robotId[0],7,robotStepPos,targetOrientation = robotEndOrientation)#计算IK的解决方案（个人理解）（下面会具体介绍参数）
    p.setJointMotorControlArray(robotId[0],range(11),p.POSITION_CONTROL,targetPositions = targetPositionsJoints)#执行方案（下面会具体介绍参数）
    for i in range (100):#移动时间
        p.stepSimulation()#这个是一定要写的，每次移动都要调用这个方法
        time.sleep(1./10.)
        print("i:",i)
    print("------------------------------------------------------------------------------")
    startPos_array = np.array(robotStepPos)#因为移动过了，所以更新一下坐标

calculateInverseKinematics

必需/可选参数	参数名字	参数类型	介绍
必需	bodyUniqueId	int	加载模型的唯一ID。（loadURDF, loadSDF, loadMJCF返回的ID）
必需	endEffectorLinkIndex	int	末端效应链接指数
必需	targetPosition	vec3, list of 3 floats	终端效应器的目标位置（它的链接坐标，而不是质量中心坐标！）。默认情况下，这是在笛卡尔世界空间，除非你提供了 currentPosition 连接角。
可选	targetOrientation	vec3, list of 4 floats	加目标方向在笛卡尔世界空间，四元数[x,y,w,z]。如果没有指定，将使用纯位置IK。
可选	lowerLimits	list of floats [0…nDof]	可选的空空间IK需要所有4个列表（下限、上限、关节范围、休息姿势）。否则将使用常规IK。只为有这些限制的关节提供限制（跳过固定关节），所以长度是自由度数。请注意， lowerLimits。upperLimits、jointRanges很容易导致 冲突和IK解决方案的不稳定性。试试 首先使用的范围和限制很广，只用了 休息的姿势。
可选	upperLimits	list of floats [0…nDof]	可选空位IK需要全部4个列表 (lowerLimits, upperLimits, jointRanges, restPoses）。) 否则将使用常规IK。lowerLimit和upperLimit指定关节极限。
可选	jointRanges	int	可选的空空间IK需要所有4个列表（下限、上限、关节范围、休息姿势）。否则将使用常规IK。
可选	restPoses	list of floats [0…nDof]	可选的空位IK需要所有4个列表（下限、上限、关节范围、休息姿势）。否则将使用常规的IK。偏向于选择一个更接近给定休息姿势的IK方案。
可选	jointDamping	list of floats [0…nDof]	jointDamping允许使用联合阻尼系数来调整IK解决方案。
可选	solver	int	p.IK_DLS或p.IK_SDLS，Damped Least Squares或Selective Damped Least Squares，如Samuel Buss的论文 "Selectively Damped Least Squares for Inverse Kinematics "中所述。
可选	currentPosition	list of floats [0…nDof]	关节位置列表。默认情况下，PyBullet使用主体的关节位置。如果提供了，targetPosition和targetOrientation是在局部空间的!
可选	maxNumIterations	int	完善IK解决方案，直到目标和实际末端效应器位置之间的距离低于该阈值，或者达到maxNumIterations。默认为20次迭代。）
可选	residualThreshold	double	完善IK解决方案，直到目标和实际末端效果器位置之间的距离低于这个阈值，或者达到最大迭代次数。
可选	physicsClientId	int	如果您连接到多个服务器，您可以 可以选一个。

setJointMotorControlArray
可以为所有的输入传递数组，而不是为每个关节进行单独的调用，以极大地减少调用开销。
setJointMotorControlArray的参数与setJointMotorControl2相同，只是用整数列表代替了整数。

必需/可选参数	参数名字	参数类型	介绍
必需	bodyUniqueId	int	加载模型的唯一ID。（loadURDF, loadSDF, loadMJCF返回的ID）
必需	jointIndices	list of int	在范围[0…getNumJoints(bodyUniqueId)]中的索引（注意链接指数==联合指数)
必需	controlMode	int	POSITION_CONTROL、VELOCITY_CONTROL、TORQUE_CONTROL、PD_CONTROL。(还有实验性的STABLE_PD_CONTROL用于稳定(隐式)的PD控制，这需要额外的准备。)
可选	targetPositions	list of float	在POSITION_CONTROL中，targetValue是关节的目标位置。
可选	targetVelocities	list of float	在PD_CONTROL、VELOCITY_CONTROL和POSITION_CONTROL中，目标值是关节的目标速度。
可选	forces	list of float	在PD_CONTROL、POSITION_CONTROL和VELOCITY_CONTROL中，这是达到目标值的最大马达力。在TORQUE_CONTROL中，这是每个模拟步骤所要施加的力/扭矩。
可选	positionGains	int	（实在不好意思，我没找到官方文档在哪里说明了这个，如果以后找到，我会回来补充）（官方文档）
可选	velocityGains	int	（实在不好意思，我没找到官方文档在哪里说明了这个，如果以后找到，我会回来补充）（官方文档）
可选	physicsClientId	int	如果您连接到多个服务器，您可以 可以选一个。

有一点是需要注意的是在我这种情况下调用"p.calculateInverseKinematics"之后得到一个长度为十一的列表，所以在接下来调用"p.setJointMotorControlArray"的时候，里面的参数"jointIndices = range(11)“是"11”，虽然整个机器人有14个结点，但是这里需要11个。

2.1.9 程序结束

p.disconnect()#有连接就有断开

2.2 代码总体

import pybullet as p
import time
import pybullet_data
import numpy as np
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-9.81)
planeId = p.loadURDF("plane.urdf")
robotId = p.loadSDF("/home/xzs/PyBullet_Practice/bullet3-master/data/kuka_iiwa/kuka_with_gripper.sdf")

robotStartPos = [0,0,0]
cylinderStartPos = [1,0,0.3]
boxStartPos = [1,0,0.6 + 0.05 + 0.01]
robotStartOrientation = p.getQuaternionFromEuler([0,0,0])
cylinderStartOrientation = p.getQuaternionFromEuler([0,0,0])
boxStartOrientation = p.getQuaternionFromEuler([0,0,0])

p.resetBasePositionAndOrientation(robotId[0],robotStartPos,robotStartOrientation)
cylinderId = p.loadURDF("/home/xzs/PyBullet_Practice/bullet3-master/data/cylinder1.urdf",cylinderStartPos,cylinderStartOrientation)
boxId = p.loadURDF("/home/xzs/PyBullet_Practice/bullet3-master/data/cube1.urdf",boxStartPos,boxStartOrientation)

p.getNumJoints(robotId[0])#得到机器人的节点总数
p.getJointInfo(robotId[0],7)#得到机器人结点的信息
robot7StartPos = [0,0,1.2]
robotEndPos = [0.75,0,0.625]
robotEndOrientation = p.getQuaternionFromEuler([1.57,0,1.57])
startPos_array = np.array(robot7StartPos)
endPos_array = np.array(robotEndPos)
stepNum = 5
step_array = (endPos_array - startPos_array)/stepNum
for j in range(stepNum):
    print(j,"step")
    robotStepPos = list(step_array + startPos_array)
    targetPositionsJoints = p.calculateInverseKinematics(robotId[0],7,robotStepPos,targetOrientation = robotEndOrientation)
    p.setJointMotorControlArray(robotId[0],range(11),p.POSITION_CONTROL,targetPositions = targetPositionsJoints)
    for i in range (100):
        p.stepSimulation()
        time.sleep(1./10.)
        print("i:",i)
    print("------------------------------------------------------------------------------")
    startPos_array = np.array(robotStepPos)
p.disconnect()

3. 效果展示

视频演示（Bilibili）
视频演示（YouTube）

机器人结点解析图：

4. 总结

这次的mini-project也算简单，很容易就能实现，后续学习内容我一会慢慢逐一完成发布到博客里，希望大家共同进步，有问题想要讨论的尽管发在评论区。由于我是初学者，可能有好多问题我也不会，到时候大家一起讨论，共同进步！！！有以后学到的内容，我会持续更新的博客中！！！