PyBullet (四) 将圆柱体看作机器人，推动目标，让目标按照输入的路径在可移动障碍物中移动

本文主要是介绍PyBullet (四) 将圆柱体看作机器人，推动目标，让目标按照输入的路径在可移动障碍物中移动，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

将圆柱体看作机器人，推动目标，让目标按照输入的路径在可移动障碍物中移动

1. 整体思路
- 1.1 分情况讨论
- - 1.1.1 锐角/直角 + 锐角
  - - 1.1.1.1 计算垂足
    - 1.1.1.2 计算距离
    - 1.1.1.3 向“上”移动
    - 1.1.1.4 “推”
  - 1.1.2 钝角 + 锐角
  - - 1.1.2.1 计算垂足
    - 1.1.2.2 计算方向
  - 1.1.3 锐角 + 直角
  - - 1.1.3.1 “1.1.1.1”
    - 1.1.3.2 向“上”移动
  - 1.1.4 锐角 + 钝角
  - - 1.1.4.1 移动
- 1.2 方法总结
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.8.1 求两直线交点
    - 2.1.8.2 计算垂心（1.1.1.1）
    - 2.1.8.3 计算移动的方向（1.1.1.2）
    - 2.1.8.3 计算向“上”移动的方向（1.1.1.3）
    - 2.1.8.4 first move（1.1.1.2）
    - 2.1.8.5 second move（1.1.1.3）
    - 2.1.8.6 third move（1.1.1.4）
    - 2.1.8.7 向“下”移动（1.1.2.2）
    - 2.1.8.8 第一种情况
    - 2.1.8.9 第二种情况
    - 2.1.8.10 第三种情况
    - 2.1.8.11 第四种情况
    - 2.1.8.12 主函数
  - 2.1.9 程序结束
- 2.2 代码总体
3. 效果展示
4. 总结

申明：本人今年博一（地点英国），项目是与机器人有关的内容。第三次导师见面，导师布置的另一个mini-experiment，用到“PyBullet”。

系列文章目录：
上一篇：
PyBullet (三) 将圆柱体看作机器人的加速和减速运动
下一篇：
PyBullet (五)（循环+优化）将圆柱体看作机器人，推动目标，让目标按照输入的路径在可移动障碍物中移动

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

1. 整体思路

将圆柱体看作是机器人来进行移动，给定机器人一个速度，通过不断调整机器人的速度推动目标，让目标按照指定路径移动。下面介绍的方法算是一种比较“蠢”的方法，只当作是为熟练使用PyBullet的一个铺垫。思路略显臃肿，并且有局限性，但是容易理解，在这里只提供一个思路，不建议使用。

1.1 分情况讨论

本方法将机器人，目标以及路径的位置关系分成4种情况讨论：

锐角/直角 + 锐角
钝角 + 锐角
锐角 + 直角
锐角 + 钝角

当然前提是我们已知机器人，目标和路径的在环境中的坐标位置。并且机器人和目标都是圆柱体。

1.1.1 锐角/直角 + 锐角

先上图片：
1.1
锐角/直角+锐角：向量RT * 向量RP >= 0
锐角：向量PT * 向量PR > 0

1.1.1.1 计算垂足

过T点向直线RP做垂线，设垂足为O：
1.2

1.1.1.2 计算距离

接下来要计算线段RO的距离(DistanceRO)，机器人R的半径(RadiusR)以及目标的半径(RadiusT)。然后我会比较他们之间的大小。
if DistanceRO <= RadiusR + RadiusT, 机器人会沿向量PR的方向移动，如图所示：
1.3
当然我会有函数用来实时检测距离，防止机器人太过于远离目标T和路径P。

if DistanceRO >> RadiusR + RadiusT(我的意思是远远大于)，那么机器人会沿向量RP的方向移动，如图所示：
1.4
当然我会有函数用来实时检测距离，防止机器人太过于接近目标T和路径P。
所以最后机器人会停在一个区间内。

1.1.1.3 向“上”移动

过点R做直线RP的垂线，然后该垂线与线段TP的延长线交与一点C：
1.5
然后机器人R会朝向点C移动，当然在移动的过程中，每时每刻都在重复运行1.1.1.1、1.1.1.2以及1.1.1.3的步骤，到最后会出现下面这种情况：
1.6

1.1.1.4 “推”

然后机器人会朝向点P运动，也就是推动目标T。在推动的过程中，必定会推歪，所以我让机器人先朝向点P运动几秒钟，然后重新执行1.1.1.1、1.1.1.2以及1.1.1.3这三个步骤，然后再执行1.1.1.4。这样就实现了实时矫正目标T的位置。

1.1.2 钝角 + 锐角

先上图片：
1.7
钝角：向量RT * 向量RP < 0
锐角：向量PT * 向量PR > 0

1.1.2.1 计算垂足

同样的操作，过T点向直线RP做垂线，设垂足为O：
1.8

1.1.2.2 计算方向

有了垂足O就能知道向量TO的方向，然后让机器人R按照向量TO的方向从O点移动，直到向量RT * 向量RP > 0：
1.9
机器人移动之后我们会发现位置的关系变成了第一种，然后剩下的步骤就可以按照第一种（1.1.1.1、1.1.1.2、1.1.1.3和1.1.1.4）情况来执行。

1.1.3 锐角 + 直角

先上图片：
1.10
锐角：向量RT * 向量RP > 0
直角：向量PT * 向量PR = 0

1.1.3.1 “1.1.1.1”

先按照1.1.1.1的步骤执行：
1.11

1.1.3.2 向“上”移动

我们直到向量PT的方向，我们让机器人R从R点出发，向向量PT的方向移动：
1.12
机器人移动之后我们会发现位置的关系变成了第一种，然后剩下的步骤就可以按照第一种（1.1.1.1、1.1.1.2、1.1.1.3和1.1.1.4）情况来执行。

1.1.4 锐角 + 钝角

先上图片：
1.13
锐角：向量RT * 向量RP > 0
钝角：向量PT * 向量PR < 0

1.1.4.1 移动

我们让机器人R从R点出发按照向量RP的方向移动，直到向量PT * 向量PR > 0:
1.14
然后我会重新计算向量RT * 向量RP和向量 PT* 向量PR 的值然后根据他们的值来进行1，2，3情况的分类。

1.2 方法总结

方法的最后会让目标按照路径Path移动。略显臃肿的方法，但是对于初学者来说更容易上手，更容易理解。方法有局限性，机器人和目标都是在圆柱体的情况下，所以只适用与部分环境。当然后续会更新稍微好一点的算法。当然算法没有最好，只有更好。

2. 代码解析

2.1 代码分布

2.1.1 库

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

最基础的导入包啊，库啊

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 设置基础参数

cylinder_Obstacle_1_StartPos = [-0.4, 0.3, 0.2 ]#圆柱体障碍物的初始位置
cylinder_Obstacle_2_StartPos = [ 0.0,-0.6, 0.15]
cylinder_Obstacle_3_StartPos = [ 0.6,-0.6, 0.1 ]
cylinder_Obstacle_4_StartPos = [ 0.8, 0.2, 0.1]
cylinder_target_StartPos = [ 0, 0.0, 0.3]
cylinder_robot_StartPos = [ -0.8, 0.0, 0.3]cylinder_Obstacle_1_StartOrientation = 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]。参数的正负表示旋转的不同方向。）
cylinder_Obstacle_2_StartOrientation = p.getQuaternionFromEuler([0,0,0])
cylinder_Obstacle_3_StartOrientation = p.getQuaternionFromEuler([0,0,0])
cylinder_Obstacle_4_StartOrientation = p.getQuaternionFromEuler([0,0,0])
cylinder_target_StartOrientation = p.getQuaternionFromEuler([0,0,0])
cylinder_robot_StartOrientation = p.getQuaternionFromEuler([0,0,0])

2.1.5 模型加载

cylinder_Obstacle_1_Id = p.loadURDF("D:/Study/PhD/2020.10.15-2020.10.21/object/cylinder_obstacle1.urdf",cylinder_Obstacle_1_StartPos,cylinder_Obstacle_1_StartOrientation)
cylinder_Obstacle_2_Id = p.loadURDF("D:/Study/PhD/2020.10.15-2020.10.21/object/cylinder_obstacle2.urdf",cylinder_Obstacle_2_StartPos,cylinder_Obstacle_2_StartOrientation)
cylinder_Obstacle_3_Id = p.loadURDF("D:/Study/PhD/2020.10.15-2020.10.21/object/cylinder_obstacle3.urdf",cylinder_Obstacle_3_StartPos,cylinder_Obstacle_3_StartOrientation)
cylinder_Obstacle_4_Id = p.loadURDF("D:/Study/PhD/2020.10.15-2020.10.21/object/cylinder_obstacle4.urdf",cylinder_Obstacle_4_StartPos,cylinder_Obstacle_4_StartOrientation)
cylinder_target_Id = p.loadURDF("D:/Study/PhD/2020.10.15-2020.10.21/object/cylinder_target.urdf",cylinder_target_StartPos,cylinder_target_StartOrientation)
cylinder_robot_Id = p.loadURDF("D:/Study/PhD/2020.10.15-2020.10.21/object/cylinder_robot.urdf",cylinder_robot_StartPos,cylinder_robot_StartOrientation)

cylinder_obstacle1.urdf：

<?xml version="1.0"?><robot name="myfirst"><material name="red"><color rgba="0.5 0 0 1"/></material><link name="base_link"><visual><geometry><cylinder length="0.4" radius="0.2"/></geometry><material name="red"/></visual><collision><geometry><cylinder length="0.4" 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>

cylinder_obstacle2.urdf：

<?xml version="1.0"?><robot name="myfirst"><material name="red"><color rgba="0.5 0 0 1"/></material><link name="base_link"><visual><geometry><cylinder length="0.3" radius="0.3"/></geometry><material name="red"/></visual><collision><geometry><cylinder length="0.3" radius="0.3"/></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>

cylinder_obstacle3.urdf：

<?xml version="1.0"?><robot name="myfirst"><material name="red"><color rgba="0.5 0 0 1"/></material><link name="base_link"><visual><geometry><cylinder length="0.2" radius="0.1"/></geometry><material name="red"/></visual><collision><geometry><cylinder length="0.2" radius="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>

cylinder_obstacle4.urdf：

<?xml version="1.0"?><robot name="myfirst"><material name="red"><color rgba="0.5 0 0 1"/></material><link name="base_link"><visual><geometry><cylinder length="0.2" radius="0.3"/></geometry><material name="red"/></visual><collision><geometry><cylinder length="0.2" radius="0.3"/></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>

cylinder_target.urdf：

<?xml version="1.0"?><robot name="myfirst"><material name="green"><color rgba="0 1 0 1"/></material><link name="base_link"><visual><geometry><cylinder length="0.6" radius="0.2"/></geometry><material name="green"/></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>

cylinder_robot.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>

具体参数细节请访问PyBullet (二) 机器人手臂的简单移动中的“2.14模型加载（一）”

2.1.6 参数检测

robot_shape = p.getVisualShapeData(cylinder_robot_Id)#得到有关模型形状的一些列信息
target_shape = p.getVisualShapeData(cylinder_target_Id)
robot_R = robot_shape[0][3][1]#半径
robot_H = robot_shape[0][3][0]#高度
target_R = target_shape[0][3][1]
target_H = target_shape[0][3][0]

getVisualShapeData
您可以使用getVisualShapeData访问视觉形状信息。您可以使用它将您自己的呈现方法与PyBullet模拟连接起来，并在每个模拟步骤之后手动同步世界转换。您还可以使用GET网格数据，特别是对于可变形对象，来接收有关顶点位置的数据。
输入参数：

必需/可选参数	参数名字	参数类型	介绍
必需	objectUniqueId	int	对象的唯一id，由加载方法返回。
可选	flags	int	VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS还将提供纹理独特的ID。
可选	physicsClientId	int	物理客户端id由“连接”返回。

输出参数：

参数名字	参数类型	介绍
objectUniqueId	int	对象的唯一id，由加载方法返回。
linkIndex	int	基础的链接索引或-1。
visualGeometryType	int	视觉几何类型(TBD)。
dimensions	vec3, list of 3 floats	几何尺寸(尺寸，局部尺度）。
meshAssetFileName	string, list of chars	到三角形网格的路径，如果有的话。通常相对于URDF、SDF或MJCF文件位置，但可能是绝对的。
localVisualFrame position	vec3, list of 3 floats	局部视觉框架的位置，相对于链接/关节框架。
localVisualFrame orientation	vec4, list of 4 floats	局部视觉框架相对于链路/关节框架的方向。
rgbaColor	vec4, list of 4 floats	红色/绿色/蓝色/阿尔法的URDF颜色(如果指定的话）。
textureUniqueId	int	(字段只存在于使用VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS标志时)纹理形状的唯一id，或-1如果没有。

2.1.7 路径输入

path_string = input("Please input the path:")
path_string = path_string.replace('[','')
path_string = path_string.replace(']','')
path_string = path_string.split(',')
length = len(path_string)
little_length = length/3
path = []
little_length = int(little_length)
for i in range(little_length):path.append([])for j in range(3):path[i].append(0.4)
path_flag = 0
for i in range(little_length):for j in range(3):path[i][j] = float(path_string[path_flag])path_flag = path_flag + 1
#path = [[0.4,0.0,0.3],[0.6,0.0,0.3],[0.8,0.0,0.3],[1.0,0.0,0.3],[1.2,0.0,0.3]]

涉及到数据处理，这里就不再赘述了（一定会有处理的比较好的同学，欢迎评论区讨论）。

2.1.8 各类函数

2.1.8.1 求两直线交点

这里借鉴了博主“AD稳稳”的方法，自己稍作了修改，博客链接私戳：python 计算两直线交点

def cross_point(line1,line2):#计算交点函数x1=line1[0]#取四点坐标y1=line1[1]x2=line1[2]y2=line1[3]x3=line2[0]y3=line2[1]x4=line2[2]y4=line2[3]if (x4-x3)==0:#L2直线斜率不存在操作k2=Noneb2=0x=x3k1=(y2-y1)*1.0/(x2-x1)#计算k1,由于点均为整数，需要进行浮点数转化b1=y1*1.0-x1*k1*1.0#整型转浮点型是关键y=k1*x*1.0+b1*1.0elif (x2-x1)==0:#L1直线斜率不存在操作k1=Noneb1=0x=x1k2=(y4-y3)*1.0/(x4-x3)b2=y3*1.0-x3*k2*1.0y=k2*x*1.0+b2*1.0else:k1=(y2-y1)*1.0/(x2-x1)#计算k1,由于点均为整数，需要进行浮点数转化k2=(y4-y3)*1.0/(x4-x3)#斜率存在操作b1=y1*1.0-x1*k1*1.0#整型转浮点型是关键b2=y3*1.0-x3*k2*1.0x=(b2-b1)*1.0/(k1-k2)y=k1*x*1.0+b1*1.0    return [x,y]

2.1.8.2 计算垂心（1.1.1.1）

def orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y):if (path_y - robot_y) == 0 and (path_x - robot_x) != 0: #水平flag = 1orthocenter_x = target_xorthocenter_y = robot_yelif (path_y - robot_y) != 0 and (path_x - robot_x) == 0: #竖直flag = 2orthocenter_x = robot_xorthocenter_y = target_yelse:flag = 3k1 = (path_y - robot_y)/(path_x - robot_x)k4 = -(1/k1)b4 = target_y - k4 * target_xpoint_fake_x = target_x + 1point_fake_y = k4 * point_fake_x + b4line1 = [path_x,path_y,robot_x,robot_y]line4 = [target_x,target_y,point_fake_x,point_fake_y]orthocenter = cross_point(line1,line4)orthocenter_x = orthocenter[0]orthocenter_y = orthocenter[1]return [orthocenter_x,orthocenter_y,flag]

2.1.8.3 计算移动的方向（1.1.1.2）

def move_direction(flag,robot_x,robot_y,path_x,path_y):if flag == 1:#水平direction_x = (robot_x - path_x)/abs((robot_x - path_x))direction_y = 0cosx = 1sinx = 0elif flag == 2:#竖直direction_x = 0direction_y = (robot_y - path_y)/abs((robot_y - path_y))cosx = 0sinx = 1else:dir_y = path_y - robot_y dir_x = path_x - robot_xcosx = abs(dir_x)/(math.sqrt(np.square(dir_x)+np.square(dir_y)))sinx = abs(dir_y)/(math.sqrt(np.square(dir_x)+np.square(dir_y)))direction_x = (robot_x - path_x)/abs((robot_x - path_x))direction_y = (robot_y - path_y)/abs((robot_y - path_y))return [direction_x,direction_y,cosx,sinx]

2.1.8.3 计算向“上”移动的方向（1.1.1.3）

def move_vertically_direction(flag,robot_x,robot_y,target_x,target_y,path_x,path_y):if flag == 1:#水平fake_point_x = robot_xfake_point_y = target_y#只要不是robot_y,其他任意数字都可以line2 = [robot_x,robot_y,fake_point_x,fake_point_y]line3 = [path_x,path_y,target_x,target_y]cross_point_2 = cross_point(line2,line3)cross_point_2_x = cross_point_2[0]cross_point_2_y = cross_point_2[1]dir_x2 = (cross_point_2_x - robot_x)/abs(cross_point_2_x - robot_x)dir_y2 = (cross_point_2_y - robot_y)/abs(cross_point_2_y - robot_y)cosx2 = 0sinx2 = 1*dir_y2    elif flag == 2:#竖直fake_point_x = target_x#只要不是robot_x,其他任意数字都可以fake_point_y = robot_yline2 = [robot_x,robot_y,fake_point_x,fake_point_y]line3 = [path_x,path_y,target_x,target_y]cross_point_2 = cross_point(line2,line3)cross_point_2_x = cross_point_2[0]cross_point_2_y = cross_point_2[1]dir_x2 = (cross_point_2_x - robot_x)/abs(cross_point_2_x - robot_x)dir_y2 = (cross_point_2_y - robot_y)/abs(cross_point_2_y - robot_y)cosx2 = 1*dir_x2sinx2 = 0else:k1 = (path_y - robot_y)/(path_x - robot_x)k2 = -(1/k1) b2 = robot_y - k2 * robot_xfake_point_x = robot_x + 1fake_point_y = k2 * fake_point_x + b2line2 = [robot_x,robot_y,fake_point_x,fake_point_y]line3 = [path_x,path_y,target_x,target_y]cross_point_2 = cross_point(line2,line3)cross_point_2_x = cross_point_2[0]cross_point_2_y = cross_point_2[1]dir_x2 = cross_point_2_x - robot_xdir_y2 = cross_point_2_y - robot_ycosx2 = dir_x2/(math.sqrt(np.square(dir_x2)+np.square(dir_y2)))sinx2 = dir_y2/(math.sqrt(np.square(dir_x2)+np.square(dir_y2)))return [cosx2,sinx2,cross_point_2_x,cross_point_2_y]

2.1.8.4 first move（1.1.1.2）

def first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y):#print("the first move")#垂点距离机器人的距离#target and robot are too close together, robot moves separate distance from targetwhile 1:cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]orthocenter = orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y)orthocenter_x = orthocenter[0]orthocenter_y = orthocenter[1]flag = orthocenter[2]orthocenter_2_robot = math.sqrt(np.square(orthocenter_x - robot_x)+np.square(orthocenter_y - robot_y))if orthocenter_2_robot - (robot_R + target_R) > 0.2 and orthocenter_2_robot - (robot_R + target_R) <= 0.3:breakif (orthocenter_2_robot - (robot_R + target_R)) <= 0.2:direction_move = move_direction(flag,robot_x,robot_y,path_x,path_y)direction_x = direction_move[0]direction_y = direction_move[1]cosx = direction_move[2]sinx = direction_move[3]#距离太远elif orthocenter_2_robot - (robot_R + target_R) > 0.3:direction_move = move_direction(flag,robot_x,robot_y,path_x,path_y)direction_x = -direction_move[0]direction_y = -direction_move[1]cosx = direction_move[2]sinx = direction_move[3]else:direction_x = 0.0direction_y = 0.0cosx = 0.0sinx = 0.0      p.resetBaseVelocity(cylinder_robot_Id,[direction_x*cosx/5,direction_y*sinx/5,0])#give the robot a velocityp.stepSimulation()time.sleep(1./240.)orthocenter = orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y)flag = orthocenter[2]return flag

resetBaseVelocity
您可以使用重置基本速度重置身体底部的线性和/或角速度
输入参数：

必需/可选参数	参数名字	参数类型	介绍
必需	objectUniqueId	int	对象的唯一id，由加载方法返回。
可选	linearVelocity	vec3, list of 3 floats	直角世界坐标中的线速度[x，y，z]。
可选	angularVelocity	vec3, list of 3 floats	直角速度[wx，wy，wz]在笛卡尔世界坐标中。
可选	physicsClientId	int	如果您连接到多个服务器，您可以选择一个。

2.1.8.5 second move（1.1.1.3）

def second_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag):#print("the second move")while 1:cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]vertically_move_direction = move_vertically_direction(flag,robot_x,robot_y,target_x,target_y,path_x,path_y)cosx2 = vertically_move_direction[0]sinx2 = vertically_move_direction[1]cross_point_2_x = vertically_move_direction[2]cross_point_2_y = vertically_move_direction[3]if (math.sqrt(np.square(robot_x - cross_point_2_x)+np.square(robot_y - cross_point_2_y))) <= 0.1:breakp.resetBaseVelocity(cylinder_robot_Id,[cosx2/5,sinx2/5,0])#p.applyExternalForce(cylinder_robot_Id,-1,[cosx2 * 2,sinx2 * 2,0],[0,0,robot_H/2],p.WORLD_FRAME) #施加力p.stepSimulation()time.sleep(1./240.)flag = first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y)return 1

2.1.8.6 third move（1.1.1.4）

def third_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag):#print("the third move")while 1:cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]if flag == 1:cosx3 = (path_x - robot_x)/abs((path_x - robot_x))sinx3 = 0elif flag == 2:cosx3 = 0sinx3 = (path_y - robot_y)/abs((path_y - robot_y))else:dir_x = path_x - robot_xdir_y = path_y - robot_ycosx3 = dir_x/(math.sqrt(np.square(dir_x)+np.square(dir_y)))sinx3 = dir_y/(math.sqrt(np.square(dir_x)+np.square(dir_y)))if (math.sqrt(np.square(target_x - path_x)+np.square(target_y - path_y))) <= 0.1:breakstart_time = datetime.datetime.now()while 1:end_time = datetime.datetime.now()interval = (end_time-start_time).secondsif interval == 3:breakp.resetBaseVelocity(cylinder_robot_Id,[cosx3/5,sinx3/5,0])p.stepSimulation()time.sleep(1./240.)flag = first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y)second_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag)

2.1.8.7 向“下”移动（1.1.2.2）

def second_time_first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag):#print("the second time first move")while 1:cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]Scalar_Product_1 = (path_x - robot_x)*(target_x - robot_x) + (path_y - robot_y)*(target_y - robot_y)#向量的点乘if Scalar_Product_1 > 0:breakif flag == 1:if target_y >= path_y:cosx21 = 0sinx21 = -1p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])else:cosx21 = 0sinx21 = 1p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])elif flag ==3:if target_x >= path_x:cosx21 = -1sinx21 = 0p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])else:cosx21 = 1sinx21 = 0p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])else:orthocenter = orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y)orthocenter_x = orthocenter[0]orthocenter_y = orthocenter[1]flag = orthocenter[2]dir_x = orthocenter_x - target_xdir_y = orthocenter_y - target_ycosx21 = dir_x/(math.sqrt(np.square(dir_x)+np.square(dir_y)))sinx21 = dir_y/(math.sqrt(np.square(dir_x)+np.square(dir_y)))p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])p.stepSimulation()time.sleep(1./240.)

2.1.8.8 第一种情况

def the_first_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y):print("The first situation")flag = first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y)time.sleep(3)print("*********************************************")#robot 移动到 target和path的延长线上,robot moves to the extension of target and path.second_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag)time.sleep(3)orthocenter = orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y)flag = orthocenter[2]#robot begin to move to target and pull the target to the goalthird_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag)

2.1.8.9 第二种情况

def the_second_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y):print("The second situation")orthocenter = orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y)flag = orthocenter[2]second_time_first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag)flag = first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y)time.sleep(3)print("*********************************************")#robot 移动到 target和path的延长线上,robot moves to the extension of target and path.second_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag)time.sleep(3)orthocenter = orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y)flag = orthocenter[2]third_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag)

2.1.8.10 第三种情况

def the_third_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y):print("The third situation")first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y)time.sleep(3)while 1:dir_x31 = target_x - path_xdir_y31 = target_y - path_ycosx31 = dir_x31/(math.sqrt(np.square(dir_x31)+np.square(dir_y31)))sinx31 = dir_y31/(math.sqrt(np.square(dir_x31)+np.square(dir_y31)))cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]Scalar_Product_2 = (target_x - path_x)*(robot_x - path_x) + (target_y - path_y)*(robot_y - path_y)if Scalar_Product_2 > 0:breakp.resetBaseVelocity(cylinder_robot_Id,[cosx31/5,sinx31/5,0])p.stepSimulation()time.sleep(1./240.)the_first_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)

2.1.8.11 第四种情况

def the_fourth_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y):print("The fourth situation")while 1:dir_x = path_x - robot_xdir_y = path_y - robot_ycosx41 = dir_x/(math.sqrt(np.square(dir_x)+np.square(dir_y)))sinx41 = dir_y/(math.sqrt(np.square(dir_x)+np.square(dir_y)))cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]path_x = path[i][0]path_y = path[i][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]            Scalar_Product_2 = (target_x - path_x)*(robot_x - path_x) + (target_y - path_y)*(robot_y - path_y)if Scalar_Product_2 > 0:start_time = datetime.datetime.now()while 1:end_time = datetime.datetime.now()interval = (end_time-start_time).secondsif interval == 1:breakp.resetBaseVelocity(cylinder_robot_Id,[cosx41/5,sinx41/5,0])p.stepSimulation()time.sleep(1./240.)breakp.resetBaseVelocity(cylinder_robot_Id,[cosx41/5,sinx41/5,0])p.stepSimulation()time.sleep(1./240.)cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]path_x = path[i][0]path_y = path[i][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]  Scalar_Product_1 = (path_x - robot_x)*(target_x - robot_x) + (path_y - robot_y)*(target_y - robot_y)Scalar_Product_2 = (target_x - path_x)*(robot_x - path_x) + (target_y - path_y)*(robot_y - path_y)if Scalar_Product_1 >= 0 and Scalar_Product_2 > 0:print("Before that I am the fourth situation")the_first_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)#点乘是钝角elif Scalar_Product_1 < 0 and Scalar_Product_2 > 0:print("Before that I am the fourth situation")the_second_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)elif Scalar_Product_2 == 0:print("Before that I am the fourth situation")the_third_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)else:print("Before that I am the fourth situation")the_fourth_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)

2.1.8.12 主函数

for i in range(len(path)):print("robot go to the",i+1,"point")cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]path_x = path[i][0]path_y = path[i][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]Scalar_Product_1 = (path_x - robot_x)*(target_x - robot_x) + (path_y - robot_y)*(target_y - robot_y)#向量的点乘Scalar_Product_2 = (target_x - path_x)*(robot_x - path_x) + (target_y - path_y)*(robot_y - path_y)print("*********************************************")#点乘是锐角或者直角if Scalar_Product_1 >= 0 and Scalar_Product_2 > 0:the_first_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)#点乘是钝角elif Scalar_Product_1 < 0 and Scalar_Product_2 > 0:the_second_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)elif Scalar_Product_2 == 0:the_third_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)else:the_fourth_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)

2.1.9 程序结束

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

2.2 代码总体

# -*- coding: utf-8 -*-
"""
Created on Mon Oct 19 11:12:43 2020@author: 12106
"""# -*- coding: utf-8 -*-
"""
Created on Sat Oct 17 16:03:00 2020@author: 12106
"""import pybullet as p
import time
import pybullet_data
import numpy as np
import datetime
import mathphysicsClient = 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")cylinder_Obstacle_1_StartPos = [-0.4, 0.3, 0.2 ]
cylinder_Obstacle_2_StartPos = [ 0.0,-0.6, 0.15]
cylinder_Obstacle_3_StartPos = [ 0.6,-0.6, 0.1 ]
cylinder_Obstacle_4_StartPos = [ 0.8, 0.2, 0.1]
cylinder_target_StartPos = [ 0, 0.0, 0.3]
cylinder_robot_StartPos = [ -0.8, 0.0, 0.3]cylinder_Obstacle_1_StartOrientation = p.getQuaternionFromEuler([0,0,0])
cylinder_Obstacle_2_StartOrientation = p.getQuaternionFromEuler([0,0,0])
cylinder_Obstacle_3_StartOrientation = p.getQuaternionFromEuler([0,0,0])
cylinder_Obstacle_4_StartOrientation = p.getQuaternionFromEuler([0,0,0])
cylinder_target_StartOrientation = p.getQuaternionFromEuler([0,0,0])
cylinder_robot_StartOrientation = p.getQuaternionFromEuler([0,0,0])cylinder_Obstacle_1_Id = p.loadURDF("D:/Study/University of Leeds/PhD/2020.10.15-2020.10.21/object/cylinder_obstacle1.urdf",cylinder_Obstacle_1_StartPos,cylinder_Obstacle_1_StartOrientation)
cylinder_Obstacle_2_Id = p.loadURDF("D:/Study/University of Leeds/PhD/2020.10.15-2020.10.21/object/cylinder_obstacle2.urdf",cylinder_Obstacle_2_StartPos,cylinder_Obstacle_2_StartOrientation)
cylinder_Obstacle_3_Id = p.loadURDF("D:/Study/University of Leeds/PhD/2020.10.15-2020.10.21/object/cylinder_obstacle3.urdf",cylinder_Obstacle_3_StartPos,cylinder_Obstacle_3_StartOrientation)
cylinder_Obstacle_4_Id = p.loadURDF("D:/Study/University of Leeds/PhD/2020.10.15-2020.10.21/object/cylinder_obstacle4.urdf",cylinder_Obstacle_4_StartPos,cylinder_Obstacle_4_StartOrientation)
cylinder_target_Id = p.loadURDF("D:/Study/University of Leeds/PhD/2020.10.15-2020.10.21/object/cylinder_target.urdf",cylinder_target_StartPos,cylinder_target_StartOrientation)
cylinder_robot_Id = p.loadURDF("D:/Study/University of Leeds/PhD/2020.10.15-2020.10.21/object/cylinder_robot.urdf",cylinder_robot_StartPos,cylinder_robot_StartOrientation)
robot_shape = p.getVisualShapeData(cylinder_robot_Id)
target_shape = p.getVisualShapeData(cylinder_target_Id)
robot_R = robot_shape[0][3][1]
robot_H = robot_shape[0][3][0]
target_R = target_shape[0][3][1]
target_H = target_shape[0][3][0]#path = [[0.4,0.4,0.3],[0.6,0.6,0.3],[0.8,0.8,0.3],[1.0,1.0,0.3],[1.2,1.2,0.3]]
path_string = input("Please input the path:")
path_string = path_string.replace('[','')
path_string = path_string.replace(']','')
path_string = path_string.split(',')
length = len(path_string)
little_length = length/3
path = []
little_length = int(little_length)
for i in range(little_length):path.append([])for j in range(3):path[i].append(0.4)
path_flag = 0
for i in range(little_length):for j in range(3):path[i][j] = float(path_string[path_flag])path_flag = path_flag + 1
#path = [[0.4,0.0,0.3],[0.6,0.0,0.3],[0.8,0.0,0.3],[1.0,0.0,0.3],[1.2,0.0,0.3]]def cross_point(line1,line2):#计算交点函数x1=line1[0]#取四点坐标y1=line1[1]x2=line1[2]y2=line1[3]x3=line2[0]y3=line2[1]x4=line2[2]y4=line2[3]if (x4-x3)==0:#L2直线斜率不存在操作k2=Noneb2=0x=x3k1=(y2-y1)*1.0/(x2-x1)#计算k1,由于点均为整数，需要进行浮点数转化b1=y1*1.0-x1*k1*1.0#整型转浮点型是关键y=k1*x*1.0+b1*1.0elif (x2-x1)==0:k1=Noneb1=0x=x1k2=(y4-y3)*1.0/(x4-x3)b2=y3*1.0-x3*k2*1.0y=k2*x*1.0+b2*1.0else:k1=(y2-y1)*1.0/(x2-x1)#计算k1,由于点均为整数，需要进行浮点数转化k2=(y4-y3)*1.0/(x4-x3)#斜率存在操作b1=y1*1.0-x1*k1*1.0#整型转浮点型是关键b2=y3*1.0-x3*k2*1.0x=(b2-b1)*1.0/(k1-k2)y=k1*x*1.0+b1*1.0    return [x,y]def orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y):if (path_y - robot_y) == 0 and (path_x - robot_x) != 0: #水平flag = 1orthocenter_x = target_xorthocenter_y = robot_yelif (path_y - robot_y) != 0 and (path_x - robot_x) == 0: #竖直flag = 2orthocenter_x = robot_xorthocenter_y = target_yelse:flag = 3k1 = (path_y - robot_y)/(path_x - robot_x)k4 = -(1/k1)b4 = target_y - k4 * target_xpoint_fake_x = target_x + 1point_fake_y = k4 * point_fake_x + b4line1 = [path_x,path_y,robot_x,robot_y]line4 = [target_x,target_y,point_fake_x,point_fake_y]orthocenter = cross_point(line1,line4)orthocenter_x = orthocenter[0]orthocenter_y = orthocenter[1]return [orthocenter_x,orthocenter_y,flag]def move_direction(flag,robot_x,robot_y,path_x,path_y):if flag == 1:#水平direction_x = (robot_x - path_x)/abs((robot_x - path_x))direction_y = 0cosx = 1sinx = 0elif flag == 2:#竖直direction_x = 0direction_y = (robot_y - path_y)/abs((robot_y - path_y))cosx = 0sinx = 1else:dir_y = path_y - robot_y dir_x = path_x - robot_xcosx = abs(dir_x)/(math.sqrt(np.square(dir_x)+np.square(dir_y)))sinx = abs(dir_y)/(math.sqrt(np.square(dir_x)+np.square(dir_y)))direction_x = (robot_x - path_x)/abs((robot_x - path_x))direction_y = (robot_y - path_y)/abs((robot_y - path_y))return [direction_x,direction_y,cosx,sinx]def move_vertically_direction(flag,robot_x,robot_y,target_x,target_y,path_x,path_y):if flag == 1:#水平fake_point_x = robot_xfake_point_y = target_y#只要不是robot_y,其他任意数字都可以line2 = [robot_x,robot_y,fake_point_x,fake_point_y]line3 = [path_x,path_y,target_x,target_y]cross_point_2 = cross_point(line2,line3)cross_point_2_x = cross_point_2[0]cross_point_2_y = cross_point_2[1]dir_x2 = (cross_point_2_x - robot_x)/abs(cross_point_2_x - robot_x)dir_y2 = (cross_point_2_y - robot_y)/abs(cross_point_2_y - robot_y)cosx2 = 0sinx2 = 1*dir_y2    elif flag == 2:#竖直fake_point_x = target_x#只要不是robot_x,其他任意数字都可以fake_point_y = robot_yline2 = [robot_x,robot_y,fake_point_x,fake_point_y]line3 = [path_x,path_y,target_x,target_y]cross_point_2 = cross_point(line2,line3)cross_point_2_x = cross_point_2[0]cross_point_2_y = cross_point_2[1]dir_x2 = (cross_point_2_x - robot_x)/abs(cross_point_2_x - robot_x)dir_y2 = (cross_point_2_y - robot_y)/abs(cross_point_2_y - robot_y)cosx2 = 1*dir_x2sinx2 = 0else:k1 = (path_y - robot_y)/(path_x - robot_x)k2 = -(1/k1) b2 = robot_y - k2 * robot_xfake_point_x = robot_x + 1fake_point_y = k2 * fake_point_x + b2line2 = [robot_x,robot_y,fake_point_x,fake_point_y]line3 = [path_x,path_y,target_x,target_y]cross_point_2 = cross_point(line2,line3)cross_point_2_x = cross_point_2[0]cross_point_2_y = cross_point_2[1]dir_x2 = cross_point_2_x - robot_xdir_y2 = cross_point_2_y - robot_ycosx2 = dir_x2/(math.sqrt(np.square(dir_x2)+np.square(dir_y2)))sinx2 = dir_y2/(math.sqrt(np.square(dir_x2)+np.square(dir_y2)))return [cosx2,sinx2,cross_point_2_x,cross_point_2_y]def first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y):#print("the first move")#垂点距离机器人的距离#target and robot are too close together, robot moves separate distance from targetwhile 1:cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]orthocenter = orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y)orthocenter_x = orthocenter[0]orthocenter_y = orthocenter[1]flag = orthocenter[2]orthocenter_2_robot = math.sqrt(np.square(orthocenter_x - robot_x)+np.square(orthocenter_y - robot_y))if orthocenter_2_robot - (robot_R + target_R) > 0.2 and orthocenter_2_robot - (robot_R + target_R) <= 0.3:breakif (orthocenter_2_robot - (robot_R + target_R)) <= 0.2:direction_move = move_direction(flag,robot_x,robot_y,path_x,path_y)direction_x = direction_move[0]direction_y = direction_move[1]cosx = direction_move[2]sinx = direction_move[3]#距离太远elif orthocenter_2_robot - (robot_R + target_R) > 0.3:direction_move = move_direction(flag,robot_x,robot_y,path_x,path_y)direction_x = -direction_move[0]direction_y = -direction_move[1]cosx = direction_move[2]sinx = direction_move[3]else:direction_x = 0.0direction_y = 0.0cosx = 0.0sinx = 0.0      p.resetBaseVelocity(cylinder_robot_Id,[direction_x*cosx/5,direction_y*sinx/5,0])#give the robot a velocityp.stepSimulation()time.sleep(1./240.)orthocenter = orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y)flag = orthocenter[2]return flagdef second_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag):#print("the second move")while 1:cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]vertically_move_direction = move_vertically_direction(flag,robot_x,robot_y,target_x,target_y,path_x,path_y)cosx2 = vertically_move_direction[0]sinx2 = vertically_move_direction[1]cross_point_2_x = vertically_move_direction[2]cross_point_2_y = vertically_move_direction[3]if (math.sqrt(np.square(robot_x - cross_point_2_x)+np.square(robot_y - cross_point_2_y))) <= 0.1:breakp.resetBaseVelocity(cylinder_robot_Id,[cosx2/5,sinx2/5,0])#p.applyExternalForce(cylinder_robot_Id,-1,[cosx2 * 2,sinx2 * 2,0],[0,0,robot_H/2],p.WORLD_FRAME) #施加力p.stepSimulation()time.sleep(1./240.)flag = first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y)return 1def third_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag):#print("the third move")while 1:cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]if flag == 1:cosx3 = (path_x - robot_x)/abs((path_x - robot_x))sinx3 = 0elif flag == 2:cosx3 = 0sinx3 = (path_y - robot_y)/abs((path_y - robot_y))else:dir_x = path_x - robot_xdir_y = path_y - robot_ycosx3 = dir_x/(math.sqrt(np.square(dir_x)+np.square(dir_y)))sinx3 = dir_y/(math.sqrt(np.square(dir_x)+np.square(dir_y)))if (math.sqrt(np.square(target_x - path_x)+np.square(target_y - path_y))) <= 0.1:breakstart_time = datetime.datetime.now()while 1:end_time = datetime.datetime.now()interval = (end_time-start_time).secondsif interval == 3:breakp.resetBaseVelocity(cylinder_robot_Id,[cosx3/5,sinx3/5,0])p.stepSimulation()time.sleep(1./240.)flag = first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y)second_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag)def second_time_first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag):#print("the second time first move")while 1:cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]Scalar_Product_1 = (path_x - robot_x)*(target_x - robot_x) + (path_y - robot_y)*(target_y - robot_y)#向量的点乘if Scalar_Product_1 > 0:breakif flag == 1:if target_y >= path_y:cosx21 = 0sinx21 = -1p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])else:cosx21 = 0sinx21 = 1p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])elif flag ==3:if target_x >= path_x:cosx21 = -1sinx21 = 0p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])else:cosx21 = 1sinx21 = 0p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])else:orthocenter = orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y)orthocenter_x = orthocenter[0]orthocenter_y = orthocenter[1]flag = orthocenter[2]dir_x = orthocenter_x - target_xdir_y = orthocenter_y - target_ycosx21 = dir_x/(math.sqrt(np.square(dir_x)+np.square(dir_y)))sinx21 = dir_y/(math.sqrt(np.square(dir_x)+np.square(dir_y)))p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])p.stepSimulation()time.sleep(1./240.)    def the_first_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y):print("The first situation")flag = first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y)time.sleep(3)print("*********************************************")#robot 移动到 target和path的延长线上,robot moves to the extension of target and path.second_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag)time.sleep(3)orthocenter = orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y)flag = orthocenter[2]#robot begin to move to target and pull the target to the goalthird_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag)def the_second_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y):print("The second situation")orthocenter = orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y)flag = orthocenter[2]second_time_first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag)flag = first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y)time.sleep(3)print("*********************************************")#robot 移动到 target和path的延长线上,robot moves to the extension of target and path.second_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag)time.sleep(3)orthocenter = orthocenter_point(robot_x,robot_y,path_x,path_y,target_x,target_y)flag = orthocenter[2]third_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y,flag) def the_third_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y):print("The third situation")first_move(cylinder_target_Id,cylinder_robot_Id,path_x,path_y)time.sleep(3)while 1:dir_x31 = target_x - path_xdir_y31 = target_y - path_ycosx31 = dir_x31/(math.sqrt(np.square(dir_x31)+np.square(dir_y31)))sinx31 = dir_y31/(math.sqrt(np.square(dir_x31)+np.square(dir_y31)))cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]Scalar_Product_2 = (target_x - path_x)*(robot_x - path_x) + (target_y - path_y)*(robot_y - path_y)if Scalar_Product_2 > 0:breakp.resetBaseVelocity(cylinder_robot_Id,[cosx31/5,sinx31/5,0])p.stepSimulation()time.sleep(1./240.)the_first_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)def the_fourth_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y):print("The fourth situation")while 1:dir_x = path_x - robot_xdir_y = path_y - robot_ycosx41 = dir_x/(math.sqrt(np.square(dir_x)+np.square(dir_y)))sinx41 = dir_y/(math.sqrt(np.square(dir_x)+np.square(dir_y)))cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]path_x = path[i][0]path_y = path[i][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]            Scalar_Product_2 = (target_x - path_x)*(robot_x - path_x) + (target_y - path_y)*(robot_y - path_y)if Scalar_Product_2 > 0:start_time = datetime.datetime.now()while 1:end_time = datetime.datetime.now()interval = (end_time-start_time).secondsif interval == 1:breakp.resetBaseVelocity(cylinder_robot_Id,[cosx41/5,sinx41/5,0])p.stepSimulation()time.sleep(1./240.)breakp.resetBaseVelocity(cylinder_robot_Id,[cosx41/5,sinx41/5,0])p.stepSimulation()time.sleep(1./240.)cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]path_x = path[i][0]path_y = path[i][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]  Scalar_Product_1 = (path_x - robot_x)*(target_x - robot_x) + (path_y - robot_y)*(target_y - robot_y)Scalar_Product_2 = (target_x - path_x)*(robot_x - path_x) + (target_y - path_y)*(robot_y - path_y)if Scalar_Product_1 >= 0 and Scalar_Product_2 > 0:print("Before that I am the fourth situation")the_first_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)#点乘是钝角elif Scalar_Product_1 < 0 and Scalar_Product_2 > 0:print("Before that I am the fourth situation")the_second_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)elif Scalar_Product_2 == 0:print("Before that I am the fourth situation")the_third_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)else:print("Before that I am the fourth situation")the_fourth_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)for i in range(len(path)):print("robot go to the",i+1,"point")cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)cylinder_robot_data = p.getBasePositionAndOrientation(cylinder_robot_Id)robot_x = cylinder_robot_data[0][0]robot_y = cylinder_robot_data[0][1]path_x = path[i][0]path_y = path[i][1]target_x = cylinder_target_data[0][0]target_y = cylinder_target_data[0][1]Scalar_Product_1 = (path_x - robot_x)*(target_x - robot_x) + (path_y - robot_y)*(target_y - robot_y)#向量的点乘Scalar_Product_2 = (target_x - path_x)*(robot_x - path_x) + (target_y - path_y)*(robot_y - path_y)print("*********************************************")#点乘是锐角或者直角if Scalar_Product_1 >= 0 and Scalar_Product_2 > 0:the_first_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)#点乘是钝角elif Scalar_Product_1 < 0 and Scalar_Product_2 > 0:the_second_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)elif Scalar_Product_2 == 0:the_third_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)else:the_fourth_situation(cylinder_target_Id,cylinder_robot_Id,robot_x,robot_y,path_x,path_y,target_x,target_y)p.disconnect()

3. 效果展示

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

移动效果图

4. 总结

像完成一个大项目先要从小项目开始慢慢学习，别看项目虽小，但是五脏俱全，这是为后续做铺垫。这个项目是为了更好的熟悉pybullet而存在。后续学习内容我一会慢慢逐一完成发布到博客里，希望大家共同进步，有问题想要讨论的尽管发在评论区。由于我是初学者，可能有好多问题我也不会，到时候大家一起讨论，共同进步！！！

这篇关于PyBullet (四) 将圆柱体看作机器人，推动目标，让目标按照输入的路径在可移动障碍物中移动的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！