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