将圆柱体看作机器人，推动目标，让目标按照输入的路径在可移动障碍物中移动
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 锐角/直角 + 锐角
先上图片：

锐角/直角+锐角：向量RT * 向量RP >= 0
锐角：向量PT * 向量PR > 0

1.1.1.1 计算垂足

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

1.1.1.2 计算距离
接下来要计算线段RO的距离(DistanceRO)，机器人R的半径(RadiusR)以及目标的半径(RadiusT)。然后我会比较他们之间的大小。
if DistanceRO <= RadiusR + RadiusT, 机器人会沿向量PR的方向移动，如图所示：

当然我会有函数用来实时检测距离，防止机器人太过于远离目标T和路径P。

if DistanceRO >> RadiusR + RadiusT(我的意思是远远大于)，那么机器人会沿向量RP的方向移动，如图所示：

当然我会有函数用来实时检测距离，防止机器人太过于接近目标T和路径P。
所以最后机器人会停在一个区间内。

1.1.1.3 向“上”移动

过点R做直线RP的垂线，然后该垂线与线段TP的延长线交与一点C：

然后机器人R会朝向点C移动，当然在移动的过程中，每时每刻都在重复运行1.1.1.1、1.1.1.2以及1.1.1.3的步骤，到最后会出现下面这种情况：

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 钝角 + 锐角
先上图片：

钝角：向量RT * 向量RP < 0
锐角：向量PT * 向量PR > 0

1.1.2.1 计算垂足

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

1.1.2.2 计算方向

有了垂足O就能知道向量TO的方向，然后让机器人R按照向量TO的方向从O点移动，直到向量RT * 向量RP > 0：

机器人移动之后我们会发现位置的关系变成了第一种，然后剩下的步骤就可以按照第一种（1.1.1.1、1.1.1.2、1.1.1.3和1.1.1.4）情况来执行。

1.1.3 锐角 + 直角

先上图片：

锐角：向量RT * 向量RP > 0
直角：向量PT * 向量PR = 0

1.1.3.1 “1.1.1.1”

先按照1.1.1.1的步骤执行：

1.1.3.2 向“上”移动

我们直到向量PT的方向，我们让机器人R从R点出发，向向量PT的方向移动：

机器人移动之后我们会发现位置的关系变成了第一种，然后剩下的步骤就可以按照第一种（1.1.1.1、1.1.1.2、1.1.1.3和1.1.1.4）情况来执行。

1.1.4 锐角 + 钝角

先上图片：

锐角：向量RT * 向量RP > 0
钝角：向量PT * 向量PR < 0

1.1.4.1 移动

我们让机器人R从R点出发按照向量RP的方向移动，直到向量PT * 向量PR > 0:

然后我会重新计算向量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)

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网格数据，特别是对于可变形对象，来接收有关顶点位置的数据。
输入参数：

输出参数：

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=None
        b2=0
        x=x3
        k1=(y2-y1)*1.0/(x2-x1)#计算k1,由于点均为整数，需要进行浮点数转化
        b1=y1*1.0-x1*k1*1.0#整型转浮点型是关键
        y=k1*x*1.0+b1*1.0
    elif (x2-x1)==0:#L1直线斜率不存在操作
        k1=None
        b1=0
        x=x1
        k2=(y4-y3)*1.0/(x4-x3)
        b2=y3*1.0-x3*k2*1.0
        y=k2*x*1.0+b2*1.0
    else:
        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.0
        x=(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 = 1
        orthocenter_x = target_x
        orthocenter_y = robot_y
    elif (path_y - robot_y) != 0 and (path_x - robot_x) == 0: #竖直
        flag = 2
        orthocenter_x = robot_x
        orthocenter_y = target_y
    else:
        flag = 3
        k1 = (path_y - robot_y)/(path_x - robot_x)
        k4 = -(1/k1)
        b4 = target_y - k4 * target_x
        point_fake_x = target_x + 1
        point_fake_y = k4 * point_fake_x + b4
        line1 = [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 = 0
        cosx = 1
        sinx = 0
    elif flag == 2:#竖直
        direction_x = 0
        direction_y = (robot_y - path_y)/abs((robot_y - path_y))
        cosx = 0
        sinx = 1
    else:
        dir_y = path_y - robot_y 
        dir_x = path_x - robot_x
        cosx = 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_x
        fake_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 = 0
        sinx2 = 1*dir_y2    
    elif flag == 2:#竖直
        fake_point_x = target_x#只要不是robot_x,其他任意数字都可以
        fake_point_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 = 1*dir_x2
        sinx2 = 0
    else:
        k1 = (path_y - robot_y)/(path_x - robot_x)
        k2 = -(1/k1) 
        b2 = robot_y - k2 * robot_x
        fake_point_x = robot_x + 1
        fake_point_y = k2 * fake_point_x + b2
        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
        dir_y2 = cross_point_2_y - robot_y
        cosx2 = 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 target
    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]
        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:
            break
        if (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.0
            direction_y = 0.0
            cosx = 0.0
            sinx = 0.0      
        p.resetBaseVelocity(cylinder_robot_Id,[direction_x*cosx/5,direction_y*sinx/5,0])#give the robot a velocity
        p.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
您可以使用重置基本速度重置身体底部的线性和/或角速度
输入参数：

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:
            break
        
        p.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 = 0
        elif flag == 2:
            cosx3 = 0
            sinx3 = (path_y - robot_y)/abs((path_y - robot_y))
        else:
            dir_x = path_x - robot_x
            dir_y = path_y - robot_y
            cosx3 = 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:
            break
        start_time = datetime.datetime.now()
        while 1:
            end_time = datetime.datetime.now()
            interval = (end_time-start_time).seconds
            if interval == 3:
                break
            p.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:
            break
        if flag == 1:
            if target_y >= path_y:
                cosx21 = 0
                sinx21 = -1
                p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])
            else:
                cosx21 = 0
                sinx21 = 1
                p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])
        elif flag ==3:
            if target_x >= path_x:
                cosx21 = -1
                sinx21 = 0
                p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])
            else:
                cosx21 = 1
                sinx21 = 0
                p.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_x
            dir_y = orthocenter_y - target_y
            cosx21 = 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 goal
    third_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_x
        dir_y31 = target_y - path_y
        cosx31 = 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:
            break
        p.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_x
        dir_y = path_y - robot_y
        cosx41 = 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).seconds
                if interval == 1:
                    break
                p.resetBaseVelocity(cylinder_robot_Id,[cosx41/5,sinx41/5,0])
                p.stepSimulation()
                time.sleep(1./240.)
            break
        p.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 math

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")

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=None
        b2=0
        x=x3
        k1=(y2-y1)*1.0/(x2-x1)#计算k1,由于点均为整数，需要进行浮点数转化
        b1=y1*1.0-x1*k1*1.0#整型转浮点型是关键
        y=k1*x*1.0+b1*1.0
    elif (x2-x1)==0:
        k1=None
        b1=0
        x=x1
        k2=(y4-y3)*1.0/(x4-x3)
        b2=y3*1.0-x3*k2*1.0
        y=k2*x*1.0+b2*1.0
    else:
        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.0
        x=(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 = 1
        orthocenter_x = target_x
        orthocenter_y = robot_y
    elif (path_y - robot_y) != 0 and (path_x - robot_x) == 0: #竖直
        flag = 2
        orthocenter_x = robot_x
        orthocenter_y = target_y
    else:
        flag = 3
        k1 = (path_y - robot_y)/(path_x - robot_x)
        k4 = -(1/k1)
        b4 = target_y - k4 * target_x
        point_fake_x = target_x + 1
        point_fake_y = k4 * point_fake_x + b4
        line1 = [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 = 0
        cosx = 1
        sinx = 0
    elif flag == 2:#竖直
        direction_x = 0
        direction_y = (robot_y - path_y)/abs((robot_y - path_y))
        cosx = 0
        sinx = 1
    else:
        dir_y = path_y - robot_y 
        dir_x = path_x - robot_x
        cosx = 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_x
        fake_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 = 0
        sinx2 = 1*dir_y2    
    elif flag == 2:#竖直
        fake_point_x = target_x#只要不是robot_x,其他任意数字都可以
        fake_point_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 = 1*dir_x2
        sinx2 = 0
    else:
        k1 = (path_y - robot_y)/(path_x - robot_x)
        k2 = -(1/k1) 
        b2 = robot_y - k2 * robot_x
        fake_point_x = robot_x + 1
        fake_point_y = k2 * fake_point_x + b2
        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
        dir_y2 = cross_point_2_y - robot_y
        cosx2 = 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 target
    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]
        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:
            break
        if (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.0
            direction_y = 0.0
            cosx = 0.0
            sinx = 0.0      
        p.resetBaseVelocity(cylinder_robot_Id,[direction_x*cosx/5,direction_y*sinx/5,0])#give the robot a velocity
        p.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

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:
            break
        
        p.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
    
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 = 0
        elif flag == 2:
            cosx3 = 0
            sinx3 = (path_y - robot_y)/abs((path_y - robot_y))
        else:
            dir_x = path_x - robot_x
            dir_y = path_y - robot_y
            cosx3 = 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:
            break
        start_time = datetime.datetime.now()
        while 1:
            end_time = datetime.datetime.now()
            interval = (end_time-start_time).seconds
            if interval == 3:
                break
            p.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:
            break
        if flag == 1:
            if target_y >= path_y:
                cosx21 = 0
                sinx21 = -1
                p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])
            else:
                cosx21 = 0
                sinx21 = 1
                p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])
        elif flag ==3:
            if target_x >= path_x:
                cosx21 = -1
                sinx21 = 0
                p.resetBaseVelocity(cylinder_robot_Id,[cosx21/5,sinx21/5,0])
            else:
                cosx21 = 1
                sinx21 = 0
                p.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_x
            dir_y = orthocenter_y - target_y
            cosx21 = 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 goal
    third_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_x
        dir_y31 = target_y - path_y
        cosx31 = 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:
            break
        p.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_x
        dir_y = path_y - robot_y
        cosx41 = 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).seconds
                if interval == 1:
                    break
                p.resetBaseVelocity(cylinder_robot_Id,[cosx41/5,sinx41/5,0])
                p.stepSimulation()
                time.sleep(1./240.)
            break
        p.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而存在。后续学习内容我一会慢慢逐一完成发布到博客里，希望大家共同进步，有问题想要讨论的尽管发在评论区。由于我是初学者，可能有好多问题我也不会，到时候大家一起讨论，共同进步！！！