diff --git a/exemple/kinova/config.yml b/exemple/kinova/config.yml index aa9907b..ffc1006 100644 --- a/exemple/kinova/config.yml +++ b/exemple/kinova/config.yml @@ -24,6 +24,8 @@ optimisation: c2 : 0.9 r1: -0.2 r2: 1.1 + iter: 100 + cmaes: stiffness_params: [0.25, 0.26, 0.369, 0.125, 0.258, 0.123, 0.11] actuator_params: diff --git a/exemple/kinova/identification.py b/exemple/kinova/identification.py new file mode 100644 index 0000000..725bf62 --- /dev/null +++ b/exemple/kinova/identification.py @@ -0,0 +1,2 @@ +# identification of the manipulator diffrent models +# implemented with diffrent identifcation alorithms \ No newline at end of file diff --git a/exemple/kinova/simulate.py b/exemple/kinova/simulation.py similarity index 80% rename from exemple/kinova/simulate.py rename to exemple/kinova/simulation.py index 45a4cc9..23b4357 100644 --- a/exemple/kinova/simulate.py +++ b/exemple/kinova/simulation.py @@ -1,6 +1,9 @@ -""" - -""" +########################################################################################### +# tghis script for robot data visulization adn the static simulation of the robt models +# with pataerts stored in config.yml file +# all figures are saved in figure/pyDynamapp +# +########################################################################################### import sys import os import pandas as pd @@ -15,20 +18,22 @@ mlogger = logging.getLogger('matplotlib') mlogger.setLevel(logging.WARNING) -src_folder = os.path.abspath(os.path.join(os.path.dirname(os.path.dirname(__file__)), '..', 'src')) +src_folder = os.path.abspath(os.path.join(os.path.dirname(\ + os.path.dirname(__file__)), '..', 'src')) sys.path.append(src_folder) from dynamics import Robot, StateSpace from utils import RobotData, plotArray, plot2Arrays, yaml2dict, RMSE, MAE cutoff_frequency = 3 -config_params = yaml2dict("C:/Users/chiha/OneDrive/Bureau/Dynamium/dynamic-identification/exemple/kinova/config.yml") -data = RobotData(config_params['identification']['dataFilePath']) +config_params = yaml2dict(\ +"C:/Users/chiha/OneDrive/Bureau/Dynamium/dynamic-identification/exemple/kinova/config.yml") +data = RobotData(config_params['identification']['dataFilePath']) -fildata = data.LowPassfilter(cutoff_frequency) +fildata = data.lowPassfilter(cutoff_frequency) kinova = Robot() -q_f = fildata['position'] +q_f = fildata ['position'] qp_f = fildata['velocity'] qpp_f = fildata['desiredAcceleration'] current_f = fildata['current'] @@ -44,6 +49,8 @@ rmse_time = RMSE(torque, data.torque_rne,axis=1) # compute the model with computed matrices tau =M(q)qddot+C(q,qp)qp+G(q) tau_sim = np.zeros_like(torque) + + x = 0.0001*np.abs(np.random.rand(13*7)) tau_f =kinova.computeFrictionTorques(qp,q) for i in range(data.numRows): diff --git a/src/identification/IDIMNLS.py b/src/identification/IDIMNLS.py index aa58869..c9930c1 100644 --- a/src/identification/IDIMNLS.py +++ b/src/identification/IDIMNLS.py @@ -2,10 +2,8 @@ import numpy as np from typing import Callable import matplotlib.pyplot as plt -import seaborn as sns -import pandas as pd from scipy.optimize import least_squares, curve_fit -from utils import RMSE +from utils import RMSE, clampArray, plotArray, plot2Arrays logging.basicConfig(level=logging.INFO) logger = logging.getLogger(__name__) @@ -23,18 +21,22 @@ class IDIMNLS: - identificationModel : function that return the model computed torques. of shape : ( Nsamples * ndof ) """ - def __init__(self,nVars,output,identificationModel: Callable[[np.ndarray],np.ndarray],time_step=0.001) -> None: - self.time_step = time_step - self.output = output - self.nVars = nVars - + def __init__(self,nVars,output,identificationModel: Callable[[np.ndarray],np.ndarray],\ + upperBound=10,lowerBound=0.001,time_step=0.001) -> None: + + # Class Attributes + self.time_step = time_step + self.output = output + self.nVars = nVars + self.upperBound = upperBound + self.lowerBound = lowerBound self.identificationModel = identificationModel self.optimized_params = None assert (np.ndim(self.output) == 2) def __str__(self) -> str: return (f"IDIMNLS Model with {self.nVars} optimization variables," - f"output shape: {self.torque.shape}, " + f"output shape: {self.output.shape}, " f"time step: {self.time_step}") def computeLsCostFunction(self, x:np.ndarray): @@ -45,17 +47,24 @@ def computeLsCostFunction(self, x:np.ndarray): """ if self.nVars < x.size : xnew = np.concatenate([x[0:self.nVars], np.zeros(x.size-self.nVars)]) + xnew = clampArray(xnew,self.lowerBound,self.upperBound) tau_s = self.identificationModel(xnew) elif self.nVars == x.size: + x = clampArray(x,self.lowerBound,self.upperBound) tau_s = self.identificationModel(x) else: logger.error(\ - 'Identification Engine: Optimisation Variables number should be <= optimisation vector. ') + 'Identification Engine: Optimisation Variables number should be <= input vector size. ') rmse = RMSE(self.output, tau_s) cost = np.mean(rmse**2) return cost - + + def computeRelativeError(self): + relaErr =0 + return relaErr + + def evaluate(self)->None: """Evaluate the model's performance using the current parameters.""" @@ -67,7 +76,7 @@ def evaluate(self)->None: mean_rmse = np.mean(rmse) logger.info(f"Evaluation result - Mean RMSE: {mean_rmse:.4f}") - def optimize(self, x0:np.ndarray, method='least_square', tol=0.5): + def optimize(self, x0:np.ndarray, method='least_square', tol=0.0001): """ Optimize the cost function with NLS alorithms to mimize it value. Args: @@ -75,7 +84,7 @@ def optimize(self, x0:np.ndarray, method='least_square', tol=0.5): method : optimisation algorithm tol : optimization alorithm error stop tolerence. """ - assert x0.size == self.nVars + xOpt = x0 if method == 'least_square': try: xOpt = least_squares(self.computeLsCostFunction, x0, xtol=tol) @@ -89,7 +98,7 @@ def optimize(self, x0:np.ndarray, method='least_square', tol=0.5): len(self.output)) popt, _ = curve_fit(self.identificationModel, x_data, \ self.output , p0=init_params,method='trf') - self.optimized_params = popt + self.optimized_params = popt.x except Exception as e: logger.error(f"An error occurred during optimization: {e}") else: @@ -97,68 +106,34 @@ def optimize(self, x0:np.ndarray, method='least_square', tol=0.5): return xOpt def visualizeCostFunction(self)->None: - """ """ - - - + """ + Plot the cost function scalar variation respect to ||x|| only + the optimisation variables + are considerd + """ + plt.figure(figsize=(12, 6)) def visualizeError(self,title=None, ylabel=None)->None: - """Plot the error between simulated and real torque using Seaborn.""" + """Plot the root squred error between simulated and inputs""" if self.optimized_params is None: - logger.error("No optimized parameters found. Run optimize() first.") + logger.error("Identification Engine : No optimized parameters found. Run optimize() first.") return tau_s = self.identificationModel(self.optimized_params) - error = self.torque - tau_s - time_steps = np.arange(error.shape[0]) - error_df = pd.DataFrame(error, columns=[f'Joint {i+1}' for i in \ - range(error.shape[1])]) - error_df['Time Step'] = time_steps - error_df_melted = error_df.melt(id_vars='Time Step', \ - var_name='Joint', value_name='Error') - - plt.figure(figsize=(12, 6)) - sns.lineplot(data=error_df_melted, x='Time Step', y='Error', hue='Joint') - if not(title is None): - plt.title(title,fontsize=9) - plt.xlabel('Time (ms)',fontsize=9) - if not(ylabel is None): - plt.ylabel(ylabel,fontsize=9) - plt.legend(title='Joint',fontsize=9) + rmse = RMSE(self.output, tau_s,1) + plotArray(rmse,'err per joint','abs err') -def visualizeResults(self, title=None, y_label=None)->None: - """Plot the simulated and real torque in one figure.""" + def visualizeResults(self, title=None, y_label=None)->None: + """Plot the simulated and real signals in one figure.""" - if self.optimized_params is None: - logger.error("No optimized parameters found. Run optimize() first.") - - tau_s = self.identificationModel(self.optimized_params) - time_steps = np.arange(self.torque.shape[0]) - - real_df = pd.DataFrame(self.torque, columns=[f'Joint {i+1}' for i in \ - range(self.torque.shape[1])]) - simulated_df = pd.DataFrame(tau_s, columns=[f'Joint {i+1}' for i in \ - range(tau_s.shape[1])]) - - real_df['Time'] = time_steps - simulated_df['Time'] = time_steps - - combined_df = pd.merge(real_df.melt(id_vars='Time', var_name='Joint', \ - value_name='Real Torque'), - simulated_df.melt(id_vars='Time', var_name='Joint',\ - value_name='Simulated Torque'), - on=['Time', 'Joint']) - - plt.figure(figsize=(12, 6)) - sns.lineplot(data=combined_df, x='Time', y='Real Torque', hue='Joint',\ - style='Joint', dashes=False, markers=True, alpha=0.6) - sns.lineplot(data=combined_df, x='Time', y='Simulated Torque', hue='Joint',\ - style='Joint', dashes=True) + if self.optimized_params is None: + logger.error("No optimized parameters found. Run optimize() first.") + tau_s = self.identificationModel(self.optimized_params) + plot2Arrays(tau_s,self.output,'simultion','true',title) + + def visualizeRelativeError(self): + """ """ + - if title is not None: - plt.title(title) - plt.xlabel('Time (ms)', fontsize=9) - if y_label is not None: - plt.ylabel(y_label, fontsize=9) - plt.legend(title='Joint', fontsize=9) \ No newline at end of file + \ No newline at end of file diff --git a/src/identification/IDIMOLS.py b/src/identification/IDIMOLS.py index 4518f0d..386e976 100644 --- a/src/identification/IDIMOLS.py +++ b/src/identification/IDIMOLS.py @@ -1,14 +1,18 @@ -import numpy as np +import numpy as np +import logging +import matplotlib.pyplot as plt + class IDIMOLS: """ Inverse Dynamics Identification Method Ordiany Least Square. + this class valid only when the friction Args: - """ - def __init__(self) -> None: + def __init__(self, ) -> None: pass - def computeCostFunction(self): + def computeLsCostFunction(self): """ """ cost = 0 return cost \ No newline at end of file diff --git a/src/identification/Kalman.py b/src/identification/Kalman.py index d5d503b..02c3a1f 100644 --- a/src/identification/Kalman.py +++ b/src/identification/Kalman.py @@ -1,7 +1,6 @@ import numpy as np import matplotlib.pyplot as plt -import seaborn as sns - + class Kalman: """ KALMAN @@ -15,6 +14,8 @@ class Kalman: R (np.ndarray): Covariance of the observation noise. P (np.ndarray): Initial error covariance. x0 (np.ndarray): Initial state estimate. + Ref: + An Introduction to the Kalman Filter - Greg Welch and Gary Bishop. """ def __init__(self, F, H, Q, R, P, x0, alpha=0.2, beta=1): @@ -58,11 +59,14 @@ def filter(self, observations): states (np.ndarray): Sequence of state estimates. """ states = [] + innovations = [] for z in observations: self.predict() self.update(z) + y = self.update(z) states.append(self.x.copy()) - return np.array(states) + innovations.append(y.copy()) + return np.array(states), np.array(innovations) def evaluate(self, true_states, estimated_states): """ @@ -89,6 +93,12 @@ def adaptNoiseCovariance(self, y): self.Q = self.Q + self.alpha * (np.outer(y, y) - self.Q) self.R = self.R + self.beta * (np.outer(y, y) - self.R) - def visualizeSatets(self): - """ """ - plt.figure(figsize=(12, 6)) \ No newline at end of file + def visualizeEstimates(self): + """Visualize the estimated states over time.""" + plt.figure(figsize=(12, 6)) + for i in range(self.x.shape[1]): + plt.plot(self.x[:, i], label=f'Estimated state {i+1}') + plt.xlabel('Time (ms)') + plt.ylabel('state values') + plt.title('Estimated states over time') + plt.legend() \ No newline at end of file diff --git a/src/identification/N4SID.py b/src/identification/N4SID.py index b802dd8..2fa5e2c 100644 --- a/src/identification/N4SID.py +++ b/src/identification/N4SID.py @@ -1,9 +1,9 @@ import numpy as np -from dynamics import StateSpace as ss +from dynamics import StateSpace class N4SID: """ - Numerical Subspace State Space System IDentification + Numerical Subspace State Space System Identification This method uses input-output data to construct a state-space model by minimizing a prediction error criterion. diff --git a/src/optimisation/DEA.py b/src/optimisation/DEA.py index 33fcd31..639b149 100644 --- a/src/optimisation/DEA.py +++ b/src/optimisation/DEA.py @@ -1,4 +1,5 @@ import numpy as np +from numba import cuda class DEA: """ diff --git a/src/optimisation/PSO.py b/src/optimisation/PSO.py index fb611da..d3e08b5 100644 --- a/src/optimisation/PSO.py +++ b/src/optimisation/PSO.py @@ -1,16 +1,18 @@ import random import numpy as np -from numba import cuda import warnings -from numba.cuda.dispatcher import NumbaPerformanceWarning +import logging -warnings.filterwarnings("ignore", category=NumbaPerformanceWarning) +from numba import cuda +if (cuda.is_available()): + from numba.cuda.dispatcher import NumbaPerformanceWarning + warnings.filterwarnings("ignore", category=NumbaPerformanceWarning) W = 0.85 c1 = 0.72 c2 = 0.91 -print(cuda.is_available()) + n_iterations = int(input("Inform the number of iterations: ")) target_error = float(input("Inform the target error: ")) n_particles = int(input("Inform the number of particles: ")) @@ -98,6 +100,10 @@ def move_particles(self): for i, particle in enumerate(self.particles): particle.position = d_positions[i] particle.velocity = d_velocities[i] + +class PSO: + def __init__(self, iter,particles,w,c1,c2) -> None: + pass diff --git a/src/utils/RobotData.py b/src/utils/RobotData.py index 64e5da6..c16756b 100644 --- a/src/utils/RobotData.py +++ b/src/utils/RobotData.py @@ -2,7 +2,12 @@ import numpy as np import matplotlib.pyplot as plt import seaborn as sns -from scipy.signal import butter, filtfilt +import logging +from scipy.signal import butter, filtfilt +from identification import Kalman + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) class RobotData: """ @@ -10,16 +15,19 @@ class RobotData: Args: - dataFilePath (str): Path to the CSV data file. + - ndof (int) : number of freedom degree of the manipulator. + - time_step (float) : step time beteewn tow conscutive sample of data - intIndex (int): Initial index for data slicing. - stepIndex (int): Step index for periodic data selection. - fnlIndex (int): Final index for data slicing. """ - def __init__(self, dataFilePath, intIndex=None, stepIndex=1, fnlIndex=None): + def __init__(self, dataFilePath,ndof=7,time_step=1e-3, intIndex=None, stepIndex=1, fnlIndex=None): try: dataTable = pd.read_csv(dataFilePath) except Exception as e: - raise ValueError(f"Error loading data: {e}") - + logger.error(f"Error loading data: {e}") + + self.ndof = ndof if intIndex is None: intIndex = 0 if fnlIndex is None: @@ -42,8 +50,8 @@ def __init__(self, dataFilePath, intIndex=None, stepIndex=1, fnlIndex=None): self.desiredAcceleration = dataTable.iloc[intIndex:fnlIndex:stepIndex, 64:71].to_numpy() self.timeUnit = 'milliseconds' - self.timeStep = 1e-3 - self.samplingRate = 1000 + self.timeStep = time_step + self.samplingRate = 1/self.timeStep self.duration = (len(self.time) - 1) * self.timeStep self.maxJointVelocity = np.max(self.velocity, axis=0) @@ -71,7 +79,7 @@ def updateSamplingRate(self, new_sampling_rate): self.timeStep = 1 / self.samplingRate self.duration = (len(self.time) - 1) * self.timeStep - def LowPassfilter(self, cutoff_frequency): + def lowPassfilter(self, cutoff_frequency): """Filtring robot data using a butter low pass filter""" nyquist_freq = 0.5 * self.samplingRate normal_cutoff = cutoff_frequency / nyquist_freq @@ -87,58 +95,111 @@ def LowPassfilter(self, cutoff_frequency): smoothed_data['desiredAcceleration'] = filtfilt(b, a, self.desiredAcceleration, axis=0) return smoothed_data + + def kalmanFilter(self,variable='torque'): + """Filtering Robot Data torques uisng an adaptive kalman filter""" + if variable =='torque_cur': + observations = np.transpose(self.torque_cur) + x0 = self.torque_cur[0,:] + elif variable == 'torque_rne': + observations = np.transpose(self.torque_rne) + x0 = self.torque_rne[0,:] + elif variable =='torque': + observations = np.transpose(self.torque) + x0 = self.torque[0,:] + else: + logger.error('variable type not supported yet') + F = np.eye(self.ndof) + H = np.random.randn(self.ndof, self.ndof) + Q = np.eye(self.ndof) * 0.01 + R = np.eye(self.ndof) * 0.1 + P = np.eye(self.ndof) + kf = Kalman(F, H, Q, R, P, x0) + estimated_states, innovations = kf.filter(observations) + return estimated_states, innovations + + def plotCorrelationGraph(self,variable='torque'): + """ + Plot the correlation graph between joints variable data given by + varaible params + """ + if variable =='torque_cur': + observations = np.transpose(self.torque_cur) + + elif variable == 'torque_rne': + observations = np.transpose(self.torque_rne) + x0 = self.torque_rne[0,:] + elif variable =='torque': + observations = np.transpose(self.torque) + x0 = self.torque[0,:] + else: + logger.error('variable type not supported yet') + + sns.set(style="whitegrid") + + def plotVelocity(self)->None: """Plot the joints velocity recorded by the sensors.""" sns.set(style="whitegrid") - fig, axes = plt.subplots(3, 3, figsize=(10, 6)) - for i in range(7): + fig, axes = plt.subplots(3, 3, figsize=(12, 6)) + for i in range(self.ndof): ax = axes[i // 3, i % 3] sns.lineplot(ax=ax, x=np.arange(len(self.velocity[:, i])), y= self.velocity[:, i],linewidth=0.5) - ax.set_xlabel("Time (seconds)") + ax.set_xlabel("Time (ms)") ax.set_ylabel("Velocity") ax.set_title(f'Joint {i+1}') fig.suptitle('Joints Recorded Velocity', fontsize=11) - plt.show() def plotPosition(self)->None: """Plot the joints position recorded by the sensors.""" sns.set(style="whitegrid") - fig, axes = plt.subplots(3, 3, figsize=(10, 6)) - for i in range(7): + fig, axes = plt.subplots(3, 3, figsize=(12, 6)) + for i in range(self.ndof): ax = axes[i // 3, i % 3] sns.lineplot(ax = ax, x = np.arange(len(self.position[:, i])), y=self.position[:, i],linewidth=0.5) - ax.set_xlabel("Time (seconds)") + ax.set_xlabel("Time (ms)") ax.set_ylabel("Position") ax.set_title(f'Joint {i+1}') fig.suptitle('Joints Recorded Position', fontsize=10) fig.tight_layout(rect = [0, 0, 1, 0.95]) - plt.show() def plotTorque(self)->None: """Plot the joints torque""" sns.set(style="whitegrid") - fig, axes = plt.subplots(3, 3, figsize=(10, 6)) - for i in range(7): + fig, axes = plt.subplots(3, 3, figsize=(12, 6)) + for i in range(self.ndof): ax = axes[i // 3, i % 3] sns.lineplot(ax=ax, x=np.arange(len(self.torque[:, i])), y=self.torque[:, i],linewidth=0.5) - ax.set_xlabel("Time (seconds)") + ax.set_xlabel("Time (ms)") ax.set_ylabel("Torque (N.m)") ax.set_title(f'Joint {i+1}') fig.suptitle('Joints Recorded Torque', fontsize=11) fig.tight_layout(rect=[0, 0, 1, 0.95]) - plt.show() - + def plotCurrent(self)-> None: """Plot the joints current """ sns.set(style="whitegrid") - fig, axes = plt.subplots(3, 3, figsize=(10, 6)) - for i in range(7): + fig, axes = plt.subplots(3, 3, figsize=(12, 6)) + for i in range(self.ndof): ax = axes[i // 3, i % 3] sns.lineplot(ax=ax, x=np.arange(len(self.current[:, i])), y=self.current[:, i],linewidth=0.5) - ax.set_xlabel("Time (seconds)") + ax.set_xlabel("Time (ms)") ax.set_ylabel("Current (mA)") ax.set_title(f'Joint {i+1}') fig.suptitle('Joints Recorded Current', fontsize=11) fig.tight_layout(rect=[0, 0, 1, 0.95]) - plt.show() + + def plotAcceleration(self)->None: + """Plot the joints desired accleration""" + sns.set(style="whitegrid") + fig, axes = plt.subplots(3, 3, figsize=(12, 6)) + for i in range(self.ndof): + ax = axes[i // 3, i % 3] + sns.lineplot(ax=ax, x=np.arange(len(self.desiredAcceleration[:, i])),\ + y=self.desiredAcceleration[:, i],linewidth=0.5) + ax.set_xlabel("Time (ms)") + ax.set_ylabel("Acceleration") + ax.set_title(f'Joint {i+1}') + fig.suptitle('Joints desired acceleration', fontsize=11) + fig.tight_layout(rect=[0, 0, 1, 0.95]) \ No newline at end of file diff --git a/src/utils/Solver.py b/src/utils/Solver.py index b53b798..ca8cb10 100644 --- a/src/utils/Solver.py +++ b/src/utils/Solver.py @@ -1,17 +1,21 @@ import numpy as np +from numba import cuda import logging - - -def solveRiccatiAlgebricEquation(A, B, C, D): +def solveAlgebraicRiccatiEquation(A, B, C, D): """ Solve the discrete time algebric riccati equation given by : Args: - - A, B, C, D : System - Ref: - + - A, B, C, D : System + Returns: + - R ARE solution """ + # first check if the riccati equation has a solution !! + + R = np.zeros_like(A) + Q = np.empty_like(B) + residu = np.dot(np.transpose(A),R) R=1 return R diff --git a/src/utils/tools.py b/src/utils/tools.py index 832f213..9e9f1fd 100644 --- a/src/utils/tools.py +++ b/src/utils/tools.py @@ -86,6 +86,24 @@ def yaml2dict(yamlFilePath) -> dict: except yaml.YAMLError as e: logger.error(f"Error parsing YAML file '{yamlFilePath}': {e}") return {} + +def wrapArray(array:np.ndarray, lower_bound, upper_bound): + range_width = upper_bound - lower_bound + wrapped_array = lower_bound + (array - lower_bound) + + return wrapped_array + +def scaleArray(array:np.ndarray, lower_bound,upper_bound): + min_val = np.min(array) + max_val = np.max(array) + scaled_array = (upper_bound - lower_bound) * (array - min_val) / (max_val - min_val) + lower_bound + + return scaled_array + +def clampArray(array:np.ndarray, lower_bound, upper_bound): + clamped_array = np.clip(array, lower_bound, upper_bound) + + return clamped_array def plotArray(array: np.ndarray,title=None,ylabel = None) -> None: """ @@ -97,7 +115,7 @@ def plotArray(array: np.ndarray,title=None,ylabel = None) -> None: """ N = array.shape[0] if array.ndim ==1 : - fig = plt.figure(figsize=(10, 6)) + fig = plt.figure(figsize=(12, 6)) ax = fig.add_subplot(111) sns.lineplot(ax=ax, x=np.arange(N), y=array, linewidth=0.5, color='blue') ax.set_xlabel("Time (ms)", fontsize=9) @@ -107,7 +125,7 @@ def plotArray(array: np.ndarray,title=None,ylabel = None) -> None: ndof = min(array.shape[1],array.shape[0]) if not(ndof == array.shape[1]): array = np.transpose(array) - fig, axes = plt.subplots(3, 3, figsize=(10, 6), dpi=100) + fig, axes = plt.subplots(3, 3, figsize=(12, 6), dpi=100) axes = axes.flatten() for i in range(ndof): ax = axes[i] @@ -129,21 +147,15 @@ def plot2Arrays(array1: np.ndarray, array2: np.ndarray, legend1=None, legend2=No color1='red', color2='blue') -> None: """ Given two (n * m) data arrays where n >> m, plot each column data - from both arrays in separate subplots. - - Args: - - array1: First numpy ndarray - - array2: Second numpy ndarray - - legend1: Legend for the first array - - legend2: Legend for the second array - - color1: Line color for the first array - - color2: Line color for the second array - - title: Title for the figure + from both arrays in separate subplots. """ - assert array1.shape == array2.shape, "Arrays should have the same size." + assert array1.shape == array2.shape, "Arrays should have the same shapes." ndof = min(array1.shape[1],array1.shape[0]) - N = array1.shape[0] - fig, axes = plt.subplots(3, 3, figsize=(10, 6), dpi=100) + if ndof == array1.shape[1]: + N = array1.shape[0] + else: + N = array1.shape[1] + fig, axes = plt.subplots(3, 3, figsize=(12, 6), dpi=100) axes = axes.flatten() for i in range(ndof): @@ -166,19 +178,7 @@ def plot3Arrays(array1: np.ndarray, array2: np.ndarray, array3: np.ndarray, title=None, color1='red', color2='blue', color3='green') -> None: """ Given three (n * m) data arrays where n >> m, plot each column data - from all arrays in separate subplots. - - Args: - - array1: First numpy ndarray - - array2: Second numpy ndarray - - array3: Third numpy ndarray - - legend1: Legend for the first array - - legend2: Legend for the second array - - legend3: Legend for the third array - - color1: Line color for the first array - - color2: Line color for the second array - - color3: Line color for the third array - - title: Title for the figure + from all arrays in separate subplots. """ ndof = array1.shape[1] N = array1.shape[0] diff --git a/test/testIDIMNLS.py b/test/testIDIMNLS.py index 667fadc..0424164 100644 --- a/test/testIDIMNLS.py +++ b/test/testIDIMNLS.py @@ -2,39 +2,58 @@ import os import unittest import numpy as np +import matplotlib.pyplot as plt src_folder = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'src')) sys.path.append(src_folder) from identification import IDIMNLS +from utils import plotArray class TestIDIMNLS(unittest.TestCase): @staticmethod def modelFun(params): - array = np.random.normal(0, 0.5, size=(1000,10)) - for i, param in enumerate(params): - array += param * np.sin((i+1) * array) + t = np.linspace(0, 2*np.pi, 1000) + array = np.zeros((1000,7)) + for i in range(7): + array[:, i] = np.sin(params[0] * t + params[1] * i) \ + * params[2] + params[3] * np.cos(params[4] * t + params[5] * i) return array + def test_model_funtion(self): + objval = self.modelFun(np.random.rand(6)) + plotArray(objval) + plt.show() + self.assertEqual(objval.shape,(1000,7)) + def test_cost_function_not_none(self): - output = 1.2*np.random.normal(0,0.5,size=(1000,10)) - model = IDIMNLS(5,output,TestIDIMNLS.modelFun) - obj = model.computeLsCostFunction(np.random.rand(7)) + output = np.random.rand(1000,7) + model = IDIMNLS(6,output,TestIDIMNLS.modelFun) + obj = model.computeLsCostFunction(np.random.rand(6)) self.assertIsNotNone(obj) def test_cost_function_shape(self): - output = 1.2*np.random.normal(0,0.5,size=(1000,10)) - model = IDIMNLS(5,output,TestIDIMNLS.modelFun) - obj = model.computeLsCostFunction(np.random.rand(7)) + output = np.random.rand(1000,7) + model = IDIMNLS(6,output,TestIDIMNLS.modelFun) + obj = model.computeLsCostFunction(np.random.rand(6)) self.assertEqual(obj.ndim==0,True) def test_optimize_not_none(self): - output = 1.2*np.random.normal(0,0.5,size=(1000,10)) - model = IDIMNLS(5,output,TestIDIMNLS.modelFun) - sol = model.optimize(np.zeros(5)) - print(sol) + output = 1.2*np.random.normal(0,0.5,size=(1000,7)) + model = IDIMNLS(6,output,TestIDIMNLS.modelFun) + sol = model.optimize(np.zeros(6)) self.assertIsNotNone(sol) + def test_evaluate(self): + output = self.modelFun([0.5,1,0.3,0.01,0.5,1.5]) + model = IDIMNLS(5,output,TestIDIMNLS.modelFun) + sol = model.optimize(np.random.rand(6)) + self.assertIsNotNone(model.optimized_params) + print(model.optimized_params) + #model.visualizeError('opt err') + model.visualizeResults('results') + plt.show() + if __name__ == "__main__": unittest.main() \ No newline at end of file diff --git a/test/testKalman.py b/test/testKalman.py index 4809799..55cedc6 100644 --- a/test/testKalman.py +++ b/test/testKalman.py @@ -1,7 +1,10 @@ - +import unittest import numpy as np import matplotlib.pyplot as plt + + + # Define system matrices A = np.array([[1, 1], [0, 1]]) # State transition matrix H = np.array([[1, 0]]) # Observation matrix diff --git a/test/testRobot.py b/test/testRobot.py index 1174ff7..1559c67 100644 --- a/test/testRobot.py +++ b/test/testRobot.py @@ -98,6 +98,9 @@ def test_update_friction_params(self): tau_f2 = robot.computeFrictionTorques(qp,q) self.assertEqual(np.any(tau_f1!=tau_f2), True) + def test_identification_model_not_none(self): + robot = Robot() + if __name__ == "__main__": unittest.main() diff --git a/test/testSolver.py b/test/testSolver.py new file mode 100644 index 0000000..e69de29