机器人手臂的简单移动

1. 整体思路2. 代码解析2.1 代码分步2.1.1 库2.1.2 连接物理模拟2.1.3 重力设置2.1.4 模型加载（一）2.1.5 设置基础参数（一）2.1.6 模型加载（二）2.1.7 设置基础参数（二）2.1.8 移动开始2.1.9 程序结束 2.2 代码总体 3. 效果展示4. 总结

申明：本人今年博一（地点英国），项目是与机器人有关的内容。承接上文，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

必需/可选参数参数名字参数类型介绍必需fileNamestring物理服务器文件系统中URDF文件的相对或绝对路径。可选basePositionvec3在世界空间坐标[X,Y,Z]的指定位置创建对象的基座。注意，这个位置是URDF链接位置。如果惯性框架是非零，这与质量中心位置不同。使用resetBasePositionAndOrientation来设置质量中心的位置/方向。可选baseOrientationvec4以世界空间四元数[X,Y,Z,W]的形式在指定的方向上创建对象的基座。参见basePosition的说明。可选useMaximalCoordinatesint实验性的。默认情况下，URDF文件中的关节是用缩减坐标法创建的：joints 使用 Featherstone 铰接体算法（ABA，Bullet 2.x 中的 btMultiBody）进行模拟。useMaximalCoordinates选项将为每个环节创建一个6自由度的刚体，这些刚体之间的约束被用来模拟关节。可选useFixedBaseint强制加载对象的基座为静态。可选flagsint以下标志可以使用位性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。可选globalScalingfloatglobalScaling将对URDF模型应用一个缩放因子。可选physicsClientIdint如果你连接到多个服务器，你可以选择一个。

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

loadSDF, loadMJCF

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

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

参数名字参数类型介绍objectUniqueIdslist 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

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

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

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

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

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

有一点是需要注意的是在我这种情况下调用"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也算简单，很容易就能实现，后续学习内容我一会慢慢逐一完成发布到博客里，希望大家共同进步，有问题想要讨论的尽管发在评论区。由于我是初学者，可能有好多问题我也不会，到时候大家一起讨论，共同进步！！！有以后学到的内容，我会持续更新的博客中！！！