不用matlab，不用ros,用pybullet，完成机械臂的轨迹规划_bullet 轨迹规划

import pybullet
from time import sleep
import pybullet_data,pybullet_envs,pybullet_robots,pybullet_utils
from circle import draw_cirlce
import numpy
############
physicsClient = pybullet.connect(pybullet.GUI)   #物理连接
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
pybullet.setGravity(0, 0, -9.81) #设置重力
planeId = pybullet.loadURDF(r"D:\anaconda\Lib\site-packages\pybullet_data\plane.urdf")   #这个是平地
cubeStartPos = [0, 0, 1] #机械臂的设置高度
cubeStartOrientation = pybullet.getQuaternionFromEuler([0, 0, 0])  ##从欧拉角得到四元数

boxId = pybullet.loadURDF(r"I:\pyqtshili\Bullet1\jr603\robots\jr603.URDF", cubeStartPos, cubeStartOrientation,useFixedBase=1)
orPos, orOrn = pybullet.getBasePositionAndOrientation(boxId) #获取机械臂的当前的位置,方位
print("pos:",orPos,"Orn:",orOrn) #打印出当前的位置  打印出来的tool的位置和四元数
print(pybullet.getLinkState(boxId,7))  #由这里得到tool0是第七个关节
pybullet.setRealTimeSimulation(0)  #设置真实模拟时间
####这个是一个简单的计算两个步骤，要想实现一条估计需要设置一个循环之类
###设置一个画圆的
circle_xyz=draw_cirlce(0.4,[0.5,0.5],360,0,table_high=1)  #360*3
###
for i in range(360):
    orenitation1=pybullet.getQuaternionFromEuler([0,0,0])
    targetposition=pybullet.calculateInverseKinematics(boxId,7,circle_xyz[i,:],targetOrientation=orenitation1) #tool0是第七个关节,上面这里需要包含全部
    pybullet.setJointMotorControlArray(boxId,range(6),pybullet.POSITION_CONTROL,targetPositions=targetposition)  #6是指只有六个能动的关节
    pybullet.stepSimulation()  # 环境开始
    position=pybullet.getLinkState(boxId,6)
    print('pos=',position[0])
    sleep(0.05)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

这个是主程序，

import  numpy as np
#import  matplotlib.pyplot as plt
#from mpl_toolkits.mplot3d import Axes3D

def draw_cirlce(r,center_vector,limit_size,theta,table_high): #需要注意是单位是米
    ######
    center_vector=np.array(center_vector)
    x=np.zeros((limit_size,1))
    y=np.zeros((limit_size,1))
    ########

    for i in range(limit_size):
        x[i,0]=center_vector[0]+r*(np.cos(i/180*np.pi))
        y[i,0]=center_vector[1]+r*(np.sin(i/180*np.pi))

    ###现在是圆的高度，和倾斜角
    z1=(np.zeros((180,1)))
    z2=np.zeros((180,1))
    for i in range(180):
        z1[i,0]=(2*r*i/180)*np.sin(theta/180*np.pi)+table_high  #这款是180个数，
        z2[i,0]=(2*r*(180-i)/180)*np.sin(theta/180*np.pi)+table_high
    z=np.vstack((z1,z2))

    ###合并起来变成圆的三维位置输出,输出一个360*3的矩阵，是圆轨迹的坐标
    all=np.hstack((x,y,z))

    #fig = plt.figure()
    #ax = Axes3D(fig)
    #ax.scatter(x, y, z)
    # 添加坐标轴(顺序是Z, Y, X)  ####
    #ax.set_zlabel('Z', fontdict={'size': 10, 'color': 'red'})
    #ax.set_ylabel('Y', fontdict={'size': 10, 'color': 'red'})
    #ax.set_xlabel('X', fontdict={'size': 10, 'color': 'red'})
    #plt.show()
    return all  #输出是一个矩阵

#draw_cirlce(2,[1,1],360,45)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

这个是画圆程序，是在三维空间中画圆