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

申明：本人今年博一（地点英国），项目是与机器人有关的内容。第四次导师见面，经过导师的提点，告诉了我更加容易操作的优化算法。不同于上一个方法，这个方法更加偏向于优化。上一个方法通过实时监控目标和机器人的位置并且不断调整机器人的位置来达到在可移动障碍物中按照输入的路径推动目标，但这一次的算法更加“暴力”一些。

系列文章目录：
上一篇：
PyBullet (四) 将圆柱体看作机器人，推动目标，让目标按照输入的路径在可移动障碍物中移动
下一篇：PyBullet (六) UR5机器人手臂模型

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

1. 整体思路

首先我们只给机器人一个向右的速度：
Velocity = [[0.19, 0, 0], [0.19, 0, 0], [0.19, 0, 0], [0.19, 0, 0], …, [0.19, 0, 0]]

“理想”情况下机器人会推着目标笔直的往右边走（机器人：蓝色，目标：绿色）
理想状态下机器人推动目标，目标的位置
假设理想状态下目标被机器人推动之后的位置：
P0(0.0,0.0)，P1(0.1,0.0)，P2(0.2,0.0)，P3(0.3,0.0)，P4(0.4,0.0)，P5(0.5,0.0)

现在我们输入想要目标移动到的位置：
N0(0.0,0.0)，N1(0.1,0.1)，N2(0.2,0.2)，N3(0.3,0.3)，N4(0.4,0.4)，N5(0.5,0.5)

我们希望目标到达的位置：
我们希望目标到达的位置
显而易见只给机器人向右的速度是一定不会让目标到达N1，N2，N3，N4，N5这五个位置的，那我们应该怎么办呢？

这种情况下，我们要计算“cost”，也就是代价。这里的代价是指目标的真实位置和目标的理想位置之间的距离和。

c o s t = P 1 N 1 + P 2 N 2 + P 3 N 3 + P 4 N 4 + P 5 N 5 cost = P1N1 + P2N2 + P3N3 + P4N4 + P5N5 cost=P1N1+P2N2+P3N3+P4N4+P5N5
合在一起

我们的目标就是让cost变的最小，这样就是换一种想法让目标到达输入的地点。其实我们不需要去手动更新机器人的速度和目标的代价，我们完全可以让机器人自己判断，自己修正。

如何让机器人自己修正呢？

我们给初始x速度和y速度分别加上一点点随机值，切记一点点！！！然后用随机到的速度跑程序，因为只加了一点点随机值，所以两个速度之间相差的不大，但是这已经足够影响目标的位置了。用新的速度跑出来的NewCost和OldCost进行比较，如果NewCost < OldCost，我们替换成随机的速度，如果NewCost > OldCost，我们保留原来的速度。然后将速度重新传入循环中，然后再给这个速度增加随机值，这样不断循环来更新速度，减小cost值。

算法如下（伪代码）：

cost <— RollOut(Velocity) #用当前的速度来执行一遍推动程序之后得到了cost
ResetSimulation() #重新设置环境（初始化环境）
Loop until cost < 0.1: #循环条件
	Velocity <— FindABetterV(Velocity) #找到更好的速度
	cost <— RollOut(Velocity) #用找到的更好的速度执行一遍推动程序
	ResetSimulation() #重新设置环境（初始化环境）

这里有些小伙伴可能会疑惑我们为什么要初始化环境呢？

其实在现实生活中，机器人想要推动目标就只是执行一次就让目标到达指定的地点，所以这些一遍一遍的循环都是机器人本身的仿真在工作，仿真一遍一遍的运行，优化然后得到最优解然后反馈给机器人，机器人再按照最优解进行移动。所以我们要“ResetSimulation()”

2. 代码解析

2.1 代码分布

2.1.1 库

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

最基础的导入包啊，库啊

2.1.2 连接物理模拟

physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version

导入PyBullet模块后，首先要做的是 "连接 "物理模拟。PyBullet是围绕客户端-服务器驱动的API设计的，客户端发送命令，物理服务器返回状态。PyBullet有一些内置的物理服务器。DIRECT和GUI。GUI和DIRECT连接都会在与PyBullet相同的过程中执行物理模拟和渲染。

2.1.3 设置环境（函数）

由于这次需要反复重新设置环境，所以我把设置环境这里写成了函数

def SetSimulation():
    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.4, 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)
    return cylinder_robot_Id,cylinder_target_Id #我们需要直到机器人和目标的Id，然后才能对机器人和目标进行监控

模型可以的参数在上一篇文章中已经详细给出，私戳链接：PyBullet (四) 将圆柱体看作机器人，推动目标，让目标按照输入的路径在可移动障碍物中移动中的模型加载

2.1.4 将输入的string类型的Path转化成float类型的Path（函数）

def TransferToFloat(path_string):
    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
    return path

2.1.5 计算点之间的距离（函数）

def ComputeDistance(path_x,path_y,target_x,target_y):
    distance = (math.sqrt(np.square(path_x - target_x)+np.square(path_y - target_y)))
    return distance

2.1.6 跑模型（函数）

def RollOut(Velocity,cylinder_robot_Id,cylinder_target_Id,path):
    length_path = len(path)
    distance = []
    for i in range(length_path):
    	#计时器：每一段的速度移动1s
        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,Velocity[i])
            p.stepSimulation()
            time.sleep(1./240.)
		#监控机器人和目标的信息
        cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)
        target_x = cylinder_target_data[0][0]
        target_y = cylinder_target_data[0][1]
        path_x = path[i][0]
        path_y = path[i][1]    
        distance.append(ComputeDistance(path_x,path_y,target_x,target_y))
    #每一段cost加和
    length = len(distance)
    cost = 0
    for i in range(length):
        cost = cost + distance[i]   
    return cost

2.1.7 找到更好的速度（函数）

def FindABetterU(Velocity,cost,cylinder_robot_Id,cylinder_target_Id):
    #给速度添加随机值，这里我一次拿出来5个不同的速度列表，
    #然后让机器人仿真分别运行这五个不同的速度，
    #然后比较五个cost，选择最优的速度。
    #当然我们可以修改“5”,其实这个随便的，当然越大效果越好。
    velocity_with_noise = []
    for i in range(5):
        Velocity_temp = copy.deepcopy(Velocity)
        velocity_with_noise.append(Velocity_temp)
        velocity_with_noise[i] = AddNoiseOnU(velocity_with_noise[i])
    print("Velocity1:",Velocity)
    print("velocity_with_noise:",velocity_with_noise)
    #分别计算不同速度时候的cost
    cost_with_noise = []
    for i in range(5):
        cost_new = RollOut(velocity_with_noise[i],cylinder_robot_Id,cylinder_target_Id,path)
        cost_with_noise.append(cost_new)
        p.resetSimulation()
        cylinder_robot_Id,cylinder_target_Id = SetSimulation()
 	#找到cost最小的那个速度的索引
    index = cost_with_noise.index(min(cost_with_noise))
    print("cost_with_noise:",cost_with_noise)
    print("index:",index)
    #和初始的cost作比较（这里有个if，else的原因是，如果随机到的速度导致的cost非常大，甚至超过了原来的cost，我们可以不更新速度，然后重新给速度一个随机值）
    if cost_with_noise[index]<cost:
        print("cost_with_noise[index]<:",cost_with_noise[index],"cost:",cost)
        print("Velocity2:",Velocity)
        print("velocity_with_noise[index]:",velocity_with_noise[index])
        return velocity_with_noise[index],cost_with_noise[index]
    else:
        print("cost_with_noise[index]>:",cost_with_noise[index],"cost:",cost)
        print("Velocity2:",Velocity)
        print("velocity_with_noise[index]:",velocity_with_noise[index])
        return Velocity,cost

大家运行的时候可以把“print”删掉，这个是我一开始测试用的。

2.1.8 增加噪音（函数）

def AddNoiseOnU(VelocityNoise):
    length = len(VelocityNoise)
    for i in range(length):
        noisy_value_x = random.uniform(-0.005,0.005)
        noisy_value_y = random.uniform(-0.005,0.005)
        VelocityNoise[i][0] = VelocityNoise[i][0] + noisy_value_x
        VelocityNoise[i][1] = VelocityNoise[i][1] + noisy_value_y
    return VelocityNoise

2.1.9 主函数

Velocity = [[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0]]
    
cylinder_robot_Id,cylinder_target_Id = SetSimulation()
path = TransferToFloat(path_string)
cost = RollOut(Velocity,cylinder_robot_Id,cylinder_target_Id,path)

print("cost:",cost)
p.resetSimulation()
cylinder_robot_Id,cylinder_target_Id = SetSimulation()

while cost > 0.1:


    print("**********************************************************")
    
    Velocity,cost = FindABetterU(Velocity,cost,cylinder_robot_Id,cylinder_target_Id)
    
    cost = RollOut(Velocity,cylinder_robot_Id,cylinder_target_Id,path)
    
    p.resetSimulation()
    cylinder_robot_Id,cylinder_target_Id = SetSimulation()

2.1.10 程序结束

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

2.2 代码总体

# -*- coding: utf-8 -*-
"""
Created on Thu Oct 22 15:57:12 2020

@author: 12106
"""

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

physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version

#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 = [[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]]

#path = [[0.1,-0.02,0.3],[0.2,-0.04,0.3],[0.3,-0.06,0.3],[0.4,-0.082,0.3],[0.5,-0.107,0.3],[0.6,-0.135,0.3],[0.7,-0.168,0.3],[0.8,-0.205,0.3],[0.9,-0.251,0.3],[1.0,-0.309,0.3],[1.1,-0.375,0.3],[1.2,-0.45,0.3],[1.3,-0.535,0.3],[1.4,-0.622,0.3],[1.5,-0.718,0.3],[1.6,-0.812,0.3],[1.7,-0.914,0.3],[1.8,-1.015,0.3],[1.9,-1.117,0.3],[2.0,-1.224,0.3]]
#p.applyExternalForce(cylinder_target_Id,-1,[1000090,0,0],[0,0,0.3],p.WORLD_FRAME)
        
#p.changeDynamics(planeId,-1,lateralFriction = 0,spinningFriction = 0,rollingFriction = 0)


def SetSimulation():
    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.4, 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)
    return cylinder_robot_Id,cylinder_target_Id
    
    
def TransferToFloat(path_string):
    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
    return path


    

def ComputeDistance(path_x,path_y,target_x,target_y):
    distance = (math.sqrt(np.square(path_x - target_x)+np.square(path_y - target_y)))
    return distance

def RollOut(Velocity,cylinder_robot_Id,cylinder_target_Id,path):
    length_path = len(path)
    distance = []
    for i in range(length_path):
        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,Velocity[i])
            p.stepSimulation()
            time.sleep(1./240.)
        cylinder_target_data = p.getBasePositionAndOrientation(cylinder_target_Id)
        target_x = cylinder_target_data[0][0]
        target_y = cylinder_target_data[0][1]
        path_x = path[i][0]
        path_y = path[i][1]    
        distance.append(ComputeDistance(path_x,path_y,target_x,target_y))
    length = len(distance)
    cost = 0
    for i in range(length):
        cost = cost + distance[i]   
    return cost

def FindABetterU(Velocity,cost,cylinder_robot_Id,cylinder_target_Id):
    
    velocity_with_noise = []
    for i in range(2):
        Velocity_temp = copy.deepcopy(Velocity)
        velocity_with_noise.append(Velocity_temp)
        velocity_with_noise[i] = AddNoiseOnU(velocity_with_noise[i])
    print("Velocity1:",Velocity)
    print("velocity_with_noise:",velocity_with_noise)
    
    cost_with_noise = []
    for i in range(2):
        cost_new = RollOut(velocity_with_noise[i],cylinder_robot_Id,cylinder_target_Id,path)
        cost_with_noise.append(cost_new)
        p.resetSimulation()
        cylinder_robot_Id,cylinder_target_Id = SetSimulation()
    index = cost_with_noise.index(min(cost_with_noise))
    
    print("cost_with_noise:",cost_with_noise)
    print("index:",index)
    if cost_with_noise[index]<cost:
        print("cost_with_noise[index]<:",cost_with_noise[index],"cost:",cost)
        print("Velocity2:",Velocity)
        print("velocity_with_noise[index]:",velocity_with_noise[index])
        return velocity_with_noise[index],cost_with_noise[index]
    else:
        print("cost_with_noise[index]>:",cost_with_noise[index],"cost:",cost)
        print("Velocity2:",Velocity)
        print("velocity_with_noise[index]:",velocity_with_noise[index])
        return Velocity,cost
    


def AddNoiseOnU(VelocityNoise):
    length = len(VelocityNoise)
    for i in range(length):
        noisy_value_x = random.uniform(-0.005,0.005)
        noisy_value_y = random.uniform(-0.005,0.005)
        VelocityNoise[i][0] = VelocityNoise[i][0] + noisy_value_x
        VelocityNoise[i][1] = VelocityNoise[i][1] + noisy_value_y
    return VelocityNoise
    

    
Velocity = [[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0],[0.19,0,0]]
    
cylinder_robot_Id,cylinder_target_Id = SetSimulation()
path = TransferToFloat(path_string)
cost = RollOut(Velocity,cylinder_robot_Id,cylinder_target_Id,path)

print("cost:",cost)
p.resetSimulation()
cylinder_robot_Id,cylinder_target_Id = SetSimulation()

while cost > 0.1:


    print("**********************************************************")
    
    Velocity,cost = FindABetterU(Velocity,cost,cylinder_robot_Id,cylinder_target_Id)
    
    cost = RollOut(Velocity,cylinder_robot_Id,cylinder_target_Id,path)
    
    p.resetSimulation()
    cylinder_robot_Id,cylinder_target_Id = SetSimulation()


    
    
print("Velocity:",Velocity)

3. 效果展示

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

4. 总结

比较暴力直接的算法，效果一般，但是也能做到优化，主要理解一下优化的思路。这个项目是为了更好的熟悉pybullet而存在。后续学习内容我一会慢慢逐一完成发布到博客里，希望大家共同进步，有问题想要讨论的尽管发在评论区。由于我是初学者，可能有好多问题我也不会，到时候大家一起讨论，共同进步！！！