update kalman identifcation [done], refactoring inverse dynamics scri…

…pts, update utils.tools

pyDynaMapp/dynamics/state_space.py

@@ -213,6 +213,9 @@ def state_place_poles(self,gain_matrix:np.ndarray,states:np.ndarray,verbose:bool
         this method is extention of scipy.signals.place_poles for state depend 
         varing systems, over each sample data
             x_{k+1} = A(k)x_{k} + B(k)u_{k}
+        Args:
+            - gain_matrix : 
+            
         Returns:
             AA  : numpy.ndarrry [A(1), A(2), ... A(N)] ( 2*ndof , 2*ndof*N)
         """

pyDynaMapp/identification/Kalman.py

@@ -129,8 +129,8 @@ def predict(self, u, k):
         A_k = self.A[:, (k-1)*self.n:k*self.n]
         B_k = self.B[:, (k-1)*self.n//2:k*self.n//2]
         Q_k = self.Q[:, (k-1)*self.n:k*self.n]
-        np.random.seed(42)
-        A_k = self._stabilize(A_k,B_k,-np.abs(np.random.rand(14)))
+        A_k = self._stabilize(A_k,B_k,np.array([-0.132,-0.552,-0.612,-0.647,-0.689,-0.701,-0.847,\
+            -0.111,-0.688,-0.674,-0.607,-0.609,-0.501,-0.637]))
         self.x = A_k @ self.x + B_k @ u
         self.P[:, (k-1)*self.n:k*self.n] = A_k @ self.P[:, (k-1)*self.n:k*self.n] @ A_k.T + Q_k
 

pyDynaMapp/script/identification_utils.py

@@ -0,0 +1,121 @@
+import sys
+import os
+import matplotlib.pyplot as plt 
+import numpy as np 
+import logging
+
+figureFolderPath = "/home/wissem/dynamic-identification/figure/kinova"
+config_file_path = "/home/wissem/dynamic-identification/exemple/kinova/config.yml"
+
+src_folder = os.path.abspath(os.path.join(os.path.dirname(os.path.dirname(__file__)), '../src'))
+sys.path.append(src_folder)
+
+if not os.path.exists(figureFolderPath):
+    os.makedirs(figureFolderPath)
+
+from dynamics import Robot, Regressor, StateSpace
+from utils import RobotData, plot3Arrays, plot2Arrays, yaml2dict, RMSE, MAE
+
+mlogger  = logging.getLogger('matplotlib')
+logging.basicConfig(level='INFO')
+mlogger.setLevel(logging.WARNING)
+
+dynamics_logger = logging.getLogger('dynamics')
+dynamics_logger.setLevel(logging.ERROR)
+
+cutoff_frequency  = 3.5
+config_params  = yaml2dict(config_file_path)
+data           = RobotData(config_params['identification']['dataFilePath'])
+fildata        = data.lowPassfilter(cutoff_frequency)
+kinova         = Robot()
+q_f            = fildata['position']
+qp_f           = fildata['velocity']
+qpp_f          = fildata['desiredAcceleration']
+current_f      = fildata['current']
+torque_f       = fildata['torque']
+torque_cur_f   = fildata['torque_cur'] 
+torque_rne_f   = fildata['torque_rne']
+torque_rne     = data.torque_rne
+
+q              = data.position
+qp             = data.velocity
+qpp            = data.desiredAcceleration
+current        = data.current
+torque         = data.torque
+
+iteration_counter = 0
+start = 0
+end = 29000
+
+
+def objective_function1(x, grad): 
+    global kinova, iteration_counter, q_f, qp_f, qpp_f, torque_f
+    kinova.setIdentificationModelData(q_f, qp_f, qpp_f)
+    tau_sim = kinova.computeIdentificationModel(x)
+    rmse_time  = RMSE(torque_f, tau_sim, axis=1)
+
+    print(
+        f"Iteration {iteration_counter}: "
+        f"RMSE = {np.sqrt(np.mean(rmse_time**2)):.5f}"
+    )
+    iteration_counter += 1
+    return np.sqrt(np.mean(rmse_time**2))
+#######################################################################################
+def objective_function2(x, grad): 
+    global kinova, iteration_counter, q_f, qp_f, qpp_f, torque_f, torque_cur_f, torque_rne_f, start, end
+    kinova.setIdentificationModelData(q_f[start:end,:], qp_f[start:end,:], qpp_f[start:end,:])
+    tau_sim = kinova.computeIdentificationModel(x)
+    rmse_time  = RMSE(torque_cur_f[start:end,:], tau_sim, axis=1)
+    print(
+        f"Iteration {iteration_counter}: "
+        f"RMSE = {np.sqrt(np.mean(rmse_time**2)):.5f}"
+    )
+    iteration_counter += 1
+    return np.sqrt(np.mean(rmse_time**2))
+
+def validation(x):
+    global kinova, q_f, qp_f, qpp_f, torque_f, figureFolderPath, torque_rne, torque_cur_f, start, end
+    kinova.setIdentificationModelData(q_f[start:end,:],qp_f[start:end,:],qpp_f[start:end,:])
+    tau_sim = kinova.computeIdentificationModel(x)
+    rmse_time  = RMSE(torque_cur_f[start:end,:], tau_sim, axis=1)
+    r = np.sqrt(np.mean(rmse_time**2))
+    # rescale the torques values by the max values from datasehht that can the  [39 39 39 39 9 9 9]
+
+    tau_sim[:,0]=1/39* tau_sim[:,0]
+    torque_rne[:,0] = 1/39 * torque_rne[:,0]
+    torque_cur_f[:,0] = 1/39 * torque_cur_f[:,0]
+
+    tau_sim[:,1]=1/39* tau_sim[:,1]
+    torque_rne[:,1] = 1/39 * torque_rne[:,1]
+    torque_cur_f[:,1] = 1/39 * torque_cur_f[:,1]
+
+    tau_sim[:,2]=1/39* tau_sim[:,2]
+    torque_rne[:,2] = 1/39 * torque_rne[:,2]
+    torque_cur_f[:,2] = 1/39 * torque_cur_f[:,2]
+
+    tau_sim[:,3]=1/39* tau_sim[:,3]
+    torque_rne[:,3] = 1/39 * torque_rne[:,3]
+    torque_cur_f[:,3] = 1/39 * torque_cur_f[:,3]
+
+    tau_sim[:,4]=1/9* tau_sim[:,4]
+    torque_rne[:,4] = 1/9 * torque_rne[:,4]
+    torque_cur_f[:,4] = 1/9 * torque_cur_f[:,4]
+
+    tau_sim[:,5]=1/9* tau_sim[:,5]
+    torque_rne[:,5] = 1/9 * torque_rne[:,5]
+    torque_cur_f[:,5] = 1/9 * torque_cur_f[:,5]
+
+    tau_sim[:,6]=1/9* tau_sim[:,6]
+    torque_rne[:,6] = 1/9 * torque_rne[:,6]
+    torque_cur_f[:,6] = 1/39 * torque_cur_f[:,6]
+    # save the values of the torque simulted or comuted from the model to csv file 
+    format = {'fmt': '%.4f', 'delimiter': ', ', 'newline': ',\n'}
+    np.savetxt('/home/wissem/dynamic-identification/autogen/model_simulation_torques_current.csv', tau_sim, **format)
+    torque_error = np.abs(torque_cur_f[start:end,:]- tau_sim)
+    blast_torque_error =np.abs(torque_rne[start:end,:] - torque_cur_f[start:end,:])
+    plot3Arrays(torque_cur_f[start:end,:],tau_sim,torque_rne[start:end,:],"current","simulation","blast",f"Manipulator Optimized Non Linear model NLopt-MaxNelder RMSE ={r}")
+    plot2Arrays(torque_error,blast_torque_error,'simulation','blast', 'absloute error blast/new_model vs torque current')
+
+    plt.savefig(os.path.join(figureFolderPath,'non_Linear_model_nlopt_best_poly_current'))
+    plt.show()
+

pyDynaMapp/script/nlopt_identifcation.py

@@ -0,0 +1,61 @@
+import os  
+import numpy as np
+import nlopt 
+import identification_utils as iu
+
+initial_guess_path = "/home/wissem/dynamic-identification/autogen/initial_guess_nlopt_best_torque_current.npy"   
+
+#####################################################################################
+# optimisation routines 
+#####################################################################################
+# Initialize the optimizer
+dim = 209  # Dimension of the input vector
+max_iter = 3000
+###################################################################################
+
+
+opt = nlopt.opt(nlopt.LN_NEWUOA, dim) 
+opt.set_initial_step([2.8] * dim)
+# Set the objective function
+opt.set_min_objective(iu.objective_function2)
+# Set optimization parameters (optional)
+opt.set_maxeval(max_iter)  
+opt.set_ftol_rel(1e-7)    
+opt.set_xtol_rel(1e-7)
+# Define bounds if necessary (optional)
+lower_bounds = np.full(dim,-1000)
+upper_bounds = np.full(dim, 1000)
+opt.set_lower_bounds(lower_bounds)
+opt.set_upper_bounds(upper_bounds)
+# Initial guess for the optimization
+if os.path.exists(initial_guess_path):
+    initial_guess = np.load(initial_guess_path)
+    print("Loaded initial guess from file.")
+else:
+    initial_guess = np.random.rand(dim)
+    print("Using random initial guess.")
+
+try:
+    x_opt = opt.optimize(initial_guess)
+    min_value = opt.last_optimum_value()
+    result_code = opt.last_optimize_result()
+    print(f'Parameters values : {x_opt}')
+    print(f'Minimum value of the objective function: {min_value}')
+except KeyboardInterrupt:
+    print("Optimization interrupted by user.")
+    #print(f'Parameters values : {x_opt}')
+    #np.save(initial_guess_path, x_opt)
+    #print(f"Saved current optimized parameters to {initial_guess_path}.")
+
+# Save the optimized vector for future use if not interrupted
+if not np.isnan(x_opt).all():
+    np.save(initial_guess_path, x_opt)
+    print("Saved optimized parameters to file.")
+else:
+    print("Optimization did not produce a valid result.")
+##############################################################################
+# visulization
+##############################################################################
+
+
+iu.validation(x_opt)

pyDynaMapp/script/run_dynamic_identification.py

@@ -1,11 +1,53 @@
+"""
+this files used for the 
+following script puropose running: 
+"""
 import os 
 import sys
-import time 
 import argparse
-import logging 
-import nlopt 
+import numpy as np 
+
+sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
 
 from dynamics.robot import Robot
 from dynamics.regressor import Regressor
-logger = logging.getLogger(__name__)
-st = time.time()
+from utils import RobotData, plot2Arrays, plot3Arrays,  yaml2dict, MAE
+
+base_dir = os.getcwd()
+figure_path      = os.path.join(base_dir ,"pyDynaMapp/figure/kinova") 
+config_file_path = os.path.join(base_dir,"pyDynaMapp/robot/kinova/config.yml")
+state_poles_path = os.path.join(base_dir,"pyDynaMapp/autogen/state_poles.npy")  
+data_file_path   = os.path.join(base_dir,"pyDynaMapp/data/kinova/identification_data/blast_traj.csv")
+urdf_file_path   = os.path.join(base_dir,"pyDynaMapp/robot/kinova/gen3.urdf")
+
+parser = argparse.ArgumentParser()
+parser.add_argument('--v',type=bool,default=False)
+parser.add_argument('--cutoff_frequency', type=float, default=3)
+parser.add_argument('--show_figures', type=bool,default=False)
+args = parser.parse_args()
+
+cutoff_frequency = args.cutoff_frequency
+config_params  = yaml2dict(config_file_path)
+data           = RobotData(data_file_path)
+q              = data.position
+qp             = data.velocity
+qpp            = data.desiredAcceleration
+torque_cur     = data.torque_cur
+
+fildata        = data.lowPassfilter(cutoff_frequency)
+q_f            = fildata['position']
+qp_f           = fildata['velocity']
+qpp_f          = fildata['desiredAcceleration']
+torque_f       = fildata['torque']
+torque_cur_f   = fildata['torque_cur']
+
+kinova         = Robot(urdf_file_path,config_file_path)
+
+ndof = kinova.model.nq
+num_samples = data.numRows
+
+# run ordiany least square identifcation for the linear of the qusqi linear system 
+reg = Regressor(kinova)
+x = np.random.rand(reg.param_vector_max_size)
+W = reg.computeFullRegressor(q_f,qp_f,qpp_f) 
+tau_sim = (W @ x).reshape((torque.shape[0],kinova.model.nq))

pyDynaMapp/script/run_kalman_identification.py

@@ -24,11 +24,12 @@
 import numpy as np
 import matplotlib.pyplot as plt
 
+
 sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
 
 from identification import Kalman, RobustKalman
 from dynamics import StateSpace, Robot
-from utils import RobotData, plot2Arrays, plotArray, yaml2dict, MAE
+from utils import RobotData, plot2Arrays, plot3Arrays,  yaml2dict, smooth_columns, RMSE
 
 base_dir = os.getcwd()
 figure_path      = os.path.join(base_dir ,"pyDynaMapp/figure/kinova") 
@@ -72,9 +73,10 @@
 Q = np.zeros((2*ndof,2*ndof*num_samples))
 
 
-start = 100
-step = 1
-stop = 5000
+start = 1
+step = 100
+stop = 29106
+
 for i in range(start, stop,step):
     print(f'process sample = {i}/{num_samples}')
     Ai, Bi, _, _ = kinova_ss.computeStateMatrices(q_f[i, :], qp_f[i, :])
@@ -90,31 +92,49 @@
     A[:, (i-1)*2*ndof:i*2*ndof] = Ai
     B[:, (i-1)*ndof:i*ndof] = Bi
     H[:, (i-1)*2*ndof:i*2*ndof] =  np.eye(2*ndof)
-    Q[:, (i-1)*2*ndof:i*2*ndof] = np.diag([.5] * 2*ndof)   
-    R[:, i] = 0.2* np.ones(2*ndof)   
-    P0[:, (i-1)*2*ndof:i*2*ndof] = np.diag([0.01] * 2*ndof)
+    Q[:, (i-1)*2*ndof:i*2*ndof] = np.diag([0.9] * 2*ndof)   
+    R[:, i] = 0.45 * np.ones(2*ndof)   
+    P0[:, (i-1)*2*ndof:i*2*ndof] = np.diag([0.1] * 2*ndof)
 
-
-x0 = kinova_ss.getStateVector(qp[start,:],q[start,:])                            
+x0 = kinova_ss.getStateVector(qp_f[start,:],q_f[start,:])                            
 sim_states = np.zeros((2*ndof,num_samples))
 kf = RobustKalman(A, B, H, Q, R, P0, x0)
+states_error = []
 for k in range(start, stop,step):
-    u = 0.1*torque_f[k,:]
-    z = kinova_ss.getStateVector(qp_f[k,:],q_f[k,:])
+    u = 1/40*data.torque[k,:] # 39 is the max torque give by any jont motor extraced form data sheeet is 39
+    z = kinova_ss.getStateVector(qp[k,:],q[k,:])
     x, _ = kf.step(u, z, k)
     sim_states[:,k] = x
-    print(f"Time {k}:, state RMSE {np.sqrt(np.mean((x-z)**2))}")
-
-states = sim_states[7:14, start:stop:step]
-vel = qp[start:stop:step,:]
-scaled_states = 2*(states - np.min(states))/(np.max(states)-np.min(states))
-scaled_vel = 2*(vel - np.min(vel))/(np.max(vel)-np.min(vel))
-plot2Arrays(scaled_states.T,scaled_vel,'state','true',\
-f'Joints Velocity State Model Simulation, cutoff = {cutoff_frequency}, sampling ={data.samplingRate}')
+    states_error.append(np.sqrt(np.mean((x-z)**2))/np.sqrt(np.mean((z)**2)))
+    print(f"Time {k}:, state relative RMSE {np.sqrt(np.mean((x-z)**2))/np.sqrt(np.mean((z)**2))}")
+
+sim_vel = sim_states[7:14, start:stop:step]
+true_vel= qp[start:stop:step,:]
+scaled_sim_vel = sim_vel
+scaled_sim_vel[4:7,:] = 1/5*scaled_sim_vel[4:7,:]
+scaled_sim_vel[:4,:] = 1/1.3899*scaled_sim_vel[:4,:]
+states_error = np.array(states_error)
+avg_error = np.mean(states_error)
+window_size = 10
+avg_error_per_window = np.convolve(states_error, np.ones(window_size)/window_size, mode='valid')
+filt_scaled_sim_vel =  smooth_columns(scaled_sim_vel.T)
+global_rmse = RMSE(filt_scaled_sim_vel,true_vel)
+rmse_moy =np.mean(global_rmse)
+# plotting routines 
+time_ms = np.arange(start, stop, step)
+plt.figure(figsize=(10, 6))
+plt.plot(time_ms, states_error, label='RMSE', color='blue')
+plt.axhline(avg_error, color='red', linestyle='--', label='Average Error')
+plt.plot(time_ms[window_size-1:], avg_error_per_window, color='green', linestyle='-.', label=f'Average Error (Window Size {window_size})')
+
+
+plt.xlabel('Time (ms)')
+plt.ylabel('RMSE')
+plt.title('Robust kalman Filter RMSE over Time')
+plt.legend()
+plt.grid(True)
+plt.tight_layout()
+
+plot3Arrays(scaled_sim_vel.T,true_vel,filt_scaled_sim_vel ,'sim','true','filt',\
+f'Robust kalman Filter Identification, cutoff={cutoff_frequency}, sampling ={data.samplingRate}, Mean RMSE={rmse_moy}')
 plt.show()
-
-
-
-
-
-

pyDynaMapp/script/sim_identified.py

@@ -0,0 +1,13 @@
+import numpy as np
+import matplotlib.pyplot as plt 
+import identification_utils as iu
+
+
+figureFolderPath = "/home/wissem/dynamic-identification/figure/kinova"
+initial_guess_path = "/home/wissem/dynamic-identification/autogen/initial_guess_nlopt_best_torque_current.npy"   
+
+x_opt = np.load(initial_guess_path)
+print("Loaded initial guess from file.")
+
+iu.validation(x_opt)
+plt.show()