机器人手臂的简单移动

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

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

loadSDF, loadMJCF

一般返回的列表的第一个元素是加载对象的唯一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)#按照以上的参数重新设置机器人的位置。

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

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

有一点是需要注意的是在我这种情况下调用"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）

运动过程1

运动过程2

机器人结点解析图：

请添加图片描述

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