将圆柱体看作机器人，推动目标，让目标按照输入的路径在可移动障碍物中移动
申明：本人今年博一（地点英国），项目是与机器人有关的内容。第三次导师见面，导师布置的另一个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)
设置重力系数分别是：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=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
您可以使用重置基本速度重置身体底部的线性和/或角速度
输入参数：
| 必需/可选参数 | 参数名字 | 参数类型 | 介绍 |
|---|---|---|---|
| 必需 | 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:
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. 效果展示
4. 总结
像完成一个大项目先要从小项目开始慢慢学习，别看项目虽小，但是五脏俱全，这是为后续做铺垫。这个项目是为了更好的熟悉pybullet而存在。后续学习内容我一会慢慢逐一完成发布到博客里，希望大家共同进步，有问题想要讨论的尽管发在评论区。由于我是初学者，可能有好多问题我也不会，到时候大家一起讨论，共同进步！！！
1272
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
