|
文章目录
概要空间直线插补仿真代码
接下篇空间圆弧插补,明天写！！
概要
经常在测量机上面会有扫描曲线等轨迹需求，空间的大概就分为直线，圆弧以及不规则的曲线。前面的比较简单，网上也有很多介绍源。不规则的曲线涉及到的一些条件的判断压缩，线性规划的问题，这期先附上直线圆弧插补介绍。
空间直线插补
空间直线相对来说比较的简单，需要求的东西也很少。 例如： 在空间上有2个点，你只要求出他们的距离，在通过s规划，就可以得到每个时刻这个距离上的点，就可以求出这个点在整个距离的占比是多少，然后把这个占比分别分到三个坐标轴上来计算当前位置。 说的应该不是很抽象，看代码就很容易理解出来。直接上代码
仿真代码
#!/usr/bin/python3
import math
import matplotlib.pyplot as plt
import numpy as np
import turtle
from mpl_toolkits.mplot3d import Axes3D
class line_space():
def __init__(self):
pass
def SpaceLine(self,S,D,V_max,a_max,J):
self.x1 = S[0]
self.y1 = S[1]
self.z1 = S[2]
self.x2 = D[0]
self.y2 = D[1]
self.z2 = D[2]
length = math.sqrt(pow((self.x2 - self.x1),2) + pow((self.y2 - self.y1),2) + pow((self.z2 - self.z1),2))
print ("length",length)
q0 = 0
q1 = length
v0 = 0
v1 = 0
v_max = V_max
j_max = J
self.d_x = self.x2 - self.x1
self.d_y = self.y2 - self.y1
self.d_z = self.z2 - self.z1
x = {}
y = {}
z = {}
self._compute_maximum_speed_reached_or_not_reached(q0,q1, v0, v1, v_max, a_max, J)
B = self.Normalization(q0,q1,v0,v1,V_max,a_max,j_max)
return B[0],B[1],B[2]
def _compute_maximum_speed_reached_or_not_reached(self,q0,q1, v0, v1, v_max, a_max, j_max):
if (v_max - v0) * j_max |