update identification & trajectory & figures

.gitignore

@@ -1 +0,0 @@
-/.vscode/

.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+    "python.analysis.extraPaths": [
+        "./src"
+    ]
+}

CHANGELOG.md

@@ -1,2 +1,3 @@
 ## version 1.0.0 
+- Imple,et basic functionly for manipulator dynamics modeling appreoches 
 - 

README.md

@@ -8,7 +8,11 @@
 
 Python code and datasets for the identification of state-dependent dynamic model parameters in collaborative robotic manipulator systems.
 ### Features
-- trajectory data preprocessing
+- find optimized excitation trajectories with non-linear optimization
+- trajectory data processing
+- manipulator state space model implentation
+- state space identification based methods 
+- transfer function manipulator models
 - dynamic model including effects of frictions, actuator inertia, joint stiffness, and torque offset.
 - generation of optimal exciting trajectories.
 - calculation of physically consistent standard inertial parameters.
@@ -29,6 +33,7 @@ create a new conda environment and install all the dependencies:
 conda env create -f environment.yml
 conda activate dynamapp
 ```
+## Tests
 
 ### References
 - **[A three-loop physical parameter identification method of robot manipulators considering physical feasibility and nonlinear friction mode](https://link.springer.com/article/10.1007/s11071-024-09755-w)**, *Tangzhong Song, Lijin Fang,Guanghui Liu, Hanyu Pang*, 2024
@@ -48,11 +53,8 @@ Cheng*, 2021
 - **[Constrained State Estimation - A Review](https://arxiv.org/pdf/1807.03463v3)**, *Nesrine Amor, Ghualm Rasool, and Nidhal C. Bouaynaya*, arXiv, 2022
 - **[The Pinocchio C++ library](https://ieeexplore.ieee.org/document/8700380)**,*J.Carpentier, G.Saurel, G.Buondonno, J.Mirabel, F.Lamiraux, O.Stasse, N.Mansard*, 2019
 
-### Contributing
-
-see the [CONTRIBUTING](CONTRIBUTING.md) guide.
-
-
+### Development
+See the [CONTRIBUTING](CONTRIBUTING.md) guide.
 ### License
 See [LICENSE](LICENSE) file.
 

exemple/kinova/config.yml

@@ -2,7 +2,7 @@ urdfFilePath: C:/Users/chiha/OneDrive/Bureau/Dynamium/dynamic-identification/exe
 robot_params:
   base_frame: world
   tool_frame: bracelet_link
-  friction : viscous
+  friction : maxwellSlip
   has_joint_offset : True
   offset   : [3.92e-1, 1.37, 3.26e-1, -1.02e-1, -2.88e-2, 1.27e-1]
 identification:
@@ -12,7 +12,7 @@ identification:
     has_stiffness        : True
     has_backlash         : False
     has_actuator         : True
-    has_ external_forces : False
+    has_ external_forces : True
     has_joint_offset     : True
     has_torque_offset    : True
   trajectory_params: 
@@ -35,26 +35,26 @@ actuator_params:
   kt: [0.81, 0.11, 0.11, 0.11, 0.11, 0.11, 0.11]
   Imax : [5, 5, 5, 5, 5, 5, 5]
   Tmax : [17, 17, 17, 17, 17, 17, 17]
-  inertia : [0.558, 0.558, 0.558, 0.558, 0.558, 0.558, 0.558]
-  damping : [0.014, 0.014, 0.014, 0.014, 0.014, 0.014, 0.14]
-  Ta : [0.22, 0.22, 0.22, 0.22, 0.22, 0.22, 0.22]
+  inertia : [0.0558, 0.0558, 0.558, 0.138,0.138, 0.138, 0.138]
+  damping : [0.014, 0.014, 0.014, 0.014, 0.014, 0.014, 0.014]
+  Ta : [0.22, 0.22, 0.12, 0.22, 0.22, 0.22, 0.22]
   Tb: [0.21, 0.21, 0.21, 0.21, 0.21, 0.21, 0.21]
   Tck : [[0.015, 0.018, 0.023, 0.0201, 0.0147],
     [0.015, 0.018, 0.023, 0.0201, 0.0147],
     [0.015, 0.018, 0.023, 0.0201, 0.0147],
-    [0.015, 0.018, 0.023, 0.0201, 0.0147],
-    [0.015, 0.018, 0.023, 0.0201, 0.0147],
-    [0.015, 0.018, 0.023, 0.0201, 0.0147],
-    [0.015, 0.018, 0.023, 0.0201, 0.0147]]
+    [0.015, 0.128, 0.023, 0.0201, 0.0147],
+    [0.015, 0.18, 0.023, 0.0201, 0.0147],
+    [0.5, 0.18, 0.023, 0.0201, 0.0147],
+    [0.5, 0.018, 0.023, 0.0201, 0.0144]]
 friction_params:
   maxwellSlip:
     n: 3
     k: [1.1, 1.2, 0.5] 
     c: [1.2, 1.5, 0.3]
     sigma0: [1.25, 1, 0.5, 0.25, 0.2, 0.3, 1.6]
   viscous:
-    Fc : [0.025, 0.025, 0.05, 0.036, 0.058, 0.058, 0.025]
-    Fs : [0.05, 0.026, 0.036, 0.02, 0.025, 0.025, 0.01]
+    Fc : [0.25, 0.25, 0.5, 0.36, 0.58, 0.58, 0.25]
+    Fs : [0.5, 0.26, 0.36, 0.2, 0.25, 0.25, 0.01]
   lugre:
     sigma0: [0.02, 0.5, 0.2, 0.3, 0.3, 0.258, 0.147]
     sigma1: [0.2, 0.5, 0.2, 0.3, 0.3, 0.258, 0.147]
@@ -91,6 +91,6 @@ trajectory:
          [0.1, 1.2, 2.1, 0.1, 1.256],
          [0.1, 1.2, 2.1, 0.1, 1.256],
          [0.1, 1.2, 2.1, 0.1, 1.256]]
-  k1:  1
+  k1:  2
   k2: 200
 

exemple/kinova/identification.py

@@ -1,5 +1,133 @@
+import sys
+import os
+import seaborn as sns
+import matplotlib.pyplot as plt 
 import numpy as np 
-# 
-# identification of the manipulator diffrent models 
-# implemented with diffrent identifcation alorithms 
+import logging
+import time
+import nlopt 
+
+# Define paths
+figureFolderPath = "C:/Users/chiha/OneDrive/Bureau/Dynamium/dynamic-identification/figure/kinova"
+config_file_path = "C:/Users/chiha/OneDrive/Bureau/Dynamium/dynamic-identification/exemple/kinova/config.yml"
+initial_guess_path = "C:/Users/chiha/OneDrive/Bureau/Dynamium/dynamic-identification/exemple/kinova/initial_guess.npy"  # File to save and load the initial guess
+
+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, plot2Arrays, plotElementWiseArray, yaml2dict, RMSE, MAE
+
+
+mlogger  = logging.getLogger('matplotlib')
+logging.basicConfig(level='INFO')
+mlogger.setLevel(logging.WARNING)
+
+# Suppress warnings from the Robot class or dynamics module
+dynamics_logger = logging.getLogger('dynamics')
+dynamics_logger.setLevel(logging.ERROR)
+
+cutoff_frequency  = 3
+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']
+
+q              = data.position
+qp             = data.velocity
+qpp            = data.desiredAcceleration
+current        = data.current
+torque         = data.torque
+
+iteration_counter = 0
+
+def objective_function(x, grad): 
+    global iteration_counter
+    config_file_path = "C:/Users/chiha/OneDrive/Bureau/Dynamium/dynamic-identification/exemple/kinova/config.yml"
+    cutoff_frequency  = 3
+    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']
+    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))
 
+
+# Initialize the optimizer
+dim = 209  # Dimension of the input vector
+opt = nlopt.opt(nlopt.LN_COBYLA, dim)  # Example optimizer (choose an appropriate one)
+
+# Set the objective function
+opt.set_min_objective(objective_function)
+
+# Set optimization parameters (optional)
+opt.set_maxeval(1000)  # Maximum number of evaluations
+opt.set_ftol_rel(1e-4)  # Relative tolerance on function value
+opt.set_xtol_rel(1e-4)
+
+# Define bounds if necessary (optional)
+lower_bounds = np.full(dim, -10)
+upper_bounds = np.full(dim, 10)
+#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.")
+
+# Run the optimization
+x_opt = opt.optimize(np.array(initial_guess))
+min_value = opt.last_optimum_value()
+result_code = opt.last_optimize_result()
+
+# Save the optimized vector for future use
+np.save(initial_guess_path, x_opt)
+print("Saved optimized parameters to file.")
+
+kinova.setIdentificationModelData(q_f, qp_f, qpp_f)
+tau_sim = kinova.computeIdentificationModel(x_opt)
+tau_sim[0:15,6]=0
+plot2Arrays(torque_f, tau_sim, "true", "simulation",\
+f"Manipulator Optimised Non Linear Model: Nlopt-NELDERMEAD, RMSE = {min_value:.4f}, Fs = {data.samplingRate:.4f}, Fc ={cutoff_frequency:.4f} ")
+plt.show()
+
+# Output the results
+print("Optimized parameters:", x_opt)
+print("Minimum value of the objective function:", min_value)
+print("Optimization result code:", result_code)
+
+# pour les paramters optimizées trouver le meilleur trajectoire identifable 
+# genrer beaucoup des trajs ---- Fourier , enregister leur params, ( coeffs, criteria 
+# cond(W) )
+# sur chaqun excuetr l optimisation ---> error RMSE model-reel ,
+# save the final RMSE of each traj 
+# TODO : plot : 2D plot of the RMSe (y) en fontion du frequance du traj (FHz)
+# TODO : plot :  2D plot of the RMSE (y) en fonction du sampling rate du traj (Fs)
+# TODO : plot : 3D plot of the RMSE en fonction du FHz and Fs 
+# TODO : for the linear model : plot of the RMSE with the Cond(W) or the crteria choosen 
+# 

exemple/kinova/simulation.py

@@ -27,7 +27,7 @@
     os.makedirs(figureFolderPath)
 
 from dynamics import Robot, Regressor, StateSpace
-from utils import RobotData,  plot2Arrays, plotElementWiseArray, yaml2dict, RMSE
+from utils import RobotData,  plot2Arrays, plotElementWiseArray, yaml2dict, RMSE, MAE
 
 mlogger  = logging.getLogger('matplotlib')
 logging.basicConfig(level='INFO')
@@ -63,6 +63,13 @@
 plot2Arrays(current, current_f , "true", "filtred", f"Joints Current, cutoff frequency = {cutoff_frequency} Hz")
 plt.savefig(os.path.join(figureFolderPath,'joints_current'))
 
+# Visualize the Correlation between torques data
+data.visualizeCorrelation('torque')
+plt.savefig(os.path.join(figureFolderPath,'sensor_torques_correlation'))
+data.visualizeCorrelation('torque_rne')
+plt.savefig(os.path.join(figureFolderPath,'blast_rne_torques_correlation'))
+data.visualizeCorrelation('torque_cur')
+plt.savefig(os.path.join(figureFolderPath,'current_torques_correlation'))
 
 # Compute and plot the RMSE between the actual RNEA model (Blast) and the 
 # torque sensor output. 
@@ -77,7 +84,7 @@
 # τ = M(Θ)Θddot + C(Θ,Θp)Θp+ G(Θ) 
 tau_sim = np.zeros_like(torque)
 for i  in range(data.numRows):
-    tau_sim[i,:] = 3*(kinova.computeDifferentialModel(q[i,:],qp[i,:],qpp[i,:]))
+    tau_sim[i,:] = 3*(kinova.computeDifferentialModel(q_f[i,:],qp_f[i,:],qpp_f[i,:]))
 rmse_per_joint = RMSE(tau_sim,torque).flatten()
 plotElementWiseArray(rmse_per_joint,"Standard Manipulator Model Error per Joint"\
     ,'Joint Index','RMSE')
@@ -91,7 +98,7 @@
 tau_f = kinova.computeFrictionTorques(qp,q)
 tau_sim = np.zeros_like(torque)
 for i  in range(data.numRows):
-    tau_sim[i,:] = 3*(kinova.computeDifferentialModel(q[i,:],qp[i,:],qpp[i,:]) + tau_f[i,:])
+    tau_sim[i,:] = 3*(kinova.computeDifferentialModel(q_f[i,:],qp_f[i,:],qpp_f[i,:]) + tau_f[i,:])
 rmse_per_joint = RMSE(tau_sim,torque).flatten()
 plotElementWiseArray(rmse_per_joint,"Standard Manipulator Model with Friction Error per Joint"\
     ,'Joint Index','RMSE')
@@ -105,7 +112,7 @@
 tau_sim = np.zeros_like(torque)
 for i  in range(data.numRows):
     tau_s = kinova.computeStiffnessTorques(q[i,:])
-    tau_sim[i,:] = 2.75*(kinova.computeDifferentialModel(q[i,:],qp[i,:],qpp[i,:]) + tau_s)
+    tau_sim[i,:] = 2.75*(kinova.computeDifferentialModel(q_f[i,:],qp_f[i,:],qpp_f[i,:]) + tau_s)
 rmse_per_joint = RMSE(tau_sim,torque).flatten()
 plotElementWiseArray(rmse_per_joint,"Standard Manipulator Model with Stiffness Error per Joint"\
     ,'Joint Index','RMSE')
@@ -120,34 +127,68 @@
 tau_f = kinova.computeFrictionTorques(qp,q)
 for i  in range(data.numRows):
     tau_s = kinova.computeStiffnessTorques(q[i,:])
-    tau_sim[i,:] = 3*(kinova.computeDifferentialModel(q[i,:],qp[i,:],qpp[i,:]) + tau_s + tau_f[i,:])
+    tau_sim[i,:] = 3*(kinova.computeDifferentialModel(q_f[i,:],qp_f[i,:],qpp_f[i,:]) + tau_s + tau_f[i,:])
 plot2Arrays(torque_f,tau_sim,"true","simulation","Standard model with stiffness and friction")
-plt.savefig(os.path.join(figureFolderPath,'standard_model_with_stiffness_friction'))
+plt.savefig(os.path.join(figureFolderPath,'standard_model_stiffness_friction'))
 
 
-# Compute and plot the standard manipulator model with actuator effect
-# τ = M(Θ)Θddot + C(Θ,Θp)Θp + [k]Θ + G(Θ) + τf
-tau_sim = np.zeros_like(torque)
-
+# Compute and plot the standard manipulator model with actuator and friction:
+# No torsional elastic effects Θ = q 
+# τ = τm -  τf - Jm Θddot
+tau_sim =np.zeros_like(torque)
+tau_f = kinova.computeFrictionTorques(qp,q)
+tau_m = kinova.computeActuatorTorques(q,qp,qpp)
+tau_sim = tau_m - tau_f
+Jm = kinova.getActuatorInertiasMatrix()
+for i in range(tau_sim.shape[0]):
+    tau_sim[i,:] -= Jm @ qpp[i,:]
+plot2Arrays(torque_f,tau_sim,"true","simulation","Standard model with actuator and friction")
+plt.savefig(os.path.join(figureFolderPath,'standard_model_actuator_friction'))
+
+
+# Compute and plot the standard manipulator model with actuator, friction and stiffness
+# τj = τm(q,qdot,qddot) -  τf(qdot,q) - Jm qddot
+# τj = M(Θ)Θddot + C(Θ,Θp)Θp + [k](Θ-q) + G(Θ)
+# solve for Θ given q from data , them simulate (2)and compare τj_sim with the real one 
+# from data
+K = kinova.getStiffnessMatrix()
 
+# Compute and plot the standard manipulator regression model:
+# τ = W Θ
+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))
+plot2Arrays(torque_f,tau_sim,"true","simulation","Standard regression model")
+plt.savefig(os.path.join(figureFolderPath,'standard_regression_model'))
+
+# Compute and plot the non linear manipultaor  regression model 
+# τ = f(q,qp,qpp,x) 
+kinova.setIdentificationModelData(q_f,qp_f,qpp_f)
+x = np.random.rand(209)
+tau_sim = kinova.computeIdentificationModel(x)
+plot2Arrays(torque_f,tau_sim,"true","simulation","Manipulator Non Linear model")
+plt.savefig(os.path.join(figureFolderPath,'non_Linear_model'))
+
+
+
 # Compute and plot the system state space model simulation
 # x(k+1) = A(x) x(k) + B(x) u(k)
-# y(k)   = C x(k)
-"""  
-kinova_ss = StateSpace(kinova)
-tau_ss = torque
-x0 = kinova_ss.getStateVector(qp[0,:],q[0,:])
-states = kinova_ss.simulate(x0,tau_ss[:30000:50,:])
-plot2Arrays(MAE(0.001*np.transpose(states[7:14,:]),30),qp[:30000:50,:],'state','true',\
-    'Joints Velocity State Model Simulation')
-plt.savefig(os.path.join(figureFolderPath,'joints velocity state model simulation'))
-plot2Arrays(MAE(0.001*np.transpose(states[0:7,:]),30),q[0:30000:50,:],'state','true',\
-    'Joints Position State Model Simulation')
+# y(k)   = C x(k) 
+#kinova_ss = StateSpace(kinova)
+#tau_ss = torque
+#x0 = kinova_ss.getStateVector(qp_f[0,:],q_f[0,:])
+#states = kinova_ss.simulate(x0,tau_ss[:30000:50,:])
+#plot2Arrays(MAE(0.001*np.transpose(states[7:14,:]),30),qp[:30000:50,:],'state','true',\
+#    'Joints Velocity State Model Simulation')
+#plt.savefig(os.path.join(figureFolderPath,'joints velocity state model simulation'))
+#plot2Arrays(MAE(0.001*np.transpose(states[0:7,:]),30),q[0:30000:50,:],'state','true',\
+#   'Joints Position State Model Simulation')
+
 
-"""
 
 # Show all figures
-plt.show()
+#plt.show()
 
 
 

run_tests.py

@@ -0,0 +1,12 @@
+import os
+import sys
+import unittest
+
+test_folder = os.path.abspath(os.path.join(os.path.dirname(__file__), 'test'))
+sys.path.append(test_folder)
+
+test_loader = unittest.TestLoader()
+test_suite = test_loader.discover('test')
+
+test_runner = unittest.TextTestRunner()
+test_runner.run(test_suite)