def data_preprocess(self):
# 正运动学求取末端位姿
X = np.zeros([self.num, 6])
for i in range(self.num):
X[i, :] = kin.fkine_euler(self.DH_0, self.qq_array[i, :])
#求取末端位置的,前向差分获取速度和加速度
X_xva = np.zeros([self.num, self.m, 3])
X_xva[:, :, 0] = X
#建立高增益观测器
observer = dp.HighGainObserver()
for i in range(self.m):
observer.get_original_data(X[:, i], self.T)
X_xva[:, i, 1] = observer.put_observer_data()
observer.get_original_data(X_xva[:, i, 1], self.T)
X_xva[:, i, 2] = observer.put_observer_data()
# for i in range(self.num):
# X_xva[i, :, 0] = X[i, :]
# if (i==0):
# X_xva[i, :, 1] = 0
# X_xva[i, :, 2] = 0
# else:
# X_xva[i, :, 1] = (X_xva[i, :, 0] - X_xva[i-1, :, 0])/self.T
# X_xva[i, :, 2] = (X_xva[i, :, 1] - X_xva[i-1, :, 1])/self.T
self.X_xva = X_xva
def __init__(self):
# 位置、力误差
self.Ex = np.zeros(6) # Ex=Xr - Xd=>Xr = Xd + Ex
self.Ef = np.zeros(6) # Ef=Fs - Fd
self.Ex_d = np.zeros(6)
self.Ef_i = np.zeros(6)
# 周期
self.T = 0.01
# 第一次标签
self.first_flag = True
# 建立高增益观测器,观测速度
self.hgo = dp.HighGainObserver()
def data_preprocess(self):
qq_qva = np.zeros([self.num, self.n, 3])
qq_qva[:, :, 0] = self.qq_array
# 建立高增益观测器
observer = dp.HighGainObserver()
for i in range(self.n):
observer.get_original_data(self.qq_array[:, i], self.T)
qq_qva[:, i, 1] = observer.put_observer_data()
observer.get_original_data(qq_qva[:, i, 1], self.T)
qq_qva[:, i, 2] = observer.put_observer_data()
self.qq_qva = qq_qva
