diff --git a/.gitignore b/.gitignore index 73551789..a80e6628 100644 --- a/.gitignore +++ b/.gitignore @@ -29,6 +29,7 @@ MANIFEST *.npy *.csv *.mat +*.zip # Output files # ############################ diff --git a/dev/active_suspension/test_2d_curve.py b/dev/active_suspension/test_2d_curve.py deleted file mode 100644 index b89f2ec3..00000000 --- a/dev/active_suspension/test_2d_curve.py +++ /dev/null @@ -1,46 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Mon Oct 24 20:24:05 2022 - -@author: alex -""" - -import numpy as np - -import matplotlib -import matplotlib.pyplot as plt - -fig , ax = plt.subplots(2, sharex=True, figsize=(6,4), dpi=200, frameon=True) - -fig.canvas.manager.set_window_title('Figure Name') - -n = 200 - -x = np.linspace(0,10,n) - -a = np.array([ 0.5 , 0.3 , 0.7 , 0.2 , 0.2 , 0.1 ]) -w = np.array([ 0.2 , 0.4 , 0.5 , 1.0 , 2.0 , 3.0 ]) -phi = np.array([ 3.0 , 2.0 , 0.0 , 0.0 , 0.0 , 0.0 ]) - -y = np.zeros(n) -dy = np.zeros(n) - -for i in range(a.size): - y = y + a[i] * np.sin( w[i] * ( x - phi[i] )) - dy = dy + a[i] * w[i] * np.cos( w[i] * ( x - phi[i] )) - - -ax[0].plot( x , y , 'b') -ax[0].set_ylabel('y') -ax[0].axis('equal') -ax[0].grid(True) -ax[0].tick_params( labelsize = 8 ) - -ax[1].plot( x , dy , 'r') -ax[1].set_ylabel('dy') -ax[1].axis('equal') -ax[1].grid(True) -ax[1].tick_params( labelsize = 8 ) - -fig.show() \ No newline at end of file diff --git a/dev/dimentionless/cases_master.py b/dev/dimentionless/cases_master.py deleted file mode 100644 index 7d9b5056..00000000 --- a/dev/dimentionless/cases_master.py +++ /dev/null @@ -1,427 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Mon Nov 12 20:28:17 2018 - -@author: Alexandre -""" -############################################################################## -import numpy as np -import matplotlib.pyplot as plt -############################################################################## -from pyro.dynamic import pendulum -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming -from pyro.analysis import graphical -############################################################################## - - -def case( m , g , l , t_max_star , q_star , case_name = 'test ', show = True, rax = None , rax2 = None, res = 'reg', legend = 1): - - # Additionnal fixed domain dimentionless parameters - theta_star = 2.0 * np.pi - dtheta_star = 1.0 * np.pi - time_star = 2.0 * np.pi * 20.0 - - # Combined system parameters - omega = np.sqrt( ( g / l ) ) - mgl = m * g * l - - # Dimentional parameters - t_max = t_max_star * mgl - q = q_star * mgl - theta = theta_star - dtheta = dtheta_star * omega - time = time_star / omega - J_max = mgl**2 / omega * time_star * ( ( q_star * theta_star )**2 + t_max_star**2 ) - - print('\n\nCase :' + case_name ) - print('----------------------------------------------------') - print(' m=',m,' g=',g,' l=',l,' t_max=', t_max, ' q=', q) - - ################################ - # Dynamic system definition - ################################ - - sys = pendulum.InvertedPendulum() - - # kinematic - sys.lc1 = l - - sys.l1 = sys.lc1 - sys.l_domain = sys.lc1 * 2 - - # dynamic - sys.m1 = m - sys.I1 = 0 - sys.gravity = g - sys.d1 = 0 - - sys.u_ub[0] = + t_max - sys.u_lb[0] = - t_max - - sys.x_ub = np.array([ + theta , + dtheta ]) - sys.x_lb = np.array([ - theta , - dtheta ]) - - ################################ - # Discritized grid - ################################ - - if res == 'test' : - - dt = 0.5 - nx = 21 - nu = 3 - - elif res == 'plus' : - - dt = 0.05 - nx = 301 - nu = 101 - - elif res == 'hi' : - - dt = 0.025 - nx = 501 - nu = 101 - - else: - - dt = 0.05 - nx = 301 - nu = 21 - - grid_sys = discretizer.GridDynamicSystem( sys , [nx,nx] , [nu] , dt , True ) - - ################################ - # Cost function - ################################ - - qcf = costfunction.QuadraticCostFunction.from_sys(sys) - - qcf.xbar = np.array([ 0 , 0 ]) # target - qcf.INF = J_max - - qcf.Q[0,0] = q ** 2 - qcf.Q[1,1] = 0.0 - - qcf.R[0,0] = 1.0 - - ################################ - # Computing optimal policy - ################################ - - dp = dynamicprogramming.DynamicProgrammingWithLookUpTable( grid_sys, qcf ) - - steps = int( time / dt ) - - dp.compute_steps( steps ) - - - #grid_sys.fontsize = 10 - qcf.INF = 0.1 * J_max - dp.clean_infeasible_set() - - - ################################## - # Fig param - ################################## - - dpi = 300 - fontsize = 10 - figsize = (4, 3) - - ################################## - # Dimensional policy plot - ################################## - - fig = plt.figure( figsize = figsize, dpi=dpi, frameon=True) - fig.canvas.manager.set_window_title( 'dimentionless policy' ) - ax = fig.add_subplot(1, 1, 1) - - xname = r'$\theta \; [rad]$' - yname = r'$\dot{\theta} \; [rad/sec]$' - zname = r'$\tau \; [Nm]$' - - sys.state_label[0] = r'$\theta$' - sys.state_label[1] = r'$\dot{\theta}$' - sys.input_label[0] = r'$\tau$' - - xrange = 2.0 * np.pi - yrange = np.pi * np.sqrt( 10 / 1. ) - zrange = 20. - - ax.set_ylabel( yname, fontsize = fontsize ) - ax.set_xlabel( xname, fontsize = fontsize ) - - x_level = grid_sys.x_level[ 0 ] - y_level = grid_sys.x_level[ 1 ] - - - u = grid_sys.get_input_from_policy( dp.pi , 0 ) - J_grid_nd = grid_sys.get_grid_from_array( u ) - J_grid_2d = grid_sys.get_2D_slice_of_grid( J_grid_nd , 0 , 1 ) - - mesh = ax.pcolormesh( x_level, - y_level, - J_grid_2d.T, - shading='gouraud', - cmap = 'bwr', - vmin = -zrange, - vmax = zrange, - rasterized = True ) - - ax.tick_params( labelsize = fontsize ) - ax.grid(True) - ax.set_ylim( -yrange, +yrange) - ax.set_xlim( -xrange, xrange) - - cbar = fig.colorbar( mesh ) - - cbar.set_label(zname, fontsize = fontsize , rotation = 90 ) - - fig.tight_layout() - #fig.show() - fig.savefig( case_name + '_policy.pdf') - fig.savefig( case_name + '_policy.png') - fig.savefig( case_name + '_policy.jpg') - - if show: - plt.show() - else: - plt.close( fig ) - - - ################################## - # Trajectory plot - ################################## - - ctl = dp.get_lookup_table_controller() - - # Simulation - cl_sys = ctl + sys - cl_sys.x0 = np.array([-3.14, 0.]) - cl_sys.compute_trajectory( 10 , 6001, 'euler') - - tp = graphical.TrajectoryPlotter( sys ) - tp.fontsize = fontsize - tp.plot( cl_sys.traj , 'xu' , show = False ) - tp.plots[1].set_ylim([-5.5, 5.5]) - tp.plots[2].set_ylim([-zrange, zrange]) - tp.fig.savefig( case_name + '_traj.pdf') - tp.fig.savefig( case_name + '_traj.png') - tp.fig.savefig( case_name + '_traj.jpg') - - if show: - plt.show() - else: - plt.close( tp.fig ) - - - ################################## - # Dimensionless policy plot - ################################## - - fig = plt.figure( figsize= figsize, dpi=dpi, frameon=True) - fig.canvas.manager.set_window_title( 'dimentionless policy' ) - ax = fig.add_subplot(1, 1, 1) - - xname = r'$\theta^*$'#self.sys.state_label[x] #+ ' ' + self.sys.state_units[x] - yname = r'$\dot{\theta}^* = \frac{\dot{\theta}}{\omega}$'#self.sys.state_label[y] #+ ' ' + self.sys.state_units[y] - zname = r'$\tau^*=\frac{\tau}{mgl}$' - - ax.set_ylabel(yname, fontsize = fontsize ) - ax.set_xlabel(xname, fontsize = fontsize ) - - x_level = grid_sys.x_level[ 0 ] * 1 - y_level = grid_sys.x_level[ 1 ] * (1 / omega) - - - u = grid_sys.get_input_from_policy( dp.pi , 0 ) - - u2 = u * (1/mgl) - - J_grid_nd = grid_sys.get_grid_from_array( u2 ) - - J_grid_2d = grid_sys.get_2D_slice_of_grid( J_grid_nd , 0 , 1 ) - - mesh = ax.pcolormesh( x_level, - y_level, - J_grid_2d.T, - shading='gouraud', - cmap = 'bwr', - rasterized = True ) - - ax.tick_params( labelsize = fontsize ) - ax.grid(True) - - cbar = fig.colorbar( mesh ) - - cbar.set_label(zname, fontsize = fontsize , rotation = 90 ) - - fig.tight_layout() - #fig.show() - fig.savefig( case_name + '_dimpolicy.pdf') - fig.savefig( case_name + '_dimpolicy.png') - fig.savefig( case_name + '_dimpolicy.jpg') - - if show: - plt.show() - else: - plt.close( fig ) - - - if rax is not None: - - ############################### - # 2D policy regime figure (dtheta = 0 ) - ############################### - - n = 101 - x_min = - theta_star - 0.1 - x_max = + theta_star + 0.1 - - x = np.linspace( x_min, x_max, n) - u = np.zeros(n) - - for i in range(n): - ri = 0 - xi = np.array([ x[i] , 0.0 ]) - ti = 0 - u[i] = ctl.c( xi, ri, ti) * (1/mgl) - - - if legend == 1: - rax.plot( x , u , label= r'$\tau_{max}^* =$ %0.1f' % t_max_star ) - elif legend == 2: - rax.plot( x , u , label= r'$q^* =$ %0.2f' % q_star ) - else: - rax.plot( x , u ) - - rax.set_xlim([ x_min, x_max ]) - rax.set_xlabel( xname, fontsize = fontsize ) - rax.grid(True) - rax.tick_params( labelsize = fontsize ) - rax.set_ylabel( zname, fontsize = fontsize ) - - if rax2 is not None: - - ############################### - # 2D policy regime figure (theta = -np.pi ) - ############################### - - n = 101 - x_min = - dtheta_star - 0.1 - x_max = + dtheta_star + 0.1 - - x = np.linspace( x_min, x_max, n) - u = np.zeros(n) - - for i in range(n): - ri = 0 - xi = np.array([ -np.pi , x[i] ]) - ti = 0 - u[i] = ctl.c( xi, ri, ti) * (1/mgl) - - - if legend == 1: - rax2.plot( x , u , label= r'$\tau_{max}^* =$ %0.1f' % t_max_star ) - elif legend == 2: - rax2.plot( x , u , label= r'$q^* =$ %0.2f' % q_star ) - else: - rax2.plot( x , u ) - - rax2.set_xlim([ x_min, x_max ]) - rax2.set_xlabel( yname, fontsize = fontsize ) - rax2.grid(True) - rax2.tick_params( labelsize = fontsize ) - rax2.set_ylabel( zname, fontsize = fontsize) - - - return dp , cl_sys - - - - -def sensitivity( ts , qs , res = 'mid' , name = 'sensitivity' , legend = 1): - - rfig = plt.figure(figsize= (4, 3), dpi=300, frameon=True) - rax = rfig.add_subplot(1, 1, 1) - - rfig2 = plt.figure(figsize= (4, 3), dpi=300, frameon=True) - rax2 = rfig2.add_subplot(1, 1, 1) - - n = ts.size - - - for i in range(n): - - case( m=1 , g=10 , l=1 , t_max_star= ts[i] , q_star= qs[i] , case_name = name + '_level_' + str(i+1) , show = False, rax = rax, rax2 = rax2, res = res, legend = legend) - - - rax.legend( loc = 'upper right' ) - rfig.tight_layout() - rfig.show() - rfig.savefig( name + '.pdf') - rfig.savefig( name + '.png') - rfig.savefig( name + '.jpg') - - rax2.legend( loc = 'upper right' ) - rfig2.tight_layout() - rfig2.show() - rfig2.savefig( name + '2.pdf') - rfig2.savefig( name + '2.png') - rfig2.savefig( name + '2.jpg') - - return (rfig, rax, rfig2, rax2) - - - -#################################### -### Quick tests -#################################### - -res = 'test' - - -dp , cl_sys = case( m=1 , g=10 , l=1 , t_max_star=0.5 , q_star= 0.1 , case_name = 'test', res = res, show = True ) - -# ts = np.array([ 0.1, 0.8 ]) -# qs = np.array([ 0.05, 0.05 ]) -# fig, ax, fig2, ax2 = sensitivity(ts, qs , res = res , name = 's1_test', legend = 1) -# fig, ax, fig2, ax2 = sensitivity(ts, qs , res = res , name = 's2_test', legend = 2) - -#################################### -### Main figures -#################################### - -# res = 'hi' - -# case( m=1 , g=10 , l=1 , t_max_star=0.5 , q_star= 0.1 , case_name = 'c1', res = res) -# case( m=1 , g=10 , l=2 , t_max_star=0.5 , q_star= 0.1 , case_name = 'c2', res = res) -# case( m=2 , g=10 , l=1 , t_max_star=0.5 , q_star= 0.1 , case_name = 'c3', res = res) -# case( m=1 , g=10 , l=1 , t_max_star=1.0 , q_star= 0.05 , case_name = 'c4', res = res) -# case( m=1 , g=10 , l=2 , t_max_star=1.0 , q_star= 0.05 , case_name = 'c5', res = res) -# case( m=2 , g=10 , l=1 , t_max_star=1.0 , q_star= 0.05 , case_name = 'c6', res = res) -# case( m=1 , g=10 , l=1 , t_max_star=1.0 , q_star= 10.0 , case_name = 'c7', res = res) -# case( m=1 , g=10 , l=2 , t_max_star=1.0 , q_star= 10.0 , case_name = 'c8', res = res) -# case( m=2 , g=10 , l=1 , t_max_star=1.0 , q_star= 10.0 , case_name = 'c9', res = res) - -#################################### -### Sensitivity Figures -#################################### - -# res = 'hi' - -ts = np.array([ 0.1, 0.3, 0.5, 1.0, 2.5, 5.0 ]) -qs = np.array([ 0.05, 0.05, 0.05, 0.05, 0.05, 0.05 ]) - -# fig, ax, fig2, ax2 = sensitivity(ts, qs , res = res , name = 's_tmax', legend = 1) - -ts = np.array([ 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5 ]) -qs = np.array([ 0.05, 0.10, 0.12, 0.15, 0.30, 0.50, 1.0, 2.0 ]) - -# fig, ax, fig2, ax2 = sensitivity(ts, qs , res = res , name = 's_q', legend = 2) - - - \ No newline at end of file diff --git a/dev/hybrid_systems/hybrid.py b/dev/hybrid_systems/hybrid.py deleted file mode 100755 index e5e43a00..00000000 --- a/dev/hybrid_systems/hybrid.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Wed Dec 9 22:04:53 2020 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import ContinuousDynamicSystem - -from pyro.analysis import simulation - - -############################################################################### -class SwitchedSystem( ContinuousDynamicSystem): - """ - Class in construction !!!!!! - - """ - ############################################ - def __init__(self, n, m, p, k): - - super().__init__(n, m, p) - - - self.k = k # Number of discrte modes - - - ############################################# - def f(self, x, u, t): - - u_mode = int(u[0]) - u_cont = u[1:] - - ################ - if u_mode == 0: - - dx = u_cont - - ################ - if u_mode == 1: - - dx = u_cont - - ################ - else: - - dx = np.zeros( self.n ) - - return dx - - - ############################# - def compute_trajectory( - self, tf=10, n=100001): - """ - Simulation of time evolution of the system - ------------------------------------------------ - tf : final time - n : time steps - """ - - sim = simulation.Simulator(self, tf, n, 'euler') - - self.traj = sim.compute() # save the result in the instance - - return self.traj \ No newline at end of file diff --git a/dev/hybrid_systems/hybrid_mechanical.py b/dev/hybrid_systems/hybrid_mechanical.py deleted file mode 100644 index beca11d9..00000000 --- a/dev/hybrid_systems/hybrid_mechanical.py +++ /dev/null @@ -1,462 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Wed Apr 6 13:46:06 2022 -@author: alex -""" - -############################################################################### -import numpy as np -############################################################################### -from pyro.dynamic import system -from pyro.dynamic import mechanical -from hybrid import SwitchedSystem -############################################################################### - -############################################################################### - -class HybridMechanicalSystem( mechanical.MechanicalSystem , SwitchedSystem ): - """ - Mechanical system with Equation of Motion in the form of - ------------------------------------------------------- - H(q,k) ddq + C(q,dq) dq + d(q,dq,k) + g(q) = B(q,k) e - ------------------------------------------------------- - u : dim = (m+1, 1) : mode + force input variables - k=u[0] : integer : operating mode - e = u[1,:] : dim = (m, 1) : force/torque input variables - q : dim = (dof, 1) : position variables - dq : dim = (dof, 1) : velocity variables - ddq : dim = (dof, 1) : acceleration variables - H(q,k) : dim = (dof, dof) : inertia matrix - C(q) : dim = (dof, dof) : corriolis matrix - B(q,k) : dim = (dof, m) : actuator matrix - ddq : dim = (dof, 1) : acceleration variables - d(q,dq,k): dim = (dof, 1) : state-dependent dissipative forces - g(q) : dim = (dof, 1) : state-dependent conservatives forces - - """ - - ############################ - def __init__(self, dof = 1 , actuators = None, k = 2): - """ """ - - # Degree of Freedom - self.dof = dof - self.k = k - - # Nb of actuators - if actuators == None: # If not specifyied the sys is fully actuated - actuators = dof - - # Dimensions - n = dof * 2 - m = actuators + 1 - p = dof * 2 - - # initialize standard params - system.ContinuousDynamicSystem.__init__(self, n, m, p) - - # Name - self.name = str(dof) + 'DoF Hybrid Mechanical System' - - # Labels, bounds and units - for i in range(dof): - # joint angle states - self.x_ub[i] = + np.pi * 2 - self.x_lb[i] = - np.pi * 2 - self.state_label[i] = 'Angle '+ str(i) - self.state_units[i] = '[rad]' - # joint velocity states - self.x_ub[i+dof] = + np.pi * 2 - self.x_lb[i+dof] = - np.pi * 2 - self.state_label[i+dof] = 'Velocity ' + str(i) - self.state_units[i+dof] = '[rad/sec]' - for i in range(actuators): - self.u_ub[i+1] = + 5 - self.u_lb[i+1] = - 5 - self.input_label[i+1] = 'Torque ' + str(i) - self.input_units[i+1] ='[Nm]' - self.output_label = self.state_label - self.output_units = self.state_units - - self.u_ub[0] = k - 1 - self.u_lb[0] = 0 - self.input_label[0] = 'Mode' - self.input_units[0] ='' - - ########################################################################### - # The following functions needs to be overloaded by child classes - # to represent the dynamic of the system - ########################################################################### - - ########################################################################### - def H(self, q , k ): - """ - Inertia matrix - ---------------------------------- - dim( H ) = ( dof , dof ) - - such that --> Kinetic Energy = 0.5 * dq^T * H(q) * dq - - """ - if k == 0: - H = np.diag( np.ones( self.dof ) ) * 100 # Default is identity matrix - else: - H = np.diag( np.ones( self.dof ) ) # Default is identity matrix - - return H - - - ########################################################################### - def B(self, q , k ): - """ - Actuator Matrix : dof x m - """ - - B = np.zeros( ( self.dof , self.m ) ) - - if k == 0: - for i in range(min(self.m,self.dof)): - B[i,i] = 10 # Diag matrix for the first m rows - else: - for i in range(min(self.m,self.dof)): - B[i,i] = 1 # Diag matrix for the first m rows - - return B - - - ########################################################################### - def d(self, q , dq , k ): - """ - State-dependent dissipative forces : dof x 1 - """ - - if k == 0: - D = np.ones( self.dof ) * 100 # Default is zero vector - else: - D = np.zeros(self.dof ) # Default is zero vector - - d = np.dot( D , dq ) - - return d - - - ########################################################################### - # No need to overwrite the following functions for custom system - ########################################################################### - - ############################## - def u2k(self, u ): - """ Compute mode k based on u vector """ - - return int(u[0]) - - - ############################## - def u2e(self, u ): - """ Compute actuator effort e based on u vector """ - - return u[1:] - - - ############################## - def generalized_forces(self, q , dq , ddq , k , t = 0 ): - """ Computed generalized forces given a trajectory """ - - H = self.H( q , k ) - C = self.C( q , dq ) - g = self.g( q ) - d = self.d( q , dq , k ) - - # Generalized forces - forces = np.dot( H , ddq ) + np.dot( C , dq ) + g + d - - return forces - - - ############################## - def actuator_forces(self, q , dq , ddq , k , t = 0 ): - """ Computed actuator forces given a trajectory (inverse dynamic) """ - - if self.dof == (self.m - 1): - - B = self.B( q , k ) - - # Generalized forces - forces = self.generalized_forces( q , dq , ddq , k , t ) - - # Actuator forces - u = np.dot( np.linalg.inv( B ) , forces ) - - return u - - else: - - raise NotImplementedError - - - ############################## - def ddq(self, q , dq , u , t = 0 ): - """ Computed accelerations given actuator forces (foward dynamic) """ - - k = self.u2k(u) - e = self.u2e(u) - - H = self.H( q , k ) - C = self.C( q , dq ) - g = self.g( q ) - d = self.d( q , dq , k ) - B = self.B( q , k ) - - ddq = np.dot( np.linalg.inv( H ) , ( np.dot( B , e ) - - np.dot( C , dq ) - g - d ) ) - - return ddq - - - - ########################################################################### - def kinetic_energy(self, q , dq , k ): - """ Compute kinetic energy of manipulator """ - - e_k = 0.5 * np.dot( dq , np.dot( self.H( q , k ) , dq ) ) - - return e_k - - -############################################################################## -# Hybrid Mechanica lSystem -############################################################################## - -class MultipleSpeedMechanicalSystem( HybridMechanicalSystem ): - """ - Mechanical system with Equation of Motion in the form of - ------------------------------------------------------- - [ H(q) + R(k)^T I R(k) ] ddq + C(q,dq) dq + [d_mech(q,dq) + R(k)^T B R(k) dq] + g(q) = R(k)^T e - ------------------------------------------------------- - u : dim = (m+1, 1) : mode + force input variables - k=u[0] : integer : operating mode - e = u[1,:] : dim = (m, 1) : force/torque input variables - q : dim = (dof, 1) : position variables - dq : dim = (dof, 1) : velocity variables - ddq : dim = (dof, 1) : acceleration variables - H_mech(q) : dim = (dof, dof) : mechanism inertia matrix - I_actuator : dim = (dof, dof) : actuator inertia matrix - B_actuator : dim = (dof, dof) : actuator damping matrix - C(q) : dim = (dof, dof) : corriolis matrix - R : dim = (dof, m) : actuator matrix - ddq : dim = (dof, 1) : acceleration variables - d_mech(q,dq): dim = (dof, 1) : state-dependent dissipative forces - g(q) : dim = (dof, 1) : state-dependent conservatives forces - - """ - - ############################ - def __init__(self, dof = 1 , k = 2): - """ """ - - super().__init__(dof, dof, k) - - # Name - self.name = str(dof) + ' DoF Multiple Speed Mechanical System' - - # Number of discrete modes - self.k = k - - # Actuator - self.I_actuators = np.diag( np.ones( self.dof ) ) - self.B_actuators = np.diag( np.ones( self.dof ) ) - - # Transmissions - self.R_options = [ np.diag( np.ones( self.dof ) ) , - np.diag( np.ones( self.dof ) ) * 10 ] - - - - ########################################################################### - # The following functions needs to be overloaded by child classes - # to represent the dynamic of the system - ########################################################################### - - ########################################################################### - def H_mech(self, q ): - """ - Inertia matrix of arm mechanism only - ---------------------------------- - dim( H ) = ( dof , dof ) - - """ - - H = np.diag( np.ones( self.dof ) ) # Default is identity matrix - - return H - - - ########################################################################### - def H(self, q , k ): - """ """ - - R = self.R_options[k] - - H = self.H_mech(q) + R.T @ self.I_actuators @ R - - return H - - - ########################################################################### - def B(self, q , k ): - """ - Actuator Matrix : dof x m - """ - - R = self.R_options[k] - - return R.T - - ########################################################################### - def d_mech(self, q , dq ): - """ - State-dependent dissipative forces : dof x 1 - """ - - d = np.zeros( self.dof ) - - return d - - - ########################################################################### - def d(self, q , dq , k ): - """ - State-dependent dissipative forces : dof x 1 - """ - - R = self.R_options[k] - - d = self.d_mech(q, dq) + R.T @ self.B_actuators @ R @ dq - - return d - - - - -########################################################################### -class TwoSpeedLinearActuator( HybridMechanicalSystem ): - """ - - - """ - - ############################ - def __init__(self): - """ """ - - super().__init__( 1 , 1 ) - - self.mass_output = 10 - - self.motor_inertia = 1 - - self.ratios = np.array([1,10]) - - self.motor_damping = 1 - - self.l2 = 1 - - ########################################################################### - # The following functions needs to be overloaded by child classes - # to represent the dynamic of the system - ########################################################################### - - ########################################################################### - def H(self, q , k ): - """ - Inertia matrix - ---------------------------------- - dim( H ) = ( dof , dof ) - - such that --> Kinetic Energy = 0.5 * dq^T * H(q) * dq - - """ - H = np.array([[ self.mass_output + self.motor_inertia * self.ratios[k]**2 ]]) - - return H - - - ########################################################################### - def B(self, q , k ): - """ - Actuator Matrix : dof x m - """ - - B = np.array([[ self.ratios[k] ]]) - - return B - - - ########################################################################### - def d(self, q , dq , k ): - """ - State-dependent dissipative forces : dof x 1 - """ - - D = np.array([[ self.motor_damping * self.ratios[k]**2 ]]) - - d = np.dot( D , dq ) - - return d - - - ########################################################################### - def forward_kinematic_lines(self, q ): - """ - Compute points p = [x;y;z] positions given config q - ---------------------------------------------------- - - points of interest for ploting - - Outpus: - lines_pts = [] : a list of array (n_pts x 3) for each lines - - """ - - lines_pts = [] # list of array (n_pts x 3) for each lines - lines_style = [] - lines_color = [] - - # mass - pts = np.zeros(( 5 , 3 )) - - - pts[0,:] = np.array([q[0] - self.l2/2,+self.l2/2,0]) - pts[1,:] = np.array([q[0] + self.l2/2,+self.l2/2,0]) - pts[2,:] = np.array([q[0] + self.l2/2,-self.l2/2,0]) - pts[3,:] = np.array([q[0] - self.l2/2,-self.l2/2,0]) - pts[4,:] = pts[0,:] - - lines_pts.append( pts ) - lines_style.append( '-') - lines_color.append( 'k') - - return lines_pts , lines_style , lines_color - - -''' -################################################################# -################## Main ######## -################################################################# -''' - - -if __name__ == "__main__": - - sys = MultipleSpeedMechanicalSystem(2,2) - - sys.x0[1] = 0 - - sys.ubar[0] = 1 - sys.ubar[1] = 5 - - sys.compute_trajectory() - - sys.plot_trajectory('xu') - - \ No newline at end of file diff --git a/dev/hybrid_systems/rmincontroller b/dev/hybrid_systems/rmincontroller deleted file mode 100644 index 73fd800c..00000000 --- a/dev/hybrid_systems/rmincontroller +++ /dev/null @@ -1,260 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Apr 8 00:14:29 2022 - -@author: alex -""" - -############################################################################### -import numpy as np -from scipy.interpolate import interp1d -############################################################################### -from pyro.control.nonlinear import ComputedTorqueController -############################################################################### - - -############################################################################### -# Computed Torque -############################################################################### - -class RminComputedTorqueController( ComputedTorqueController ) : - """ - Inverse dynamic controller for mechanical system - - """ - - ############################ - def __init__(self, model , traj = None ): - """ - --------------------------------------- - r : reference signal vector k x 1 - y : sensor signal vector p x 1 - u : control inputs vector m x 1 - t : time 1 x 1 - --------------------------------------- - u = c( y , r , t ) - - """ - - super().__init__( model , traj ) - - # Label - self.name = 'R min Computed Torque Controller' - - self.modes_options = model.k - - - ############################# - def c_fixed_goal( self , y , r , t = 0 ): - """ - Feedback static computation u = c(y,r,t) - - INPUTS - y : sensor signal vector p x 1 - r : reference signal vector k x 1 - t : time 1 x 1 - - OUPUTS - u : control inputs vector m x 1 - - """ - - x = y - q_d = r - - u = self.fixed_goal_ctl( x , q_d , t ) - - return u - - - - ############################ - def fixed_goal_ctl( self , x , q_d , t = 0 ): - """ - - Given desired fixed goal state and actual state, compute torques - - """ - [ q , dq ] = self.model.x2q( x ) - - ddq_d = np.zeros( self.model.dof ) - dq_d = np.zeros( self.model.dof ) - - ddq_r = self.compute_ddq_r( ddq_d , dq_d , q_d , dq , q ) - - u = self.rmin_forces( q , dq , ddq_r ) - - return u - - - ############################ - def traj_following_ctl( self , x , t = 0 ): - """ - - Given desired loaded trajectory and actual state, compute torques - - """ - - [ q , dq ] = self.model.x2q( x ) - - ddq_d , dq_d , q_d = self.get_traj( t ) - - ddq_r = self.compute_ddq_r( ddq_d , dq_d , q_d , dq , q ) - - u = self.rmin_forces( q , dq , ddq_r ) - - return u - - - ############################ - def rmin_forces( self, q , dq , ddq_r ): - """ """ - - # Cost is Q - costs = np.zeros( self.modes_options ) - efforts = np.zeros( ( self.modes_options , self.model.dof ) ) - - #for all gear ratio options - for k in range( self.modes_options ): - - efforts[k] = self.model.actuator_forces( q , dq , ddq_r , k ) - - - costs[k] = np.dot( efforts[k] , efforts[k] ) - - """ - u = np.append( efforts[k] , k ) - x = self.model.q2x(q, dq) - - if self.model.isavalidinput( x , k ): - # Cost is norm of torque - costs[k] = np.dot( efforts[k] , efforts[k] ) - else: - # Bad option - costs[k] = 9999999999 # INF - """ - - # Optimal dsicrete mode - k_star = costs.argmin() - e_star = efforts[ k_star ] - - u = np.append( k_star , e_star ) - - return u - - - ############################ - def compute_ddq_r( self , ddq_d , dq_d , q_d , dq , q ): - """ - - Given desired trajectory and actual state, compute ddq_r - - """ - - q_e = q - q_d - dq_e = dq - dq_d - - ddq_r = ddq_d - 2 * self.zeta * self.w0 * dq_e - self.w0 ** 2 * q_e - - return ddq_r - - - ############################ - def load_trajectory( self , traj ): - """ - - Load Open-Loop trajectory solution to use as reference trajectory - - """ - - self.trajectory = traj - - q = traj.x[ :, 0 : self.model.dof ] - dq = traj.x[ :, self.model.dof : 2 * self.model.dof ] - ddq = traj.dx[:, self.model.dof : 2 * self.model.dof ] - t = traj.t - - # Create interpol functions - self.q = interp1d(t,q.T) - self.dq = interp1d(t,dq.T) - self.ddq = interp1d(t,ddq.T) - - - ############################ - def get_traj( self , t ): - """ - - Find closest point on the trajectory - - """ - - if t < self.trajectory.time_final : - - # Load trajectory - q = self.q( t ) - dq = self.dq( t ) - ddq = self.ddq( t ) - - else: - - q = self.rbar - dq = np.zeros( self.model.dof ) - ddq = np.zeros( self.model.dof ) - - return ddq , dq , q - - - ############################# - def c_trajectory_following( self , y , r , t ): - """ - Feedback static computation u = c(y,r,t) - - INPUTS - y : sensor signal vector p x 1 - r : reference signal vector k x 1 - t : time 1 x 1 - - OUPUTS - u : control inputs vector m x 1 - - """ - - x = y - - u = self.traj_following_ctl( x , t ) - - - return u - - - - -''' -################################################################# -################## Main ######## -################################################################# -''' - - -if __name__ == "__main__": - """ MAIN TEST """ - - from hybrid_mechanical import MultipleSpeedMechanicalSystem - - - sys = MultipleSpeedMechanicalSystem(1,2) - - sys.B_actuators[0,0] = 0.1 - sys.I_actuators[0,0] = 0.1 - - ctl = RminComputedTorqueController( sys ) - - # New cl-dynamic - cl_sys = ctl + sys - - cl_sys.x0 = np.array([2,0]) - cl_sys.compute_trajectory() - #cl_sys.animate_simulation() - cl_sys.plot_trajectory('xu') - \ No newline at end of file diff --git a/dev/hybrid_systems/test_hybrid_vi.py b/dev/hybrid_systems/test_hybrid_vi.py deleted file mode 100644 index 7372761d..00000000 --- a/dev/hybrid_systems/test_hybrid_vi.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Wed Apr 6 14:53:54 2022 - -@author: alex -""" - -############################################################################### -import numpy as np -############################################################################### -from hybrid_mechanical import TwoSpeedLinearActuator - -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import valueiteration -############################################################################### - -sys = TwoSpeedLinearActuator() - -############################################################################### - -# Planning - -# Set domain -sys.x_ub = np.array([+3, +3]) -sys.x_lb = np.array([-3, -3]) - -sys.u_ub = np.array([+1, +1]) -sys.u_lb = np.array([-0, -1]) - -# Discrete world -grid_sys = discretizer.GridDynamicSystem(sys, (51, 51), (2, 11), 0.1) - -# Cost Function -cf = costfunction.QuadraticCostFunction.from_sys( sys ) -cf.xbar = np.array( [1, 0] ) # target -cf.INF = 1E9 -cf.EPS = 0.2 -cf.R = np.array([[0,0],[0,1]]) - -# VI algo -vi = valueiteration.ValueIteration_ND( grid_sys , cf ) - -vi.uselookuptable = True -vi.initialize() - - -vi.compute_steps(100) - -vi.assign_interpol_controller() -vi.plot_policy(0) -vi.plot_policy(1) - -cl_sys = vi.ctl + sys - -cl_sys.x0 = np.array([0, 0]) -cl_sys.compute_trajectory( 10, 10001, 'euler') -cl_sys.plot_trajectory('xu') diff --git a/dev/kinematic/__init__.py b/dev/kinematic/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/dev/kinematic/kinematic.py b/dev/kinematic/kinematic.py deleted file mode 100644 index 65fa95bb..00000000 --- a/dev/kinematic/kinematic.py +++ /dev/null @@ -1,659 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Sat Feb 11 17:07:22 2012 - -@author: gira2403 -""" - - -""" -########################################################### -# Modules -########################################################### -""" - -# Numpy -import numpy as np -from numpy.linalg import norm - - -""" -########################################################### -## Constantes -########################################################### -""" - -epsilon = 1e-12 - - -""" -########################################################### -## Class -########################################################### -""" - -########################################################### -## 3D Vector -########################################################### - -class Vector: - """ 3D Vector """ - - def __init__(self, x = 0, y = 0, z = 0): - """ Default is a null vector """ - - self.x = x - self.y = y - self.z = z - - """ Numpy matrix (COLUNM) """ - self.col = np.matrix([[x],[y],[z]]) - - """ Numpy skew matrix """ - self.skew = np.matrix([[0,-z,y],[z,0,-x],[-y,x,0]]) - - """ Vectorial norm """ - self.norm = norm(self.col) - - - ################################################## - def __call__(self): - - print('3D Vector : [',self.x,';',self.y,';',self.z,']') - - - ################################# - def __add__(self,other): - """ Vectorial addition """ - - x = self.x + other.x - y = self.y + other.y - z = self.z + other.z - - return Vector(x,y,z) - - - ################################# - def __sub__(self,other): - """ Vectorial substraction """ - - x = self.x - other.x - y = self.y - other.y - z = self.z - other.z - - return Vector(x,y,z) - - - ################################# - def __neg__(self): - """ Vectorial inversion """ - - x = - self.x - y = - self.y - z = - self.z - - return Vector(x,y,z) - - - ################################# - def __mul__(self,other): - """ Scalar multiplication with a scalar or dot product with a vector """ - - """ Check if its a scalar multiplication or a dot product """ - isvector = isinstance(other,Vector) - - ############## - if isvector: - - # Dot product - ans = other.col.T * self.col - - return ans[0,0] - - ############## - else: - - # Scalar multiplication - x = self.x * other - y = self.y * other - z = self.z * other - - return Vector(x,y,z) - - - ################################# - def __pow__(self,other): - """ Vectorial product """ - - col = self.skew * other.col - - return col2vec(col) - - - ################################# - def normalize(self): - """ Return normalized vector """ - - ############################ - if self.norm > 0 : - - direction = self * ( 1 / self.norm ) - - else : - print('Kinematic warning : not able to normalize a zero vector') - direction = Vector(0,0,0) - - - return direction - - - ################################# - def copy(self): - """ Return a copy of the current vector """ - - copy = self * 1 - - return copy - - - -########################################################### -## Rotation Matrix -########################################################### - -class RotationMatrix: - """ Matrix representation of a 3D rotation """ - - ########################### - def __init__(self, matrix = np.matrix(np.eye(3,3)) ): - """ Default is a identity matrix """ - - self.C = matrix - - - ####################### - def __call__(self): - - print('Rotation Matrix : \n', self.C) - - - ################################# - def __mul__(self,other): - """ Matrix multiplication """ - - """ Check if other is RotationMatrix or a vector """ - isvector = isinstance(other,Vector) - ismatrix = isinstance(other,RotationMatrix) - - ################## - if isvector: - - col = self.C * other.col - - return col2vec(col) - - ################## - elif ismatrix: - - mat = self.C * other.C - - return RotationMatrix(mat) - - ################## - else: - """ Scale the rotation arround the same axis with a scalar """ - - new_rotation = self.toAngleAxis() * other - - return AngleAxis2RotationMatrix(new_rotation) - - - ################################# - def __neg__(self): - """ Inverse Matrix """ - - return RotationMatrix(self.C.T) - - - ################################# - def toAngleAxis(self): - """ Compute equivalent Angle-Axis representation """ - - return RotationMatrix2AngleAxis(self) - - - ################################# - def toQuaternion(self): - """ Compute equivalent quaternion representation """ - - return RotationMatrix2Quaternion(self) - - - ################################# - def toRotationVector(self): - """ Compute equivalent Rotation Vector """ - - a = self.toAngleAxis() - - return a.toRotationVector() - - -########################################################### -## Angle-Axis 3D rotation -########################################################### - -class AngleAxis: - """ Angle-Axis representation of a rotation in 3D """ - - ################ - def __init__(self, rad = 0, axis = Vector(1,0,0) ): - """ Default is a rotation of 0 degree arround x """ - - self.rad = rad - self.deg = np.rad2deg(rad) - - self.axis = axis - - - ################ - def __call__(self): - """ Print Angle Axis """ - - print('AngleAxis: \n deg : ',self.deg,' axis : [',self.axis.x,';',self.axis.y,';',self.axis.z,']') - - - ################################# - def __mul__(self,other): - """ Scale the rotation around the same axis """ - - rad = self.rad * other - axis = self.axis.copy() - - return AngleAxis(rad, axis) - - - ################ - def toRotationMatrix(self): - """ Convert to 3x3 rotation matrix """ - - return AngleAxis2RotationMatrix(self) - - - ################ - def toQuaternion(self): - """ Convert to Quaternion """ - - return AngleAxis2Quaternion(self) - - - ################ - def toRotationVector(self): - """ Convert to Rotation Vector """ - - vector = self.axis * self.rad - - return RotationVector( vector.x , vector.y , vector.z ) - - -########################################################### -## Vecteur Rotation Rodrigues ( axis * angle ) -########################################################### - -class RotationVector(Vector): - """ 3D Vector representation of a rotation in 3D (angle axis : axis * angle) """ - - #################### - def __call__(self): - """ Print Rotation Vector """ - - print('3D Rotation Vector : [',self.x,';',self.y,';',self.z,']') - - - #################### - def toAngleAxis(self): - """ Convert to Angle-Axis """ - - rad = self.norm - axis = self.normalize() - - return AngleAxis(rad,axis) - - ################ - def toRotationMatrix(self): - """ Convert to 3x3 rotation matrix """ - - a = self.toAngleAxis() - - return AngleAxis2RotationMatrix(a) - - - ################ - def toQuaternion(self): - """ Convert to Quaternion """ - - a = self.toAngleAxis() - - return AngleAxis2Quaternion(a) - - -########################################################### -## Quaternion -########################################################### - -class Quaternion: - """ Quaternion representation of a 3D rotation """ - - ################# - def __init__(self, e = Vector(), n = 1 ): - """ Default is a zero rotation """ - - self.e = e - self.n = n - - - ################ - def __call__(self): - """ Print Quaternion """ - - print('Quaternion: \n e : [',self.e.x,';',self.e.y,';',self.e.z,'] n : ',self.n) - - - ################################# - def __mul__(self,other): - """ Quaternion multiplication or vector rotation with the Quaternion """ - - - """ Check if other is RotationMatrix or a vector """ - isvector = isinstance(other,Vector) - isquaternion = isinstance(other,Quaternion) - - ################ - if isquaternion: - """ Quaternion Multiplication """ - # Q = Qa * Qb - - na = self.n - nb = other.n - - ea = self.e - eb = other.e - - n = na * nb - ea * eb # * operator = scalar product for vector - e = (eb * na) + (ea * nb) + (ea ** eb) # ** operator = vectorial product for vector - - # Attention définition right handed, donc R2*R1 = Q1*Q2 - - """ - # 4x4 matrix to compute quaternion multiplication - qskew = np.matrix(np.zeros((4,4))) - - # Identitie matrix - I = np.matrix(np.eye(3,3)) - - qskew[0:3,0:3] = self.n * I - self.e.skew - qskew[3,0:3] = - self.e.col.T - qskew[0:3,3] = self.e.col - qskew[3,3] = self.n - """ - - return Quaternion(e,n) - - ################ - if isvector: - """ Rotate vector computing the rotation matrix """ - - new_vector = self.toRotationMatrix() * other - - return new_vector - - ############ - else: - """ Scale the rotation arround the same axis (other * angle arround the same axis) """ - - new_rotation = self.toAngleAxis() * other - - return AngleAxis2Quaternion(new_rotation) - - - ################################# - def __neg__(self): - """ Inverse Quaternion """ - - return Quaternion(-self.e,self.n) - - - ################################# - def toRotationMatrix(self): - """ Compute equivalent rotation matrix """ - - return Quaternion2RotationMatrix(self) - - - ################################# - def toAngleAxis(self): - """ Compute equivalent Angle-Axis """ - - return Quaternion2AngleAxis(self) - - - ################################# - def toRotationVector(self): - """ Compute equivalent Rotation Vector """ - - a = self.toAngleAxis() - - return a.toRotationVector() - - - -""" -############################################################################## -############# Functions ########################################### -############################################################################## -""" - -######################################## -def col2vec(col): - """ - Create a vector class from a numpy matrix - - col = - matrix([[1], - [2], - [1]]) - - """ - - x = col[0,0] - y = col[1,0] - z = col[2,0] - - return Vector(x,y,z) - - -######################################## -def list2vec(l): - """ - Create a vector class from a list - - l = [x,y,z] - - """ - - x = l[0] - y = l[1] - z = l[2] - - return Vector(x,y,z) - - -######################################## -def euler2RotationMatrix( teta_1 , teta_2 , teta_3 ): - """ Convert 3 euler angle to a 321 rotation matrix """ - - """ Convert degree to radian """ - r1 = np.deg2rad(teta_1) - r2 = np.deg2rad(teta_2) - r3 = np.deg2rad(teta_3) - - """ Compute cosinus """ - c1 = np.cos(r1) - c2 = np.cos(r2) - c3 = np.cos(r3) - - """ Compute sinus """ - s1 = np.sin(r1) - s2 = np.sin(r2) - s3 = np.sin(r3) - - """ Compute rotation matrix """ - R1 = np.matrix([[1,0,0],[0,c1,s1],[0,-s1,c1]]) - R2 = np.matrix([[c2,0,-s2],[0,1,0],[s2,0,c2]]) - R3 = np.matrix([[c3,s3,0],[-s3,c3,0],[0,0,1]]) - - """ Rotation 3-2-1 """ - C = R1 * R2 * R3 - - return RotationMatrix(C) - - -########################################### -def RotationMatrix2AngleAxis(RM): - """ Convert a rotation matrix to a angle axis class """ - - C = RM.C - - """ Compute Angle """ - cos = (C.trace()[0,0] - 1) * 0.5 - - rad = np.arccos(cos) - - sin = np.sin(rad) - - """ Compute axis of rotation """ - ############## - if ( abs(rad - np.pi) > epsilon ) and ( abs(rad + np.pi) > epsilon ): - - ax = ( C[1,2] - C[2,1] ) / (2 * sin) - ay = ( C[2,0] - C[0,2] ) / (2 * sin) - az = ( C[0,1] - C[1,0] ) / (2 * sin) - - ############## - else: - - print('\n RotationMatrix2AngleAxis : bad matrix : to do!!!') - - axis = Vector(ax,ay,az) - - return AngleAxis(rad,axis) - - -########################################### -def AngleAxis2Quaternion(AA): - """ Convert a angle axis class to a Quaternion """ - - """ Vector """ - e = AA.axis * np.sin( AA.rad * 0.5 ) - - """ Scalar """ - n = np.cos( AA.rad * 0.5 ) - - return Quaternion(e,n) - - -########################################### -def AngleAxis2RotationMatrix(AA): - """ Convert a angle axis class to a Rotation Matrix """ - - a = AA.axis - rad = AA.rad - - """ Eye matrix """ - I = np.matrix(np.eye(3,3)) - - cos = np.cos(rad) - sin = np.sin(rad) - - """ Compute Matrix """ - C = cos * I + (1 - cos) * a.col * a.col.T - sin * a.skew - - return RotationMatrix(C) - - -########################################### -def Quaternion2RotationMatrix(Q): - """ Convert a Quaternion to a Rotation Matrix """ - - n = Q.n - e = Q.e - - """ Eye matrix """ - I = np.matrix(np.eye(3,3)) - - C = I * ( n**2 - e.norm**2 ) + 2 * e.col * e.col.T - 2 * n * e.skew - - return RotationMatrix(C) - - -########################################### -def Quaternion2AngleAxis(Q): - """ Convert a Quaternion to a angle axis """ - - n = Q.n - e = Q.e - - """ Axis """ - axis = e * ( 1 / e.norm ) - - """ Angle """ - rad = 2 * np.arccos(n) - - return AngleAxis(rad,axis) - - -########################################### -def RotationMatrix2Quaternion(RM): - """ Convert a Quaternion to a Rotation Matrix """ - - C = RM.C - - trace = C.trace()[0,0] - - if trace < epsilon : - print(' Warning : trace of the matrix is near zero ') - - n = 0.5 * np.sqrt( trace + 1 ) - - ex = C[1,2] - C[2,1] - ey = C[2,0] - C[0,2] - ez = C[0,1] - C[1,0] - - e = Vector(ex,ey,ez) * ( 1 / ( 4 * n ) ) - - return Quaternion(e,n) - - - - -""" -############################################################################## -############# MAIN ########################################### -############################################################################## -""" - - -if __name__ == '__main__': - - V = Vector(10,10,10) - - C1 = euler2RotationMatrix(0,0,90) - C2 = euler2RotationMatrix(90,0,0) - - Q1 = C1.toQuaternion() - Q2 = C2.toQuaternion() - - C21 = C1 * C2 - - Q21 = Q2 * Q1 - - \ No newline at end of file diff --git a/dev/linedrone/linedrone.py b/dev/linedrone/linedrone.py deleted file mode 100644 index d8de9233..00000000 --- a/dev/linedrone/linedrone.py +++ /dev/null @@ -1,32 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Thu May 18 12:30:17 2023 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import drone -from pyro.planning import dynamicprogramming - -sys = drone.SpeedControlledDrone2D() - - -sys.obstacles = [ - np.array([ 0. , 20. ]) , - np.array([ 0. , 25. ]) , - np.array([ 0. , 30. ]) , - np.array([ 5. , 20. ]) , - np.array([ 5. , 25. ]) , - np.array([ 5. , 30. ]) , - np.array([ 10. , 20. ]) , - np.array([ 10. , 25. ]) , - np.array([ 10. , 30. ]) , - np.array([ 2.5 , 35 ]) , - np.array([ 7.5 , 35 ]) , - ] - -x_start = np.array([8.0,0]) -x_goal = np.array([0.0,0]) \ No newline at end of file diff --git a/dev/markov/markov.py b/dev/markov/markov.py deleted file mode 100644 index f9dd861a..00000000 --- a/dev/markov/markov.py +++ /dev/null @@ -1,291 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Jan 9 09:07:29 2022 - -@author: alex -""" - -import numpy as np - - -############################################################################### -class MarkovSystem: - """ - Mother class for discrete stochastic dynamical systems - ---------------------------------------------- - n : number of finite states - m : number of control action - p : number of outputs - k : time index - --------------------------------------- - x_k+1 = fk( x_k , u_k , k ) - y_k = hk( x_k , u_k , k ) - - optionnal: - u_k = policy( y_k , k ) : action autonomous decision - - - """ - ########################################################################### - # The two following functions needs to be implemented by child classes - ########################################################################### - - ############################ - def __init__(self, n = 1, m = 1): - """ - The __init__ method of the Mother class can be used to fill-in default - labels, units, bounds, and base values. - """ - - ############################# - # Parameters - ############################# - - p = n - - # Dimensions - self.n = n - self.m = m - self.p = p - - # Labels - self.name = 'ContinuousDynamicSystem' - self.state_label = [] - self.input_label = [] - self.output_label = [] - - # Default Label and units - for i in range(n): - self.state_label.append('State '+str(i)) - for i in range(m): - self.input_label.append('Action '+str(i)) - for i in range(p): - self.output_label.append('Output '+str(i)) - - # Default state and inputs values - self.xbar = 0 - self.ubar = 0 - - ################################ - # Transition Probabilities - ################################ - - self.T_jia = np.zeros((n,n,m)) - - for a in range(m): - - self.T_jia[:,:,a] = np.diag(np.ones((n))) - - ################################ - # Transition cost - ################################ - - self.a_jia = np.zeros((n,n,m)) - - ################################ - # Final cost - ################################ - - self.gN_i = np.zeros((n)) - - - ################################ - # Variables - ################################ - - # Initial value for simulations - self.x0 = np.zeros( n ) - self.x0[0] = 1 - - # Result of last simulation - self.traj = None - - # Cost function for evaluation - # TODO - - - ############################# - def fk( self , x , u , k = 0 ): - """ - compute the evolution of the probability distribution - - """ - - T_ji = self.T_jia[:,:,u] # transition matrix of given action - - x_k1 = np.dot( T_ji , x ) - - return x_k1 - - - ############################# - def check_probability_matrix( self ): - """ - check if transition prob sums to 1 - - """ - - print( self.T_jia.sum( axis = 0 ) ) - - return self.T_jia.sum( axis = 0 ) # should be all ones - - - ########################################################################### - # The following functions can be overloaded when necessary by child classes - ########################################################################### - - ############################# - def h( self , x , k = 0 ): - """ - Output fonction y = h(x,u,t) - - """ - - y = x # default output is state - - return y - - ############################# - def policy( self , y , k ): - """ - - """ - - # Default action - u = self.ubar - - return u - - - ############################# - def simulation_of_density_probability( self , N = 10 , plot = True ): - """ - N = number of steps - """ - - x_k = self.x0 - - for k in range(N): - - y_k = self.h( x_k , k ) - u_k = self.policy( y_k , k ) - x_k1 = self.fk( x_k, u_k , k ) - - x_k = x_k1 - - if plot: - print(x_k1) - - - return x_k1 # return probability distribution at k = N - - - ############################# - def get_valueiteration_algo(self): - - vi = ValueIterationForMarkovProcess(self.T_jia, self.a_jia, self.gN_i) - - return vi - - - -############################################################################### -class ValueIterationForMarkovProcess: - """ - - - - """ - - ############################ - def __init__(self, T_jia , a_jia, gN_i ): - - - self.alpha = 1.0 # discount factor - - - self.T = T_jia - self.a = a_jia - self.g = gN_i - - self.n = T_jia.shape[0] # Number of states - self.m = T_jia.shape[2] # Number of actions - - - # Initialise cost-to-go with final cost - self.J = self.g.copy() - - # Initialise policy map - self.c = np.zeros((self.n)) - - # Initialise Q-values - self.Q = np.zeros((self.n,self.m)) - - - self.print = True - - - ############################### - def compute_backward_step(self): - - # For all states - for i in range(self.n): - - # For all actions - for a in range(self.m): - - Q_j = self.a[:,i,a] + self.alpha * self.J # array of possible cost - - self.Q[i,a] = np.dot( self.T[:,i,a] , Q_j ) # expected value - - - self.J = self.Q.min(1) # Minimum over all possible actions - self.c = self.Q.argmin(1) # Action that minimise Q for all i - - - ############################### - def compute_n_backward_steps(self, n): - - for k in range(n): - - self.compute_backward_step() - - if self.print: - print('Backward step N-',k) - print('J = ',self.J) - print('c = ',self.c) - - - - - - - - - - - - -''' -################################################################# -################## Main ######## -################################################################# -''' - - -if __name__ == "__main__": - """ MAIN TEST """ - - # - m = MarkovSystem(5,3) - - - m.check_probability_matrix() - - vi = m.get_valueiteration_algo() - - vi.alpha = 0.9 - - vi.compute_n_backward_steps(100) - \ No newline at end of file diff --git a/dev/mpc/mpc.py b/dev/mpc/mpc.py deleted file mode 100644 index 04b156f4..00000000 --- a/dev/mpc/mpc.py +++ /dev/null @@ -1,121 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Nov 21 03:14:40 2021 - -@author: alex -""" - -import numpy as np - -from pyro.planning.trajectoryoptimisation import DirectCollocationTrajectoryOptimisation -from pyro.dynamic.statespace import linearize - - -from pyro.control import controller - -########################################################################## -#T -########################################################################### -class NonLinearMPC( controller.StaticController ) : - """ - - """ - ############################ - def __init__(self, sys, dt = 0.1 , n = 10 ): - """ """ - - # Dimensions - self.k = 1 - self.m = sys.m - self.p = sys.p - - super().__init__(self.k, self.m, self.p) - - # Label - self.name = 'MPC Controller' - - # Gains - self.goal = sys.xbar - - self.planner = DirectCollocationTrajectoryOptimisation( sys , dt , n ) - - self.planner.x_goal = self.goal - - - ############################# - def c( self , y , r , t = 0 ): - """ - Feedback static computation u = c(y,r,t) - - INPUTS - y : sensor vector p x 1 - r : reference vector k x 1 - t : time 1 x 1 - - OUPUTS - u : control inputs vector m x 1 - - """ - - self.planner.x_start = y # actual state - - self.planner.compute_optimal_trajectory() - - x,u = self.planner.decisionvariables2xu( self.planner.res.x ) - - #DEBUG - self.planner.show_solution() - print(self.planner.x_start) - - u_next = u[:,0] - - return u_next - - - -''' -################################################################# -################## Main ######## -################################################################# -''' - - -if __name__ == "__main__": - """ MAIN TEST """ - - from pyro.dynamic.massspringdamper import TwoMass - from pyro.analysis.costfunction import QuadraticCostFunction - - sys = TwoMass() - - #Full state feedback (default of class is x2 output only) - sys.C = np.diag([1,1,1,1]) - sys.p = 4 # dim of output vector - sys.output_label = sys.state_label - sys.output_units = sys.state_units - - # Cost function - cf = QuadraticCostFunction.from_sys( sys ) - cf.Q[0,0] = 1 - cf.Q[1,1] = 1 - cf.Q[2,2] = 1 - cf.Q[3,3] = 1 - cf.R[0,0] = 1 - sys.cost_function = cf - - sys.u_ub[0] = +2 - sys.u_lb[0] = 0 - - ctl = NonLinearMPC( sys , 0.1 , 30) - - ctl.planner.goal = np.array([0,0,0,0]) - ctl.planner.maxiter = 10 - - # New cl-dynamic - cl_sys = ctl + sys - - cl_sys.x0 = np.array([0.5,0.5,0,0]) - cl_sys.compute_trajectory(0.3,4,'euler') - cl_sys.plot_trajectory('xu') - cl_sys.animate_simulation() \ No newline at end of file diff --git a/dev/old_to_remove/trajectory_optimisation/Charles_Khazoom_tests/trajopt.py b/dev/old_to_remove/trajectory_optimisation/Charles_Khazoom_tests/trajopt.py deleted file mode 100644 index ee1dfd9f..00000000 --- a/dev/old_to_remove/trajectory_optimisation/Charles_Khazoom_tests/trajopt.py +++ /dev/null @@ -1,279 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Wed Jul 12 12:09:37 2017 - -@author: Charles.Khazoom@hotmail.com -""" - -import numpy as np -import numpy.matlib as mb -import matplotlib.pyplot as plt - -from scipy.optimize import minimize, NonlinearConstraint, rosen, rosen_der,HessianUpdateStrategy,BFGS#, interpolate -from scipy.interpolate import interp1d -from pyro.dynamic import manipulator -from pyro.planning import plan -from pyro.analysis import costfunction -from pyro.analysis import Trajectory -''' -################################################################################ -''' - - -#class TorqueSquaredCostFunction( costfunction.QuadraticCostFunction ): -# def __init__(self): -# costfunction.QuadraticCostFunction.__init__( self ) - -''' -################################################################################ -''' -# create system -sys = manipulator.OneLinkManipulator() - - -''' -Create optization problem -''' - -# set cost function -# minimize torque square is quadraic cost with Q and V set to 0 -#class TorqueSquaredCostFunction( costfunction.QuadraticCostFunction ): should implement this class -sys.cost_function.Q=np.zeros(sys.cost_function.Q.shape) -sys.cost_function.V=np.zeros(sys.cost_function.V.shape) -sys.cost_function.R=np.ones(sys.cost_function.R.shape) # - -global ngrid -ngrid = 30 # number of gridpoint - -######## set bounds ########## -ub_t0 = 0 # bounds on t0 -lb_t0 = 0 - -ub_tf = 2 # bounds on tf -lb_tf = 2 -# should implement an error message if tf is not >t0 and in t0<0 - -#ub_state = np.array([]) -#lb_state -sys.x_ub=[2*np.pi,None] -sys.x_lb=[-2*np.pi,None] - -sys.u_ub = [None] -sys.u_lb = [None] - -ub_x = sys.x_ub # bounds on x -lb_x = sys.x_lb - -ub_u = sys.u_ub # bounds on inputs u -lb_u = sys.u_lb - -ub_x0 = [np.pi,0] # bounds on inital state -lb_x0 = [np.pi,0] - - -ub_xf = [0,0] #sys.x_ub # bounds on final state -lb_xf = [0,0] - -''' -create initial guess -''' - -#x_guess = np.linspace(ub_x0, ub_xf, ngrid) -#u_guess = np.ones([ngrid,sys.m]) -#t_guess = np.linspace(ub_t0, ub_tf, ngrid) -#y_guess= x_guess -#dx_guess=np.zeros(x_guess.shape) -# -#for i in range(ngrid): -# dx_guess[i,] = sys.f(x_guess[i,],u_guess[i,]) # compute f -# -#guess_traj = Trajectory(x_guess, u_guess, t_guess, dx_guess, y_guess) - - - -######## set equality contraints (other than dynamics) ########## -# sys.f contains the dynamics ! - -''' -Convert to non-linear program (direct collocation) - -decision variables are ordered: - - x0[0],x0[1]...,x0[ngrid-1], x1[0],x1[1]...,x1[ngrid-1],...,...xn[0],xn[1]...,xn[ngrid-1] - u0[0],u0[1]...,u0[ngrid-1], u1[0],x1[1]...,u1[ngrid-1],...,...un[0],un[1]...,un[ngrid-1], - t0, tf -''' - -#convert bounds in discete form, for all x(.),u(.),t(.) - -def pack_bounds(lb_x,ub_x,lb_x0,ub_x0,lb_xf,ub_xf,lb_u,ub_u,lb_t0,ub_t0,lb_tf,ub_tf): - - bnds = np.array([]).reshape(0,2) # initialize bounds np.array to append to - - for i in range(sys.n): # convert bounds on x - bnd_arr_to_add = np.concatenate([mb.repmat(lb_x[i], ngrid, 1),mb.repmat(ub_x[i], ngrid, 1)],axis=1) - bnd_arr_to_add[0,:]=np.array([lb_x0[i],ub_x0[i]])#enforce bound on initial value - bnd_arr_to_add[ngrid-1,:]=np.array([lb_xf[i],ub_xf[i]])#enforce bound on final value - bnds = np.append(bnds,bnd_arr_to_add,axis=0) - - for i in range(sys.m): # convert bounds on u - bnd_arr_to_add = np.concatenate([mb.repmat(lb_u[i], ngrid, 1),mb.repmat(ub_u[i], ngrid, 1)],axis=1) - bnds = np.append(bnds,bnd_arr_to_add,axis=0) - - # append bounds on t0 and tF - bnds=np.append(bnds,np.array([lb_t0,lb_t0]).reshape(1,2),axis=0) - bnds=np.append(bnds,np.array([lb_tf,lb_tf]).reshape(1,2),axis=0) - - bnds = tuple(map(tuple, bnds))#convert bnds np.array to tuple for input in NLP solver - - return bnds - - -def traj_2_dec_var(traj): - - dec_vars = np.array([]).reshape(0,1) # initialize dec_vars array - - for i in range(sys.n): # append states x - arr_to_add = traj.x[:,i].reshape(ngrid,1) - dec_vars = np.append(dec_vars,arr_to_add,axis=0) - - for i in range(sys.m): # append inputs u - arr_to_add = traj.u[:,i].reshape(ngrid,1) - dec_vars = np.append(dec_vars,arr_to_add,axis=0) - - # append t0 and tF - dec_vars=np.append(dec_vars,np.array(traj.t[0]).reshape(1,1),axis=0) - dec_vars=np.append(dec_vars,traj.t[-1].reshape(1,1),axis=0) - dec_vars=dec_vars.reshape(ngrid*(sys.n+sys.m)+2,) - return dec_vars - - - - -def unpack_dec_var(decision_variables): - global ngrid - x = np.zeros([ngrid,sys.n]) - u = np.zeros([ngrid,sys.m]) - # unpack decision variables into trajectory - for i in range(0,sys.n): - x[:,i] = decision_variables[i*ngrid:(i+1)*ngrid].reshape(ngrid) - - for i in range(sys.n,sys.n+sys.m): - u[:,i-sys.n] = decision_variables[i*ngrid:(i+1)*ngrid].reshape(ngrid) - - t0 = decision_variables[-2] - tf = decision_variables[-1] - return x,u,t0,tf - -def compute_dx(x,u,sys): - dx = np.zeros(x.shape) - global ngrid - for i in range(ngrid): - dx[i,:]=sys.f(x[i,:],u[i,:]) - return dx - - -def dec_var_2_traj(decision_variables): - global ngrid - - x,u,t0,tf = unpack_dec_var(decision_variables) - - dx = compute_dx(x,u,sys) - - t = np.linspace(t0, tf, ngrid).reshape(ngrid,) - - y=x # find proper fct evaluation later from sys object - - traj = Trajectory(x, u, t, dx, y) - - return traj - -def dynamics_cstr(traj): - - t0 = traj.t[0] - - tf = traj.t[-1] - - h = (tf-t0)/(ngrid-1) #step between each grid - - dyn_constr = np.diff(traj.x,axis=0) - (traj.dx[0:-1]+traj.dx[1:])*h/2 # this must be = 0 - dyn_constr = dyn_constr.reshape((ngrid-1)*sys.n,) - return dyn_constr - -def compute_cost(decision_variables): - traj_opt = dec_var_2_traj(decision_variables) #get trajectorty - traj_opt = sys.cost_function.trajectory_evaluation(traj_opt)#compute cost fcn - cost_function = traj_opt.J[-1]# traj_opt.J is the cumulated cost from integral, we take only the last value - return cost_function - -def interp_traj(traj,ngrid): - - f_x = interp1d(traj.t, traj.x, kind='cubic',axis=0) - f_u = interp1d(traj.t, traj.u, kind='quadratic',axis=0) - - t_interp = np.linspace(traj.t[0],traj.t[-1],ngrid) - x_interp = f_x(t_interp) - u_interp = f_u(t_interp) - dx_interp=compute_dx(x_interp,u_interp,sys) - y_interp = x_interp - traj_interp = Trajectory(x_interp, u_interp, t_interp, dx_interp, y_interp) - return traj_interp - -A = np.load(r'solution_with_slsqp_onelink_200.npy') -ngrid=200 -loaded_traj = dec_var_2_traj(A) -ngrid=250 -bnds = pack_bounds(lb_x,ub_x0,lb_x,ub_x0,lb_xf,ub_xf,lb_u,ub_u,lb_t0,lb_t0,lb_tf,lb_tf) - -cons_slsqp = ({'type': 'eq', 'fun': lambda x: dynamics_cstr(dec_var_2_traj( x )) }) - -cons_trust=NonlinearConstraint(lambda x: dynamics_cstr(dec_var_2_traj( x )), 0, 0)# - - - - - - -guess_traj = interp_traj(loaded_traj,ngrid) -dec_var_guess = traj_2_dec_var(guess_traj) - - - -''' -Solve non-linear program -''' -#method='trust-constr' -res4 = minimize(compute_cost, dec_var_guess,method='SLSQP' , bounds=bnds, constraints=cons_slsqp,tol=1e-6,options={'disp': True,'maxiter':1000}) - - - -#dec_var_guess = np.load('C:\Users\Charles Khazoom\Documents\git\pyro\trajresults\solution_with_trust_const.npy') - -#res4 = minimize(compute_cost, dec_var_guess,method='trust-constr' , bounds=bnds, constraints=cons_trust,tol=1e-6,options={'disp': True,'maxiter':1000},jac='2-point',hess=BFGS()) -# - - - - -''' -Interpolate solution in trajectory object -''' - -result_traj = dec_var_2_traj(res4.x) - -interp_value = 1000 -result_traj_int = interp_traj(result_traj,interp_value) - -sys.traj = result_traj_int -sys.x0=sys.traj.x[0,:] -#sys.plot_trajectory('xu') -ctl = plan.OpenLoopController(result_traj_int) -# - -## New cl-dynamic -cl_sys = ctl + sys -# -cl_sys.compute_trajectory( sys.traj.t[-1] ,interp_value ) -cl_sys.plot_trajectory('xu') -#sys.cost_function.trajectory_evaluation(sys.traj) -#cl_sys.animate_simulation() \ No newline at end of file diff --git a/dev/old_to_remove/trajectory_optimisation/Charles_Khazoom_tests/trajopt_double_pendulum_based_on_rrt_guess.py b/dev/old_to_remove/trajectory_optimisation/Charles_Khazoom_tests/trajopt_double_pendulum_based_on_rrt_guess.py deleted file mode 100755 index 399f5be4..00000000 --- a/dev/old_to_remove/trajectory_optimisation/Charles_Khazoom_tests/trajopt_double_pendulum_based_on_rrt_guess.py +++ /dev/null @@ -1,304 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Wed Jul 12 12:09:37 2017 - -@author: Charles.Khazoom@hotmail.com -""" - -import numpy as np -import numpy.matlib as mb -import matplotlib.pyplot as plt - -from scipy.optimize import minimize, NonlinearConstraint, rosen, rosen_der,HessianUpdateStrategy,BFGS#, interpolate -from scipy.interpolate import interp1d -from pyro.dynamic import manipulator -from pyro.planning import plan -from pyro.analysis import costfunction -from pyro.analysis import Trajectory - -from pyro.control import nonlinear -from pyro.analysis import simulation - -''' -############################################################################## -''' - - -#class TorqueSquaredCostFunction( costfunction.QuadraticCostFunction ): -# def __init__(self): -# costfunction.QuadraticCostFunction.__init__( self ) - -''' -############################################################################## -''' -# create system -#sys = manipulator.OneLinkManipulator() - -from pyro.dynamic import pendulum - -sys = pendulum.DoublePendulum() - -''' -Create optization problem -''' - -# set cost function -# minimize torque square is quadraic cost with Q and V set to 0 -#class TorqueSquaredCostFunction( costfunction.QuadraticCostFunction ): should implement this class -#sys.cost_function.Q=np.zeros(sys.cost_function.Q.shape) -#sys.cost_function.V=np.zeros(sys.cost_function.V.shape) -sys.cost_function.Q[0,0] = 1 -sys.cost_function.Q[1,1] = 1 -sys.cost_function.R[0,0] = 1 -sys.cost_function.R[1,1] = 1 -sys.cost_function.xbar = np.array([0,0,0,0]) - -global ngrid -ngrid = 30 # number of gridpoint - -######## set bounds ########## -ub_t0 = 0 # bounds on t0 -lb_t0 = 0 - -ub_tf = 8 # bounds on tf -lb_tf = 7 -# should implement an error message if tf is not >t0 and in t0<0 - -#ub_state = np.array([]) -#lb_state -sys.x_ub=[+2*np.pi,+2*np.pi,0,0] -sys.x_lb=[-2*np.pi,-2*np.pi,0,0] - -sys.u_ub = [+50.0,+50.0] -sys.u_lb = [-50.0,-50.0] - -ub_x = sys.x_ub # bounds on x -lb_x = sys.x_lb - -ub_u = sys.u_ub # bounds on inputs u -lb_u = sys.u_lb - -ub_x0 = [-3.14,0,0,0] # bounds on inital state -lb_x0 = [-3.14,0,0,0] - - -ub_xf = [0,0,0,0] #sys.x_ub # bounds on final state -lb_xf = [0,0,0,0] - -''' -create initial guess -''' - -#x_guess = np.linspace(ub_x0, ub_xf, ngrid) -#u_guess = np.ones([ngrid,sys.m]) -#t_guess = np.linspace(ub_t0, ub_tf, ngrid) -#y_guess= x_guess -#dx_guess=np.zeros(x_guess.shape) -# -#for i in range(ngrid): -# dx_guess[i,] = sys.f(x_guess[i,],u_guess[i,]) # compute f -# -#guess_traj = Trajectory(x_guess, u_guess, t_guess, dx_guess, y_guess) - - - -######## set equality contraints (other than dynamics) ########## -# sys.f contains the dynamics ! - -''' -Convert to non-linear program (direct collocation) - -decision variables are ordered: - - x0[0],x0[1]...,x0[ngrid-1], x1[0],x1[1]...,x1[ngrid-1],...,...xn[0],xn[1]...,xn[ngrid-1] - u0[0],u0[1]...,u0[ngrid-1], u1[0],x1[1]...,u1[ngrid-1],...,...un[0],un[1]...,un[ngrid-1], - t0, tf -''' - -#convert bounds in discete form, for all x(.),u(.),t(.) - -def pack_bounds(lb_x,ub_x,lb_x0,ub_x0,lb_xf,ub_xf,lb_u,ub_u,lb_t0,ub_t0,lb_tf,ub_tf): - - bnds = np.array([]).reshape(0,2) # initialize bounds np.array to append to - - for i in range(sys.n): # convert bounds on x - bnd_arr_to_add = np.concatenate([mb.repmat(lb_x[i], ngrid, 1),mb.repmat(ub_x[i], ngrid, 1)],axis=1) - bnd_arr_to_add[0,:]=np.array([lb_x0[i],ub_x0[i]])#enforce bound on initial value - bnd_arr_to_add[ngrid-1,:]=np.array([lb_xf[i],ub_xf[i]])#enforce bound on final value - bnds = np.append(bnds,bnd_arr_to_add,axis=0) - - for i in range(sys.m): # convert bounds on u - bnd_arr_to_add = np.concatenate([mb.repmat(lb_u[i], ngrid, 1),mb.repmat(ub_u[i], ngrid, 1)],axis=1) - bnds = np.append(bnds,bnd_arr_to_add,axis=0) - - # append bounds on t0 and tF - bnds=np.append(bnds,np.array([lb_t0,lb_t0]).reshape(1,2),axis=0) - bnds=np.append(bnds,np.array([lb_tf,lb_tf]).reshape(1,2),axis=0) - - bnds = tuple(map(tuple, bnds))#convert bnds np.array to tuple for input in NLP solver - - return bnds - - -def traj_2_dec_var(traj): - - dec_vars = np.array([]).reshape(0,1) # initialize dec_vars array - - for i in range(sys.n): # append states x - arr_to_add = traj.x[:,i].reshape(ngrid,1) - dec_vars = np.append(dec_vars,arr_to_add,axis=0) - - for i in range(sys.m): # append inputs u - arr_to_add = traj.u[:,i].reshape(ngrid,1) - dec_vars = np.append(dec_vars,arr_to_add,axis=0) - - # append t0 and tF - dec_vars=np.append(dec_vars,np.array(traj.t[0]).reshape(1,1),axis=0) - dec_vars=np.append(dec_vars,traj.t[-1].reshape(1,1),axis=0) - dec_vars=dec_vars.reshape(ngrid*(sys.n+sys.m)+2,) - return dec_vars - - - - -def unpack_dec_var(decision_variables): - global ngrid - x = np.zeros([ngrid,sys.n]) - u = np.zeros([ngrid,sys.m]) - # unpack decision variables into trajectory - for i in range(0,sys.n): - x[:,i] = decision_variables[i*ngrid:(i+1)*ngrid].reshape(ngrid) - - for i in range(sys.n,sys.n+sys.m): - u[:,i-sys.n] = decision_variables[i*ngrid:(i+1)*ngrid].reshape(ngrid) - - t0 = decision_variables[-2] - tf = decision_variables[-1] - return x,u,t0,tf - -def compute_dx(x,u,sys): - dx = np.zeros(x.shape) - global ngrid - for i in range(ngrid): - dx[i,:]=sys.f(x[i,:],u[i,:]) - return dx - - -def dec_var_2_traj(decision_variables): - global ngrid - - x,u,t0,tf = unpack_dec_var(decision_variables) - - dx = compute_dx(x,u,sys) - - t = np.linspace(t0, tf, ngrid).reshape(ngrid,) - - y=x # find proper fct evaluation later from sys object - - traj = Trajectory(x, u, t, dx, y) - - return traj - -def dynamics_cstr(traj): - - t0 = traj.t[0] - - tf = traj.t[-1] - - h = (tf-t0)/(ngrid-1) #step between each grid - - dyn_constr = np.diff(traj.x,axis=0) - (traj.dx[0:-1]+traj.dx[1:])*h/2 # this must be = 0 - dyn_constr = dyn_constr.reshape((ngrid-1)*sys.n,) - return dyn_constr - -def compute_cost(decision_variables): - traj_opt = dec_var_2_traj(decision_variables) #get trajectorty - traj_opt = sys.cost_function.trajectory_evaluation(traj_opt)#compute cost fcn - cost = traj_opt.J[-1]# traj_opt.J is the cumulated cost from integral, we take only the last value - return cost - -def interp_traj(traj,ngrid): - - f_x = interp1d(traj.t, traj.x, kind='cubic',axis=0) - f_u = interp1d(traj.t, traj.u, kind='quadratic',axis=0) - - t_interp = np.linspace(traj.t[0],traj.t[-1],ngrid) - x_interp = f_x(t_interp) - u_interp = f_u(t_interp) - dx_interp=compute_dx(x_interp,u_interp,sys) - y_interp = x_interp - traj_interp = Trajectory(x_interp, u_interp, t_interp, dx_interp, y_interp) - return traj_interp - - - - - - -# Guess traj based from RRT solution -loaded_traj = simulation.Trajectory.load('double_pendulum_rrt.npy') -ngrid = loaded_traj.time_steps * 10 - - -# - -bnds = pack_bounds(lb_x,ub_x0,lb_x,ub_x0,lb_xf,ub_xf,lb_u,ub_u,lb_t0,lb_t0,lb_tf,lb_tf) - -cons_slsqp = ({'type': 'eq', 'fun': lambda x: dynamics_cstr(dec_var_2_traj( x )) }) - -cons_trust=NonlinearConstraint(lambda x: dynamics_cstr(dec_var_2_traj( x )), 0, 0)# - - -guess_traj = interp_traj( loaded_traj , ngrid ) -dec_var_guess = traj_2_dec_var( guess_traj ) - - - -''' -Solve non-linear program -''' -#method='trust-constr' -res4 = minimize(compute_cost, dec_var_guess,method='SLSQP' , bounds=bnds, constraints=cons_slsqp,tol=1e-6,options={'disp': True,'maxiter':1000}) - - - -#dec_var_guess = np.load('C:\Users\Charles Khazoom\Documents\git\pyro\trajresults\solution_with_trust_const.npy') - -#res4 = minimize(compute_cost, dec_var_guess,method='trust-constr' , bounds=bnds, constraints=cons_trust,tol=1e-6,options={'disp': True,'maxiter':1000},jac='2-point',hess=BFGS()) -# - - - -''' -Analyze results -''' - - -result_traj = dec_var_2_traj(res4.x) - -result_traj.save('double_pendulum_rrt_optimised.npy') - - -#show solution vs initial guess -sys.traj = loaded_traj -sys.plot_trajectory('xu') -sys.traj = result_traj -sys.plot_trajectory('xu') -#sys.animate_simulation() - -# CLosed-loop controller -ctl = nonlinear.ComputedTorqueController( sys , result_traj ) -ctl.rbar = np.array([0,0]) - -## New cl-dynamic -cl_sys = ctl + sys - -cl_sys.x0 = np.array([-3.14,0,0,0]) -cl_sys.compute_trajectory( 10 ) -cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation() - - -print(sys.cost_function.trajectory_evaluation( guess_traj ).J[-1]) -print(sys.cost_function.trajectory_evaluation( result_traj ).J[-1]) -print(sys.cost_function.trajectory_evaluation( cl_sys.traj ).J[-1]) \ No newline at end of file diff --git a/dev/old_to_remove/trajectory_optimisation/Charles_Khazoom_tests/trajopt_single_pendulum_based_on_rrt_guess.py b/dev/old_to_remove/trajectory_optimisation/Charles_Khazoom_tests/trajopt_single_pendulum_based_on_rrt_guess.py deleted file mode 100755 index 0790842f..00000000 --- a/dev/old_to_remove/trajectory_optimisation/Charles_Khazoom_tests/trajopt_single_pendulum_based_on_rrt_guess.py +++ /dev/null @@ -1,302 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Wed Jul 12 12:09:37 2017 - -@author: Charles.Khazoom@hotmail.com -""" - -import numpy as np -import numpy.matlib as mb -import matplotlib.pyplot as plt - -from scipy.optimize import minimize, NonlinearConstraint, rosen, rosen_der,HessianUpdateStrategy,BFGS#, interpolate -from scipy.interpolate import interp1d -from pyro.dynamic import manipulator -from pyro.planning import plan -from pyro.analysis import costfunction -from pyro.analysis import Trajectory - -from pyro.control import nonlinear -from pyro.analysis import simulation - -''' -################################################################################ -''' - - -#class TorqueSquaredCostFunction( costfunction.QuadraticCostFunction ): -# def __init__(self): -# costfunction.QuadraticCostFunction.__init__( self ) - -''' -################################################################################ -''' -# create system -#sys = manipulator.OneLinkManipulator() - -from pyro.dynamic import pendulum - -sys = pendulum.SinglePendulum() - -''' -Create optization problem -''' - -# set cost function -# minimize torque square is quadraic cost with Q and V set to 0 -#class TorqueSquaredCostFunction( costfunction.QuadraticCostFunction ): should implement this class -#sys.cost_function.Q=np.zeros(sys.cost_function.Q.shape) -#sys.cost_function.V=np.zeros(sys.cost_function.V.shape) -sys.cost_function.Q[0,0] = 0 -sys.cost_function.Q[1,1] = 0 -sys.cost_function.R[0,0] = 10 -sys.cost_function.xbar = np.array([-3.14,0]) - -global ngrid -ngrid = 30 # number of gridpoint - -######## set bounds ########## -ub_t0 = 0 # bounds on t0 -lb_t0 = 0 - -ub_tf = 7 # bounds on tf -lb_tf = 6 -# should implement an error message if tf is not >t0 and in t0<0 - -#ub_state = np.array([]) -#lb_state -sys.x_ub=[2*np.pi,None] -sys.x_lb=[-2*np.pi,None] - -sys.u_ub = [20.0] -sys.u_lb = [-20.0] - -ub_x = sys.x_ub # bounds on x -lb_x = sys.x_lb - -ub_u = sys.u_ub # bounds on inputs u -lb_u = sys.u_lb - -ub_x0 = [0.0,0] # bounds on inital state -lb_x0 = [0.0,0] - - -ub_xf = [-3.14,0] #sys.x_ub # bounds on final state -lb_xf = [-3.14,0] - -''' -create initial guess -''' - -#x_guess = np.linspace(ub_x0, ub_xf, ngrid) -#u_guess = np.ones([ngrid,sys.m]) -#t_guess = np.linspace(ub_t0, ub_tf, ngrid) -#y_guess= x_guess -#dx_guess=np.zeros(x_guess.shape) -# -#for i in range(ngrid): -# dx_guess[i,] = sys.f(x_guess[i,],u_guess[i,]) # compute f -# -#guess_traj = Trajectory(x_guess, u_guess, t_guess, dx_guess, y_guess) - - - -######## set equality contraints (other than dynamics) ########## -# sys.f contains the dynamics ! - -''' -Convert to non-linear program (direct collocation) - -decision variables are ordered: - - x0[0],x0[1]...,x0[ngrid-1], x1[0],x1[1]...,x1[ngrid-1],...,...xn[0],xn[1]...,xn[ngrid-1] - u0[0],u0[1]...,u0[ngrid-1], u1[0],x1[1]...,u1[ngrid-1],...,...un[0],un[1]...,un[ngrid-1], - t0, tf -''' - -#convert bounds in discete form, for all x(.),u(.),t(.) - -def pack_bounds(lb_x,ub_x,lb_x0,ub_x0,lb_xf,ub_xf,lb_u,ub_u,lb_t0,ub_t0,lb_tf,ub_tf): - - bnds = np.array([]).reshape(0,2) # initialize bounds np.array to append to - - for i in range(sys.n): # convert bounds on x - bnd_arr_to_add = np.concatenate([mb.repmat(lb_x[i], ngrid, 1),mb.repmat(ub_x[i], ngrid, 1)],axis=1) - bnd_arr_to_add[0,:]=np.array([lb_x0[i],ub_x0[i]])#enforce bound on initial value - bnd_arr_to_add[ngrid-1,:]=np.array([lb_xf[i],ub_xf[i]])#enforce bound on final value - bnds = np.append(bnds,bnd_arr_to_add,axis=0) - - for i in range(sys.m): # convert bounds on u - bnd_arr_to_add = np.concatenate([mb.repmat(lb_u[i], ngrid, 1),mb.repmat(ub_u[i], ngrid, 1)],axis=1) - bnds = np.append(bnds,bnd_arr_to_add,axis=0) - - # append bounds on t0 and tF - bnds=np.append(bnds,np.array([lb_t0,lb_t0]).reshape(1,2),axis=0) - bnds=np.append(bnds,np.array([lb_tf,lb_tf]).reshape(1,2),axis=0) - - bnds = tuple(map(tuple, bnds))#convert bnds np.array to tuple for input in NLP solver - - return bnds - - -def traj_2_dec_var(traj): - - dec_vars = np.array([]).reshape(0,1) # initialize dec_vars array - - for i in range(sys.n): # append states x - arr_to_add = traj.x[:,i].reshape(ngrid,1) - dec_vars = np.append(dec_vars,arr_to_add,axis=0) - - for i in range(sys.m): # append inputs u - arr_to_add = traj.u[:,i].reshape(ngrid,1) - dec_vars = np.append(dec_vars,arr_to_add,axis=0) - - # append t0 and tF - dec_vars=np.append(dec_vars,np.array(traj.t[0]).reshape(1,1),axis=0) - dec_vars=np.append(dec_vars,traj.t[-1].reshape(1,1),axis=0) - dec_vars=dec_vars.reshape(ngrid*(sys.n+sys.m)+2,) - return dec_vars - - - - -def unpack_dec_var(decision_variables): - global ngrid - x = np.zeros([ngrid,sys.n]) - u = np.zeros([ngrid,sys.m]) - # unpack decision variables into trajectory - for i in range(0,sys.n): - x[:,i] = decision_variables[i*ngrid:(i+1)*ngrid].reshape(ngrid) - - for i in range(sys.n,sys.n+sys.m): - u[:,i-sys.n] = decision_variables[i*ngrid:(i+1)*ngrid].reshape(ngrid) - - t0 = decision_variables[-2] - tf = decision_variables[-1] - return x,u,t0,tf - -def compute_dx(x,u,sys): - dx = np.zeros(x.shape) - global ngrid - for i in range(ngrid): - dx[i,:]=sys.f(x[i,:],u[i,:]) - return dx - - -def dec_var_2_traj(decision_variables): - global ngrid - - x,u,t0,tf = unpack_dec_var(decision_variables) - - dx = compute_dx(x,u,sys) - - t = np.linspace(t0, tf, ngrid).reshape(ngrid,) - - y=x # find proper fct evaluation later from sys object - - traj = Trajectory(x, u, t, dx, y) - - return traj - -def dynamics_cstr(traj): - - t0 = traj.t[0] - - tf = traj.t[-1] - - h = (tf-t0)/(ngrid-1) #step between each grid - - dyn_constr = np.diff(traj.x,axis=0) - (traj.dx[0:-1]+traj.dx[1:])*h/2 # this must be = 0 - dyn_constr = dyn_constr.reshape((ngrid-1)*sys.n,) - return dyn_constr - -def compute_cost(decision_variables): - traj_opt = dec_var_2_traj(decision_variables) #get trajectorty - traj_opt = sys.cost_function.trajectory_evaluation(traj_opt)#compute cost fcn - cost = traj_opt.J[-1]# traj_opt.J is the cumulated cost from integral, we take only the last value - return cost - -def interp_traj(traj,ngrid): - - f_x = interp1d(traj.t, traj.x, kind='cubic',axis=0) - f_u = interp1d(traj.t, traj.u, kind='quadratic',axis=0) - - t_interp = np.linspace(traj.t[0],traj.t[-1],ngrid) - x_interp = f_x(t_interp) - u_interp = f_u(t_interp) - dx_interp=compute_dx(x_interp,u_interp,sys) - y_interp = x_interp - traj_interp = Trajectory(x_interp, u_interp, t_interp, dx_interp, y_interp) - return traj_interp - - - - - - -# Guess traj based from RRT solution -loaded_traj = simulation.Trajectory.load('pendulum_rrt.npy') -ngrid = loaded_traj.time_steps - - -# - -bnds = pack_bounds(lb_x,ub_x0,lb_x,ub_x0,lb_xf,ub_xf,lb_u,ub_u,lb_t0,lb_t0,lb_tf,lb_tf) - -cons_slsqp = ({'type': 'eq', 'fun': lambda x: dynamics_cstr(dec_var_2_traj( x )) }) - -cons_trust=NonlinearConstraint(lambda x: dynamics_cstr(dec_var_2_traj( x )), 0, 0)# - - -guess_traj = interp_traj( loaded_traj , ngrid ) -dec_var_guess = traj_2_dec_var( guess_traj ) - - - -''' -Solve non-linear program -''' -#method='trust-constr' -res4 = minimize(compute_cost, dec_var_guess,method='SLSQP' , bounds=bnds, constraints=cons_slsqp,tol=1e-6,options={'disp': True,'maxiter':1000}) - - - -#dec_var_guess = np.load('C:\Users\Charles Khazoom\Documents\git\pyro\trajresults\solution_with_trust_const.npy') - -#res4 = minimize(compute_cost, dec_var_guess,method='trust-constr' , bounds=bnds, constraints=cons_trust,tol=1e-6,options={'disp': True,'maxiter':1000},jac='2-point',hess=BFGS()) -# - - - -''' -Analyze results -''' - - -result_traj = dec_var_2_traj(res4.x) - -result_traj.save('pendulum_rrt_optimised.npy') - - -#show solution vs initial guess -sys.traj = loaded_traj -sys.plot_trajectory('xu') -sys.traj = result_traj -sys.plot_trajectory('xu') -#sys.animate_simulation() - -# CLosed-loop controller -ctl = nonlinear.ComputedTorqueController( sys , result_traj ) -ctl.rbar = np.array([-3.14]) - -## New cl-dynamic -cl_sys = ctl + sys - -cl_sys.compute_trajectory( 10 ) -cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation() - - -print(sys.cost_function.trajectory_evaluation( guess_traj ).J[-1]) -print(sys.cost_function.trajectory_evaluation( result_traj ).J[-1]) -print(sys.cost_function.trajectory_evaluation( cl_sys.traj ).J[-1]) \ No newline at end of file diff --git a/dev/old_to_remove/trajectory_optimisation/direct_collocation/block_minimal_force.py b/dev/old_to_remove/trajectory_optimisation/direct_collocation/block_minimal_force.py deleted file mode 100644 index 2e7acab2..00000000 --- a/dev/old_to_remove/trajectory_optimisation/direct_collocation/block_minimal_force.py +++ /dev/null @@ -1,128 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Sep 10 11:12:11 2021 - -@author: alexandregirard -""" - -import numpy as np -from scipy.optimize import minimize - -n = 2 -m = 1 -grid = 100 -dt = 0.01 - -mass = 1 -target = 1 - -#dec = np.linspace(0,grid*3,grid*3) - -count = 0 - - -def dec2xu(dec): - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - u[0,:] = dec[2*grid:3*grid] - - return x,u - -def cost(dec): - - J = 0 - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - u[0,:] = dec[2*grid:3*grid] - - for i in range(grid-1): - - #J = J + 0.5 * dt * ( (x[0,i]-target)**2 + (x[0,i+1]-target)**2 + x[1,i]**2 + x[1,i+1]**2 + u[0,i]**2 + u[0,i+1]**2 ) - - J = J + 0.5 * dt * ( u[0,i]**2 + u[0,i+1]**2 ) - - return J - -def constraints(dec): - - x,u = dec2xu(dec) - - vec=np.zeros(grid*2-2) - - for i in range(grid-1): - - vec[i] = (x[0,i+1] - x[0,i]) - 0.5 * dt * ( x[1,i+1] + x[1,i] ) - vec[i+grid-1] = (x[1,i+1] - x[1,i]) - 0.5 * dt * ( u[0,i+1]/mass + u[0,i]/mass ) - - return vec - - -def compute_bounds(): - bounds = [] - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (0,10) ) - - bounds.append( (0.99,1) ) - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-100,100) ) - - bounds.append( (0,0.01) ) - - for i in range(grid): - - bounds.append( (-100,100) ) - - return bounds - -def display_callback(a): - - - print('One iteration completed') - - return True - - -# Guess -dec = np.zeros(grid*(n+m)) -dec[0:grid] = np.linspace(0,1,grid) -dec[grid:2*grid] = 1 -dec[2*grid:3*grid] = 1 - -bnds = compute_bounds() - -cons = {'type': 'eq', 'fun': constraints } -res = minimize( cost, dec, method='SLSQP', bounds=bnds, constraints=cons, callback=display_callback ) # - -print(res) - -from pyro.dynamic import integrator - -sys = integrator.DoubleIntegrator() - -sys.compute_trajectory(1,100) - - -dec = res.x - -sys.traj.x[:,0] = dec[0:grid] -sys.traj.x[:,1] = dec[grid:2*grid] -sys.traj.u[:,0] = dec[2*grid:3*grid] - -sys.plot_trajectory('xu') diff --git a/dev/old_to_remove/trajectory_optimisation/direct_collocation/double_pendulum_2sec_swing_up.py b/dev/old_to_remove/trajectory_optimisation/direct_collocation/double_pendulum_2sec_swing_up.py deleted file mode 100644 index e905c084..00000000 --- a/dev/old_to_remove/trajectory_optimisation/direct_collocation/double_pendulum_2sec_swing_up.py +++ /dev/null @@ -1,174 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Sep 10 11:12:11 2021 - -@author: alexandregirard -""" - -import numpy as np -from scipy.optimize import minimize - -from pyro.dynamic import pendulum - - - -sys = pendulum.DoublePendulum() - -n = 4 -m = 2 -grid = 20 -dt = 0.1 - - -#dec = np.linspace(0,grid*3,grid*3) - - -def dec2xu(dec): - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - x[2,:] = dec[2*grid:3*grid] - x[3,:] = dec[3*grid:4*grid] - u[0,:] = dec[4*grid:5*grid] - u[1,:] = dec[5*grid:6*grid] - - return x,u - -def cost(dec): - - J = 0 - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - x[2,:] = dec[2*grid:3*grid] - x[3,:] = dec[3*grid:4*grid] - u[0,:] = dec[4*grid:5*grid] - u[1,:] = dec[5*grid:6*grid] - - for i in range(grid-1): - - #J = J + 0.5 * dt * ( (x[0,i]-target)**2 + (x[0,i+1]-target)**2 + x[1,i]**2 + x[1,i+1]**2 + u[0,i]**2 + u[0,i+1]**2 ) - - J = J + 0.5 * dt * ( x[0,i]**2 + x[0,i+1]**2 + x[1,i]**2 + x[1,i+1]**2 ) - J = J + 0.5 * dt * ( x[2,i]**2 + x[2,i+1]**2 + x[3,i]**2 + x[3,i+1]**2 ) - J = J + 0.5 * dt * ( u[0,i]**2 + u[0,i+1]**2 + u[1,i]**2 + u[1,i+1]**2 ) * 5 - - return J - -def constraints(dec): - - x,u = dec2xu(dec) - - vec=np.zeros(grid*4-4) - - for i in range(grid-1): - - vec[i+grid*0-0] = (x[0,i+1] - x[0,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[0] + sys.f(x[:,i+1],u[:,i+1])[0] ) - vec[i+grid*1-1] = (x[1,i+1] - x[1,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[1] + sys.f(x[:,i+1],u[:,i+1])[1] ) - vec[i+grid*2-2] = (x[2,i+1] - x[2,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[2] + sys.f(x[:,i+1],u[:,i+1])[2] ) - vec[i+grid*3-3] = (x[3,i+1] - x[3,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[3] + sys.f(x[:,i+1],u[:,i+1])[3] ) - - return vec - - -def compute_bounds(): - - bounds = [] - - #x0 - bounds.append( (-3.14,-3.13) ) - - for i in range(1,grid-1): - - bounds.append( (-4,1) ) - - bounds.append( (0.0,0.01) ) - - #x1 - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-3,1) ) - - bounds.append( (0,0.01) ) - - #x2 - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-2,6) ) - - bounds.append( (0,0.01) ) - - #x3 - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-10,10) ) - - bounds.append( (0,0.01) ) - - #u0 - - for i in range(grid): - - bounds.append( (-20,30) ) - - #u1 - - for i in range(grid): - - bounds.append( (-20,30) ) - - return bounds - - -def display_callback(a): - - - print('One iteration completed') - - return True - - - - -# Guess -dec = np.zeros(grid*(n+m)) -#dec[0:grid] = traj.x[:,0] -#dec[grid:2*grid] = traj.x[:,1] -#dec[2*grid:3*grid] = traj.u[:,0] - -bnds = compute_bounds() - -cons = {'type': 'eq', 'fun': constraints } -res = minimize( cost, dec, method='SLSQP', bounds=bnds, constraints=cons, callback=display_callback, options={'disp':True,'maxiter':1000}) # - - -sys.compute_trajectory(grid*dt,grid) - - -dec = res.x - -sys.traj.x[:,0] = dec[0:grid] -sys.traj.x[:,1] = dec[grid:2*grid] -sys.traj.x[:,2] = dec[2*grid:3*grid] -sys.traj.x[:,3] = dec[3*grid:4*grid] -sys.traj.u[:,0] = dec[4*grid:5*grid] -sys.traj.u[:,1] = dec[5*grid:6*grid] - - -sys.plot_trajectory('xu') -sys.animate_simulation() \ No newline at end of file diff --git a/dev/old_to_remove/trajectory_optimisation/direct_collocation/double_pendulum_4sec_swing_up.py b/dev/old_to_remove/trajectory_optimisation/direct_collocation/double_pendulum_4sec_swing_up.py deleted file mode 100644 index 369c56ae..00000000 --- a/dev/old_to_remove/trajectory_optimisation/direct_collocation/double_pendulum_4sec_swing_up.py +++ /dev/null @@ -1,174 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Sep 10 11:12:11 2021 - -@author: alexandregirard -""" - -import numpy as np -from scipy.optimize import minimize - -from pyro.dynamic import pendulum - - - -sys = pendulum.DoublePendulum() - -n = 4 -m = 2 -grid = 20 -dt = 0.2 - - -#dec = np.linspace(0,grid*3,grid*3) - - -def dec2xu(dec): - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - x[2,:] = dec[2*grid:3*grid] - x[3,:] = dec[3*grid:4*grid] - u[0,:] = dec[4*grid:5*grid] - u[1,:] = dec[5*grid:6*grid] - - return x,u - -def cost(dec): - - J = 0 - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - x[2,:] = dec[2*grid:3*grid] - x[3,:] = dec[3*grid:4*grid] - u[0,:] = dec[4*grid:5*grid] - u[1,:] = dec[5*grid:6*grid] - - for i in range(grid-1): - - #J = J + 0.5 * dt * ( (x[0,i]-target)**2 + (x[0,i+1]-target)**2 + x[1,i]**2 + x[1,i+1]**2 + u[0,i]**2 + u[0,i+1]**2 ) - - J = J + 0.5 * dt * ( x[0,i]**2 + x[0,i+1]**2 + x[1,i]**2 + x[1,i+1]**2 ) - J = J + 0.5 * dt * ( x[2,i]**2 + x[2,i+1]**2 + x[3,i]**2 + x[3,i+1]**2 ) - J = J + 0.5 * dt * ( u[0,i]**2 + u[0,i+1]**2 + u[1,i]**2 + u[1,i+1]**2 ) * 5 - - return J - -def constraints(dec): - - x,u = dec2xu(dec) - - vec=np.zeros(grid*4-4) - - for i in range(grid-1): - - vec[i+grid*0-0] = (x[0,i+1] - x[0,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[0] + sys.f(x[:,i+1],u[:,i+1])[0] ) - vec[i+grid*1-1] = (x[1,i+1] - x[1,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[1] + sys.f(x[:,i+1],u[:,i+1])[1] ) - vec[i+grid*2-2] = (x[2,i+1] - x[2,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[2] + sys.f(x[:,i+1],u[:,i+1])[2] ) - vec[i+grid*3-3] = (x[3,i+1] - x[3,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[3] + sys.f(x[:,i+1],u[:,i+1])[3] ) - - return vec - - -def compute_bounds(): - - bounds = [] - - #x0 - bounds.append( (-3.14,-3.13) ) - - for i in range(1,grid-1): - - bounds.append( (-5,2) ) - - bounds.append( (0.0,0.01) ) - - #x1 - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-3,3) ) - - bounds.append( (0,0.01) ) - - #x2 - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-3,6) ) - - bounds.append( (0,0.01) ) - - #x3 - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-10,10) ) - - bounds.append( (0,0.01) ) - - #u0 - - for i in range(grid): - - bounds.append( (-20,30) ) - - #u1 - - for i in range(grid): - - bounds.append( (-20,30) ) - - return bounds - - -def display_callback(a): - - - print('One iteration completed') - - return True - - - - -# Guess -dec = np.zeros(grid*(n+m)) -#dec[0:grid] = traj.x[:,0] -#dec[grid:2*grid] = traj.x[:,1] -#dec[2*grid:3*grid] = traj.u[:,0] - -bnds = compute_bounds() - -cons = {'type': 'eq', 'fun': constraints } -res = minimize( cost, dec, method='SLSQP', bounds=bnds, constraints=cons, callback=display_callback, options={'disp':True,'maxiter':1000}) # - - -sys.compute_trajectory(grid*dt,grid) - - -dec = res.x - -sys.traj.x[:,0] = dec[0:grid] -sys.traj.x[:,1] = dec[grid:2*grid] -sys.traj.x[:,2] = dec[2*grid:3*grid] -sys.traj.x[:,3] = dec[3*grid:4*grid] -sys.traj.u[:,0] = dec[4*grid:5*grid] -sys.traj.u[:,1] = dec[5*grid:6*grid] - - -sys.plot_trajectory('xu') -sys.animate_simulation() \ No newline at end of file diff --git a/dev/old_to_remove/trajectory_optimisation/direct_collocation/double_pendulum_8sec_swing_up.py b/dev/old_to_remove/trajectory_optimisation/direct_collocation/double_pendulum_8sec_swing_up.py deleted file mode 100644 index e5604ae0..00000000 --- a/dev/old_to_remove/trajectory_optimisation/direct_collocation/double_pendulum_8sec_swing_up.py +++ /dev/null @@ -1,174 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Sep 10 11:12:11 2021 - -@author: alexandregirard -""" - -import numpy as np -from scipy.optimize import minimize - -from pyro.dynamic import pendulum - - - -sys = pendulum.DoublePendulum() - -n = 4 -m = 2 -grid = 40 -dt = 0.2 - - -#dec = np.linspace(0,grid*3,grid*3) - - -def dec2xu(dec): - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - x[2,:] = dec[2*grid:3*grid] - x[3,:] = dec[3*grid:4*grid] - u[0,:] = dec[4*grid:5*grid] - u[1,:] = dec[5*grid:6*grid] - - return x,u - -def cost(dec): - - J = 0 - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - x[2,:] = dec[2*grid:3*grid] - x[3,:] = dec[3*grid:4*grid] - u[0,:] = dec[4*grid:5*grid] - u[1,:] = dec[5*grid:6*grid] - - for i in range(grid-1): - - #J = J + 0.5 * dt * ( (x[0,i]-target)**2 + (x[0,i+1]-target)**2 + x[1,i]**2 + x[1,i+1]**2 + u[0,i]**2 + u[0,i+1]**2 ) - - J = J + 0.5 * dt * ( x[0,i]**2 + x[0,i+1]**2 + x[1,i]**2 + x[1,i+1]**2 ) - J = J + 0.5 * dt * ( x[2,i]**2 + x[2,i+1]**2 + x[3,i]**2 + x[3,i+1]**2 ) - J = J + 0.5 * dt * ( u[0,i]**2 + u[0,i+1]**2 + u[1,i]**2 + u[1,i+1]**2 ) * 5 - - return J - -def constraints(dec): - - x,u = dec2xu(dec) - - vec=np.zeros(grid*4-4) - - for i in range(grid-1): - - vec[i+grid*0-0] = (x[0,i+1] - x[0,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[0] + sys.f(x[:,i+1],u[:,i+1])[0] ) - vec[i+grid*1-1] = (x[1,i+1] - x[1,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[1] + sys.f(x[:,i+1],u[:,i+1])[1] ) - vec[i+grid*2-2] = (x[2,i+1] - x[2,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[2] + sys.f(x[:,i+1],u[:,i+1])[2] ) - vec[i+grid*3-3] = (x[3,i+1] - x[3,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[3] + sys.f(x[:,i+1],u[:,i+1])[3] ) - - return vec - - -def compute_bounds(): - - bounds = [] - - #x0 - bounds.append( (-3.14,-3.13) ) - - for i in range(1,grid-1): - - bounds.append( (-5,0.5) ) - - bounds.append( (0.0,0.01) ) - - #x1 - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-4,4) ) - - bounds.append( (0,0.01) ) - - #x2 - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-10,10) ) - - bounds.append( (0,0.01) ) - - #x3 - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-10,10) ) - - bounds.append( (0,0.01) ) - - #u0 - - for i in range(grid): - - bounds.append( (-100,100) ) - - #u1 - - for i in range(grid): - - bounds.append( (-100,100) ) - - return bounds - - -def display_callback(a): - - - print('One iteration completed') - - return True - - - - -# Guess -dec = np.zeros(grid*(n+m)) -#dec[0:grid] = traj.x[:,0] -#dec[grid:2*grid] = traj.x[:,1] -#dec[2*grid:3*grid] = traj.u[:,0] - -bnds = compute_bounds() - -cons = {'type': 'eq', 'fun': constraints } -res = minimize( cost, dec, method='SLSQP', bounds=bnds, constraints=cons, callback=display_callback, options={'disp':True,'maxiter':1000}) # - - -sys.compute_trajectory(grid*dt,grid) - - -dec = res.x - -sys.traj.x[:,0] = dec[0:grid] -sys.traj.x[:,1] = dec[grid:2*grid] -sys.traj.x[:,2] = dec[2*grid:3*grid] -sys.traj.x[:,3] = dec[3*grid:4*grid] -sys.traj.u[:,0] = dec[4*grid:5*grid] -sys.traj.u[:,1] = dec[5*grid:6*grid] - -#sys.traj.save('doublependulumswingup8sec') -sys.plot_trajectory('xu') -sys.animate_simulation() \ No newline at end of file diff --git a/dev/old_to_remove/trajectory_optimisation/direct_collocation/double_stage.py b/dev/old_to_remove/trajectory_optimisation/direct_collocation/double_stage.py deleted file mode 100644 index 79459f36..00000000 --- a/dev/old_to_remove/trajectory_optimisation/direct_collocation/double_stage.py +++ /dev/null @@ -1,69 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Fri Nov 16 12:05:08 2018 - -@author: Alexandre -""" -############################################################################### -import numpy as np -############################################################################### -from pyro.dynamic import pendulum -from pyro.control import nonlinear -from pyro.planning import trajectoryoptimisation -from pyro.analysis import simulation -############################################################################### - - -sys = pendulum.DoublePendulum() - -#Max/Min torque -sys.u_ub[0] = +20 -sys.u_ub[1] = +20 -sys.u_lb[0] = -20 -sys.u_lb[1] = -20 - - -tf = 4.0 - -# Coarce traj optimization -n = 20 -dt = tf/n - -planner = trajectoryoptimisation.DirectCollocationTrajectoryOptimisation( sys , dt , n ) - -planner.x_start = np.array([-3.14,0,0,0]) -planner.x_goal = np.array([0,0,0,0]) - -planner.maxiter = 500 -planner.compute_optimal_trajectory() -planner.show_solution() - -# Fine traj optimization -n = 100 -dt = tf/n -planner2 = trajectoryoptimisation.DirectCollocationTrajectoryOptimisation( sys , dt , n ) - -planner2.x_start = np.array([-3.14,0,0,0]) -planner2.x_goal = np.array([0,0,0,0]) - -planner2.set_initial_trajectory_guest( planner.traj ) -planner2.maxiter = 500 -planner2.compute_optimal_trajectory() -planner2.show_solution() - -planner2.save_solution( 'double_pendulum_directcollocation_hires.npy' ) - -# Controller -ctl = nonlinear.ComputedTorqueController( sys , planner2.traj ) - -ctl.rbar = np.array([0,0]) -ctl.w0 = 5 -ctl.zeta = 1 - -# New cl-dynamic -cl_sys = ctl + sys - -# Simultation -cl_sys.x0 = np.array([-3.14,0,0,0]) -cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation() \ No newline at end of file diff --git a/dev/old_to_remove/trajectory_optimisation/direct_collocation/pendulum_from_RRT.py b/dev/old_to_remove/trajectory_optimisation/direct_collocation/pendulum_from_RRT.py deleted file mode 100644 index 07791bf4..00000000 --- a/dev/old_to_remove/trajectory_optimisation/direct_collocation/pendulum_from_RRT.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Sep 10 11:12:11 2021 - -@author: alexandregirard -""" - -import numpy as np -from scipy.optimize import minimize -from pyro.dynamic import pendulum - -from pyro.analysis import simulation - - - - -sys = pendulum.SinglePendulum() - -n = 2 -m = 1 -grid = 45 -dt = 0.1 - -mass = 1 -target = 1 - -#dec = np.linspace(0,grid*3,grid*3) - - -def dec2xu(dec): - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - u[0,:] = dec[2*grid:3*grid] - - return x,u - -def cost(dec): - - J = 0 - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - u[0,:] = dec[2*grid:3*grid] - - for i in range(grid-1): - - #J = J + 0.5 * dt * ( (x[0,i]-target)**2 + (x[0,i+1]-target)**2 + x[1,i]**2 + x[1,i+1]**2 + u[0,i]**2 + u[0,i+1]**2 ) - - J = J + 0.5 * dt * ( u[0,i]**2 + u[0,i+1]**2 ) - - return J - -def constraints(dec): - - x,u = dec2xu(dec) - - vec=np.zeros(grid*2-2) - - for i in range(grid-1): - - vec[i] = (x[0,i+1] - x[0,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[0] + sys.f(x[:,i+1],u[:,i+1])[0] ) - vec[i+grid-1] = (x[1,i+1] - x[1,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[1] + sys.f(x[:,i+1],u[:,i+1])[1] ) - return vec - - -def compute_bounds(): - bounds = [] - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-4,1) ) - - bounds.append( (-3.14,-3.13) ) - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-6,4) ) - - bounds.append( (0,0.01) ) - - for i in range(grid): - - bounds.append( (-10,10) ) - - return bounds - - - -# Guess -traj = simulation.Trajectory.load('pendulum_rrt.npy') -sys.traj = traj -sys.plot_trajectory('xu') - -dec = np.zeros(grid*(n+m)) -dec[0:grid] = traj.x[:,0] -dec[grid:2*grid] = traj.x[:,1] -dec[2*grid:3*grid] = traj.u[:,0] - -bnds = compute_bounds() - -cons = {'type': 'eq', 'fun': constraints } -res = minimize( cost, dec, method='SLSQP', bounds=bnds, constraints=cons, options={'disp':True,'maxiter':1000}) # - - - -dec = res.x - -sys.traj.x[:,0] = dec[0:grid] -sys.traj.x[:,1] = dec[grid:2*grid] -sys.traj.u[:,0] = dec[2*grid:3*grid] - - -sys.plot_trajectory('xu') diff --git a/dev/old_to_remove/trajectory_optimisation/direct_collocation/pendulum_from_sratch.py b/dev/old_to_remove/trajectory_optimisation/direct_collocation/pendulum_from_sratch.py deleted file mode 100644 index 12e1835e..00000000 --- a/dev/old_to_remove/trajectory_optimisation/direct_collocation/pendulum_from_sratch.py +++ /dev/null @@ -1,129 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Sep 10 11:12:11 2021 - -@author: alexandregirard -""" - -import numpy as np -from scipy.optimize import minimize - -from pyro.dynamic import pendulum - - - -sys = pendulum.SinglePendulum() - -n = 2 -m = 1 -grid = 30 -dt = 0.2 - - -#dec = np.linspace(0,grid*3,grid*3) - - -def dec2xu(dec): - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - u[0,:] = dec[2*grid:3*grid] - - return x,u - -def cost(dec): - - J = 0 - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - u[0,:] = dec[2*grid:3*grid] - - for i in range(grid-1): - - target = -3.14 - J = J + 0.5 * dt * ( (x[0,i]-target)**2 + (x[0,i+1]-target)**2 + x[1,i]**2 + x[1,i+1]**2 + u[0,i]**2 + u[0,i+1]**2 ) - - #J = J + 0.5 * dt * ( u[0,i]**2 + u[0,i+1]**2 ) - - return J - -def constraints(dec): - - x,u = dec2xu(dec) - - vec=np.zeros(grid*2-2) - - for i in range(grid-1): - - vec[i] = (x[0,i+1] - x[0,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[0] + sys.f(x[:,i+1],u[:,i+1])[0] ) - vec[i+grid-1] = (x[1,i+1] - x[1,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[1] + sys.f(x[:,i+1],u[:,i+1])[1] ) - return vec - - -def compute_bounds(): - bounds = [] - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-4,1) ) - - bounds.append( (-3.14,-3.13) ) - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-6,4) ) - - bounds.append( (0,0.01) ) - - for i in range(grid): - - bounds.append( (-10,10) ) - - return bounds - - -def display_callback(a): - - - print('Iteration completed') - - return True - - - -# Guess -dec = np.zeros(grid*(n+m)) -#dec[0:grid] = traj.x[:,0] -#dec[grid:2*grid] = traj.x[:,1] -#dec[2*grid:3*grid] = traj.u[:,0] - -bnds = compute_bounds() - -cons = {'type': 'eq', 'fun': constraints } -res = minimize( cost, dec, method='SLSQP', bounds=bnds, constraints=cons, callback=display_callback, options={'disp':True,'maxiter':1000}) # - - -sys.compute_trajectory(grid*dt,grid) - - -dec = res.x - -sys.traj.x[:,0] = dec[0:grid] -sys.traj.x[:,1] = dec[grid:2*grid] -sys.traj.u[:,0] = dec[2*grid:3*grid] - - -sys.plot_trajectory('xu') -sys.animate_simulation() \ No newline at end of file diff --git a/dev/old_to_remove/trajectory_optimisation/direct_collocation/view_doublependulum_optimized_trajectory.py b/dev/old_to_remove/trajectory_optimisation/direct_collocation/view_doublependulum_optimized_trajectory.py deleted file mode 100644 index 4ddd518f..00000000 --- a/dev/old_to_remove/trajectory_optimisation/direct_collocation/view_doublependulum_optimized_trajectory.py +++ /dev/null @@ -1,17 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sat Oct 2 16:11:12 2021 - -@author: alex -""" -from pyro.dynamic import pendulum -from pyro.analysis import Trajectory - -sys = pendulum.DoublePendulum() - -#sys.traj = Trajectory.load('doublependulumswingup2sec.npy') -sys.traj = Trajectory.load('doublependulumswingup4sec.npy') - -sys.plot_trajectory('xu') -sys.animate_simulation() \ No newline at end of file diff --git a/dev/old_to_remove/trajectory_optimisation/ipopt_tests/test_ipopt_install.py b/dev/old_to_remove/trajectory_optimisation/ipopt_tests/test_ipopt_install.py deleted file mode 100644 index c153d36e..00000000 --- a/dev/old_to_remove/trajectory_optimisation/ipopt_tests/test_ipopt_install.py +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Sep 10 11:12:11 2021 - -@author: alexandregirard -""" -from scipy.optimize import minimize -from cyipopt import minimize_ipopt - -fun = lambda x: (x[0] - 1)**2 + (x[1] - 2.5)**2 - -cons = ({'type': 'ineq', 'fun': lambda x: x[0] - 2 * x[1] + 2}, {'type': 'ineq', 'fun': lambda x: -x[0] - 2 * x[1] + 6}, {'type': 'ineq', 'fun': lambda x: -x[0] + 2 * x[1] + 2}) - -bnds = ((0, None), (0, None)) - -res = minimize(fun, (2, 0), method='SLSQP', bounds=bnds, constraints=cons) - -print(res) - -res2 = minimize_ipopt(fun, (2, 0), bounds=bnds, constraints=cons) - -print(res2) \ No newline at end of file diff --git a/dev/old_to_remove/trajectory_optimisation/ipopt_tests/test_ipopt_simple.py b/dev/old_to_remove/trajectory_optimisation/ipopt_tests/test_ipopt_simple.py deleted file mode 100644 index bcd501a1..00000000 --- a/dev/old_to_remove/trajectory_optimisation/ipopt_tests/test_ipopt_simple.py +++ /dev/null @@ -1,35 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Sep 10 11:12:11 2021 - -@author: alexandregirard -""" -from scipy.optimize import minimize -from cyipopt import minimize_ipopt - -def cost (x): - - j = x[0]+x[1] - - return -j - -def constraint_circle(x): - - res = x[0]**2+x[1]**2 - 1 - - return -res - - -cons = ({'type': 'ineq', 'fun': constraint_circle}) -#cons = ({'type': 'eq', 'fun': constraint_circle}) - -bnds = ((0, None), (0, None)) - -res = minimize( cost, (0, 0), method='SLSQP', bounds=bnds, constraints=cons) - -print(res) - -res2 = minimize_ipopt( cost, (0, 0), bounds=bnds, constraints=cons) - -print(res2) \ No newline at end of file diff --git a/dev/old_to_remove/value_iteration/3d-vi-test/bicycle_parking_with_valueiteration.py b/dev/old_to_remove/value_iteration/3d-vi-test/bicycle_parking_with_valueiteration.py deleted file mode 100644 index 32961354..00000000 --- a/dev/old_to_remove/value_iteration/3d-vi-test/bicycle_parking_with_valueiteration.py +++ /dev/null @@ -1,59 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Mon Nov 12 20:28:17 2018 - -@author: Alexandre -""" - -import numpy as np - -from pyro.dynamic import vehicle -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import valueiteration -from pyro.control import controller - -sys = vehicle.KinematicBicyleModel() - -sys.x_ub = np.array( [+2,+2,+0.5] ) -sys.x_lb = np.array( [-0,-0,-0.5] ) - -sys.u_ub = np.array( [+1,+1.0] ) -sys.u_lb = np.array( [-1,-1.0] ) - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , (41,41,21) , (3,3) , 0.05 ) -# Cost Function -cf = costfunction.QuadraticCostFunction.from_sys(sys) - - -cf.xbar = np.array( [1,1,0] ) # target -cf.INF = 1E4 -cf.EPS = 0.2 -cf.R = np.array([[0.01,0],[0,0]]) - -# VI algo - -""" -EXPERIMENTAL TEST NOT WORKING -""" - -vi = valueiteration.ValueIteration_ND( grid_sys , cf ) - -vi.uselookuptable = True -vi.initialize() -vi.load_data('parking_vi') -vi.compute_steps(2) -vi.save_data('parking_vi') - -vi.assign_interpol_controller() - - -# -cl_sys = controller.ClosedLoopSystem( sys , vi.ctl ) - - -## Simulation and animation -cl_sys.x0 = np.array([0.2,0.2,0]) -tf = 5 -cl_sys.compute_trajectory( tf , 10001 , 'euler') diff --git a/dev/old_to_remove/value_iteration/bicycle_parking_with_valueiteration.py b/dev/old_to_remove/value_iteration/bicycle_parking_with_valueiteration.py deleted file mode 100644 index 58b09b99..00000000 --- a/dev/old_to_remove/value_iteration/bicycle_parking_with_valueiteration.py +++ /dev/null @@ -1,58 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Mon Nov 12 20:28:17 2018 - -@author: Alexandre -""" - -import numpy as np - -from pyro.dynamic import vehicle -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import valueiteration -from pyro.control import controller - -# initialize system -sys = vehicle.KinematicBicyleModel() - -sys.x_ub = np.array( [+2,+2,+0.5] ) -sys.x_lb = np.array( [-0,-0,-0.5] ) - -sys.u_ub = np.array( [+1,+1.0] ) -sys.u_lb = np.array( [-1,-1.0] ) - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , (41,41,21) , (3,3) , 0.05 ) -# Cost Function -cf = costfunction.QuadraticCostFunction.from_sys( sys ) -cf.xbar = np.array( [1,1,0]) -cf.INF = 1E4 -cf.EPS = 0 -cf.R = np.array([[0.01,0],[0,0]]) - -# VI algo - -vi = valueiteration.ValueIteration_ND( grid_sys , cf ) - -vi.uselookuptable = True -vi.initialize() -vi.load_data('bicycle_parking_vi') -#vi.compute_steps(100, maxJ=args.maxJ, plot=has_dynamic_plot) -#vi.save_data('bicycle_parking_vi') - -vi.assign_interpol_controller() - -vi.plot_cost2go(100) -vi.plot_policy(0) -vi.plot_policy(1) - -cl_sys = controller.ClosedLoopSystem( sys , vi.ctl ) - -## Simulation and animation -tf = 5 - -cl_sys.x0 = np.array( [0,0,0]) -cl_sys.compute_trajectory( tf , 10001 , 'euler') -cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation() diff --git a/dev/old_to_remove/value_iteration/car_with_valueiteration.py b/dev/old_to_remove/value_iteration/car_with_valueiteration.py deleted file mode 100644 index a542f491..00000000 --- a/dev/old_to_remove/value_iteration/car_with_valueiteration.py +++ /dev/null @@ -1,70 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Tue Nov 13 11:05:07 2018 - -@author: Alexandre -""" - -############################################################################### -import numpy as np -import argparse -############################################################################### -from pyro.dynamic import vehicle -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import valueiteration -from pyro.control import controller -############################################################################### - -sys = vehicle.KinematicCarModelwithObstacles() - -############################################################################### - -# Planning - -# Set domain -sys.x_ub = np.array([+35, +3, +3]) -sys.x_lb = np.array([-5, -2, -3]) - -sys.u_ub = np.array([+3, +1]) -sys.u_lb = np.array([-3, -1]) - -# Discrete world -grid_sys = discretizer.GridDynamicSystem(sys, (51, 51, 21), (3, 3), 0.1) - -# Cost Function -cf = costfunction.QuadraticCostFunction.from_sys( sys ) -cf.xbar = np.array( [30, 0, 0] ) # target -cf.INF = 1E8 -cf.EPS = 0.00 -cf.R = np.array([[0.1,0],[0,0]]) - -# VI algo - -vi = valueiteration.ValueIteration_ND( grid_sys , cf ) - -vi.uselookuptable = True -vi.initialize() - -#if load_data: -vi.load_data('car_vi') -# vi.compute_steps(100, plot=True, maxJ=100) -#if save_data: -# vi.save_data('car_vi_racecar_proto_scale_8') - -vi.assign_interpol_controller() - -#vi.plot_cost2go(maxJ=100) -vi.plot_policy(0) -vi.plot_policy(1) - -cl_sys = controller.ClosedLoopSystem( sys , vi.ctl ) -# -## Simulation and animation -x0 = np.array([0, 0, 0]) -tf = 20 - -cl_sys.x0 = x0 -cl_sys.compute_trajectory(tf, 10001, 'euler') -cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation() \ No newline at end of file diff --git a/dev/old_to_remove/value_iteration/car_with_valueiteration_computation.py b/dev/old_to_remove/value_iteration/car_with_valueiteration_computation.py deleted file mode 100644 index 28b920bd..00000000 --- a/dev/old_to_remove/value_iteration/car_with_valueiteration_computation.py +++ /dev/null @@ -1,70 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Tue Nov 13 11:05:07 2018 - -@author: Alexandre -""" - -############################################################################### -import numpy as np -import argparse -############################################################################### -from pyro.dynamic import vehicle -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import valueiteration -from pyro.control import controller -############################################################################### - -sys = vehicle.KinematicCarModelwithObstacles() - -############################################################################### - -# Planning - -# Set domain -sys.x_ub = np.array([+35, +3, +3]) -sys.x_lb = np.array([-5, -2, -3]) - -sys.u_ub = np.array([+3, +1]) -sys.u_lb = np.array([-3, -1]) - -# Discrete world -grid_sys = discretizer.GridDynamicSystem(sys, (51, 51, 21), (3, 3), 0.1) - -# Cost Function -cf = costfunction.QuadraticCostFunction.from_sys( sys ) -cf.xbar = np.array( [30, 0, 0] ) # target -cf.INF = 1E6 -cf.EPS = 0.00 -cf.R = np.array([[0.1,0],[0,0]]) - -# VI algo - -vi = valueiteration.ValueIteration_ND( grid_sys , cf ) - -vi.uselookuptable = True -vi.initialize() - - -vi.compute_steps(30, plot=True, maxJ=100) - -#if save_data: -vi.save_data('car_vi_test_mac_mini') - -vi.assign_interpol_controller() - -#vi.plot_cost2go(maxJ=100) -vi.plot_policy(0) -vi.plot_policy(1) - -cl_sys = controller.ClosedLoopSystem( sys , vi.ctl ) -# -## Simulation and animation -x0 = np.array([0, 0, 0]) -tf = 20 - -cl_sys.x0 = x0 -cl_sys.compute_trajectory(tf, 10001, 'euler') -cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation() \ No newline at end of file diff --git a/dev/old_to_remove/value_iteration/double_pendulum_with_valueiteration_nd.py b/dev/old_to_remove/value_iteration/double_pendulum_with_valueiteration_nd.py deleted file mode 100644 index e4d1881f..00000000 --- a/dev/old_to_remove/value_iteration/double_pendulum_with_valueiteration_nd.py +++ /dev/null @@ -1,63 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Fri Nov 16 12:05:08 2018 - -@author: Alexandre -""" - -#import matplotlib -#matplotlib.use('Qt4Agg') - -############################################################################### -import numpy as np -############################################################################### -from pyro.analysis import costfunction -from pyro.control import controller -from pyro.dynamic import pendulum -############################################################################### - - -import matplotlib.pyplot as plt - -from pyro.planning import discretizer, valueiteration - -# Continuous dynamic system -sys = pendulum.DoublePendulum() - -sys.x_ub = np.array([0.2,0.2,0.2,0.2]) -sys.x_lb = np.array([-0.2,-0.2,-0.2,-0.2]) - -sys.u_ub = np.array([5,2]) -sys.u_lb = np.array([-5,-2]) - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , ( 11, 11, 11, 11) , (3,3) , 0.05) - -# Cost Function -qcf = sys.cost_function - -qcf.xbar = np.array([ 0,0,0,0 ]) # target -qcf.INF = 100 - -# VI algo -vi = valueiteration.ValueIteration_ND( grid_sys , qcf ) - -vi.initialize() -#vi.load_data('bbb') -vi.compute_steps(2,True) -#vi.load_data() -vi.assign_interpol_controller() -vi.plot_policy(0) -vi.plot_cost2go() -vi.save_data('bbb') - - -#asign controller -cl_sys = controller.ClosedLoopSystem( sys , vi.ctl ) - -# Simulation and animation -cl_sys.x0 = np.array([0.1,0.1,0,0]) -cl_sys.compute_trajectory(2,2001,'euler') -cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation() - diff --git a/dev/old_to_remove/value_iteration/holonomic_3d_with_obstacles_valueiteration_nd.py b/dev/old_to_remove/value_iteration/holonomic_3d_with_obstacles_valueiteration_nd.py deleted file mode 100644 index 50f973a6..00000000 --- a/dev/old_to_remove/value_iteration/holonomic_3d_with_obstacles_valueiteration_nd.py +++ /dev/null @@ -1,48 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Mon Nov 12 20:28:17 2018 - -@author: Alexandre -""" - -import numpy as np - -from pyro.dynamic import vehicle -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import valueiteration - -sys = vehicle.Holonomic3DMobileRobotwithObstacles() - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , (41,41,21) , (3,3) ) - -# Cost Function -cf = costfunction.QuadraticCostFunction( - q=np.ones(sys.n), - r=np.ones(sys.m), - v=np.zeros(sys.p) -) - -cf.INF = 1E9 - -# VI algo -vi = valueiteration.ValueIteration_ND( grid_sys , cf ) - -vi.initialize() -vi.load_data('holonomic_3d_obstacles_vi') -# vi.compute_steps(0, maxJ=4000, plot=False) -vi.plot_cost2go(4000) -vi.assign_interpol_controller() -vi.plot_policy(0) -vi.plot_policy(1) -# vi.save_data('holonomic_3d_obstacles_vi') - -# Closed loop -cl_sys = vi.ctl + sys - -# Simulation and animation -cl_sys.x0 = np.array([9,0,0]) -cl_sys.compute_trajectory(tf=20) -cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation() diff --git a/dev/old_to_remove/value_iteration/holonomic_mobile_robot_with_valueiteration_nd.py b/dev/old_to_remove/value_iteration/holonomic_mobile_robot_with_valueiteration_nd.py deleted file mode 100644 index ee3d470b..00000000 --- a/dev/old_to_remove/value_iteration/holonomic_mobile_robot_with_valueiteration_nd.py +++ /dev/null @@ -1,49 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Mon Nov 12 20:28:17 2018 - -@author: Alexandre -""" - -import numpy as np - -from pyro.dynamic import vehicle -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import valueiteration - -sys = vehicle.HolonomicMobileRobotwithObstacles() - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , (51,51) , (3,3) ) - -# Cost Function -cf = costfunction.QuadraticCostFunction( - q=np.ones(sys.n), - r=np.ones(sys.m), - v=np.zeros(sys.p) -) - -cf.INF = 1E9 - -# VI algo -vi = valueiteration.ValueIteration_ND( grid_sys , cf ) - -vi.initialize() -vi.load_data('holonomic_vi') -# vi.compute_steps(500) -vi.plot_cost2go(40000) -vi.assign_interpol_controller() -vi.plot_policy(0) -vi.plot_policy(1) -vi.plot_cost2go() -vi.save_data('holonomic_vi') - -# Closed loop -cl_sys = vi.ctl + sys - -# Simulation and animation -x0 = [9,0] -sim = cl_sys.compute_trajectory(x0 , tf=20) -cl_sys.plot_trajectory(sim, 'xu') -cl_sys.animate_simulation(sim, save=True, file_name='holonomic') diff --git a/dev/old_to_remove/value_iteration/simple_pendulum_with_valueiteration.py b/dev/old_to_remove/value_iteration/simple_pendulum_with_valueiteration.py deleted file mode 100644 index f96c1965..00000000 --- a/dev/old_to_remove/value_iteration/simple_pendulum_with_valueiteration.py +++ /dev/null @@ -1,49 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Mon Nov 12 20:28:17 2018 - -@author: Alexandre -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import valueiteration -from pyro.control import controller - -sys = pendulum.SinglePendulum() - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys ) - -# Cost Function -qcf = sys.cost_function - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.INF = 10000 - -# VI algo -vi = valueiteration.ValueIteration_ND( grid_sys , qcf ) - -vi.initialize() -#vi.load_data('simple_pendulum_vi') -vi.plot_max_J = 250 -vi.compute_steps(100, plot=True) -vi.assign_interpol_controller() -vi.plot_policy(0) -vi.plot_cost2go() -# vi.save_data('simple_pendulum_vi') - -#asign controller -cl_sys = controller.ClosedLoopSystem( sys , vi.ctl ) -cl_sys.cost_function = None - -# Simulation and animation -cl_sys.x0 = np.array([0,0]) -tf = 10 -cl_sys.compute_trajectory(tf) -cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation() - diff --git a/dev/old_to_remove/value_iteration/udes_racecar_with_valueiteration.py b/dev/old_to_remove/value_iteration/udes_racecar_with_valueiteration.py deleted file mode 100644 index 475f4504..00000000 --- a/dev/old_to_remove/value_iteration/udes_racecar_with_valueiteration.py +++ /dev/null @@ -1,68 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Tue Nov 13 11:05:07 2018 - -@author: Alexandre -""" - -############################################################################### -import numpy as np -############################################################################### -from pyro.dynamic import vehicle -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import valueiteration -from pyro.control import controller -############################################################################### - -sys = vehicle.KinematicProtoCarModelwithObstacles() - -############################################################################### - -# Planning - -# Set domain -sys.x_ub = np.array([+3.5, +1, +0.3]) -sys.x_lb = np.array([-2, -1, -0.3]) - -sys.u_ub = np.array([+1, +1]) -sys.u_lb = np.array([-1, -1]) - -# Discrete world -grid_sys = discretizer.GridDynamicSystem(sys, (61, 61, 21), (3, 3), 0.025) -# Cost Function -cf = costfunction.QuadraticCostFunction(sys.n,sys.m,sys.p) - -cf.xbar = np.array( [3, 0, 0] ) # target -cf.INF = 1E4 -cf.EPS = 0.05 -cf.R = np.array([[0.01,0],[0,0]]) - -# VI algo - -vi = valueiteration.ValueIteration_ND( grid_sys , cf ) - -vi.uselookuptable = True -vi.initialize() - -#if load_data: -vi.load_data('udes_racecar') -# vi.compute_steps(50, plot=True, maxJ=100) -#if save_data: -# vi.save_data('udes_racecar') - -vi.assign_interpol_controller() - -vi.plot_cost2go(maxJ=100) -vi.plot_policy(0) -vi.plot_policy(1) - -cl_sys = controller.ClosedLoopSystem( sys , vi.ctl ) -# -## Simulation and animation -cl_sys.x0 = np.array([0, 0, 0]) -tf = 5 - -cl_sys.compute_trajectory(tf, 10001, 'euler') -cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation() \ No newline at end of file diff --git a/dev/plane/plane.py b/dev/plane/plane.py deleted file mode 100644 index 43a31d07..00000000 --- a/dev/plane/plane.py +++ /dev/null @@ -1,117 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Tue Jun 20 10:29:50 2023 - -@author: alex -""" - -############################################################################### -import numpy as np -import matplotlib.pyplot as plt -############################################################################### -from pyro.dynamic import system -from pyro.dynamic import mechanical -############################################################################### - - - - -############################################################################## -# 2D planar drone -############################################################################## - -class Plane2D( mechanical.MechanicalSystem ): - - """ - Equations of Motion - ------------------------- - - """ - - ############################ - def __init__(self): - """ """ - - # initialize standard params - mechanical.MechanicalSystemWithPositionInputs.__init__( self, 3 , 1 , 1 ) - - # Labels - self.name = '2D plane model' - self.state_label = ['x','y','theta','vx','vy','w'] - self.input_label = ['Trust', 'delta'] - self.output_label = self.state_label - - # Units - self.state_units = ['[m]','[m]','[rad]','[m/sec]','[m/sec]','[rad/sec]'] - self.input_units = ['[N]', '[Rad]'] - self.output_units = self.state_units - - # State working range - self.x_ub = np.array([+50,+100,+2,10,10,10]) - self.x_lb = np.array([-50,-0,-2,-10,-10,-10]) - - # Model param - self.mass = 1000 - self.inertia = 100 - self.gravity = 9.8 - - # Kinematic param - - - # Graphic output parameters - self.dynamic_domain = True - self.dynamic_range = 10 - - - - ########################################################################### - def H(self, q ): - """ - Inertia matrix - ---------------------------------- - dim( H ) = ( dof , dof ) - - such that --> Kinetic Energy = 0.5 * dq^T * H(q) * dq - - """ - - H = np.zeros((3,3)) - - H[0,0] = self.mass - H[1,1] = self.mass - H[2,2] = self.inertia - - return H - - - ########################################################################### - def C(self, q , dq ): - """ - Corriolis and Centrifugal Matrix - ------------------------------------ - dim( C ) = ( dof , dof ) - - such that --> d H / dt = C + C^T - - - """ - - C = np.zeros((3,3)) - - return C - - - ########################################################################### - def g(self, q ): - """ - Gravitationnal forces vector : dof x 1 - """ - - g = np.zeros(3) - - g[1] = self.mass * self.gravity - - return g - - \ No newline at end of file diff --git a/dev/reinforcement_learning/approximate_dp_tests/approximatedynamicprogramming.py b/dev/reinforcement_learning/approximate_dp_tests/approximatedynamicprogramming.py deleted file mode 100644 index e7591e02..00000000 --- a/dev/reinforcement_learning/approximate_dp_tests/approximatedynamicprogramming.py +++ /dev/null @@ -1,234 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Thu Mar 30 20:27:58 2023 - -@author: alex -""" - -import numpy as np -import matplotlib.pyplot as plt -import matplotlib.animation as animation - -import time - -from pyro.planning import dynamicprogramming - -from functionapproximation import LinearFunctionApproximator - - -############################################################################### -### DP Algo -############################################################################### - -class LinearApproximateDynamicProgramming( dynamicprogramming.DynamicProgrammingWithLookUpTable ): - """ """ - - ############################ - def __init__(self, grid_sys , cost_function , function_approximation , t = 0): - - # Dynamic system - self.grid_sys = grid_sys # Discretized Dynamic system class - self.sys = grid_sys.sys # Base Dynamic system class - - # Function approx - self.fa = function_approximation - - # Cost function - self.cf = cost_function - self.t = t - - # Options - self.alpha = 0.9 # exponential forgetting factor - self.gamma = 0.2 - self.save_time_history = True - - # Memory Variables - self.k = 0 # Number of computed steps - - # Start time (needed to plot elapsed computation time) - self.start_time = time.time() - - # Init params - self.w = np.zeros( self.fa.n ) # Initial params - - self.compute_cost_lookuptable() - - self.compute_kernels() - - self.J = self.P.T @ self.w # Initial J approx on the grid - - # - if self.save_time_history: - - self.w_list = [] - self.J_list = [] - self.t_list = [] - - # Value at t = t_f - self.J_list.append( self.J ) - self.w_list.append( self.w ) - self.t_list.append( self.k ) - - - ############################### - def compute_kernels(self): - """ Compute Kernels """ - - print('\nComputing Funtion Approximation Kernels:') - print('-----------------------------------------') - - self.P = self.fa.compute_all_kernel( self.grid_sys.state_from_node_id ) - - m = self.grid_sys.nodes_n * self.grid_sys.actions_n - n = self.sys.n - - Xnext = self.grid_sys.x_next_table.reshape( ( m , n) ) - self.P_next = self.fa.compute_all_kernel( Xnext ) - - - #### Test - eps = 0.4 #self.cf.EPS - xbar = self.cf.xbar - - on_target = np.full( self.grid_sys.x_next_table[:,:,0].shape , True ) - - for i in range( self.sys.n ): - - on_target = np.logical_and( on_target , (self.grid_sys.x_next_table[:,:,i] - xbar[i])**2 < eps ) - - self.on_target = on_target - - #self.off_target = - #### - - - ############################### - def initialize_backward_step(self): - """ One step of value iteration """ - - # Update values - self.w_old = self.w - self.k = self.k + 1 # index backward in time - - - ############################### - def compute_backward_step(self): - """ One step of value iteration """ - - J_next = self.P_next.T @ self.w - - Q = self.G + self.alpha * J_next.reshape( ( self.grid_sys.nodes_n , self.grid_sys.actions_n ) ) - - Q[ self.on_target ] = 0. # Test - Q[ Q > self.cf.INF ] = self.cf.INF # Test - - J_d = Q.min( axis = 1 ) # New J Samples - - w , J_hat = self.fa.least_square_fit( J_d , self.P ) - - #e = self.J - J_d - #dJ_dw = self.fa.dJ_dw() - - - - #self.J = J_hat - #self.w = w - self.w = self.w + self.gamma * ( w - self.w ) - self.J = self.P.T @ self.w - - - ############################### - def finalize_backward_step(self): - """ One step of value iteration """ - - # Computation time - elapsed_time = time.time() - self.start_time - - # Convergence check - j_max = self.J.max() - delta = self.w - self.w_old - delta_max = delta.max() - delta_min = delta.min() - - print('%d t:%.2f Elasped:%.2f max: %.2f dmax:%.2f dmin:%.2f' % (self.k,self.t,elapsed_time,j_max,delta_max,delta_min) ) - - # List in memory - if self.save_time_history: - self.J_list.append( self.J ) - self.w_list.append( self.w ) - self.t_list.append( self.k ) - - # return largest J change for usage as stoping criteria - return abs(np.array([delta_max,delta_min])).max() - - ################################ - def clean_infeasible_set(self , tol = 1): - """ - Set default policy and cost2go to cf.INF for state for which it is unavoidable - that they will reach unallowable regions - - """ - - pass - - - - -''' -################################################################# -################## Main ######## -################################################################# -''' - - -if __name__ == "__main__": - """ MAIN TEST """ - - - from pyro.dynamic import pendulum - from pyro.planning import discretizer - from pyro.analysis import costfunction - from functionapproximation import QuadraticFunctionApproximator - from functionapproximation import MultipleGaussianFunctionApproximator - - sys = pendulum.SinglePendulum() - - # Discrete world - grid_sys = discretizer.GridDynamicSystem( sys , [101,101] , [3] ) - - # Cost Function - qcf = costfunction.QuadraticCostFunction.from_sys(sys) - - qcf.xbar = np.array([ -3.14 , 0 ]) # target - qcf.INF = 300 - - # Approx - - fa = QuadraticFunctionApproximator( sys.n , x0 = qcf.xbar ) - - # Discrete world - grid_sys_gaussian = discretizer.GridDynamicSystem( sys , [11,11] , [3] , 0.05) - X0 = grid_sys_gaussian.state_from_node_id - - #fa = MultipleGaussianFunctionApproximator( X0 , 3.0 ) + fa - - fa = MultipleGaussianFunctionApproximator( X0 , 1.0 ) - - # DP algo - dp = LinearApproximateDynamicProgramming( grid_sys, qcf, fa ) - - dp.alpha = 0.8 - dp.gamma = 1.0 - - dp.w = dp.w + 20 - - #dp.solve_bellman_equation( tol = 0.1 ) - dp.compute_steps(100) - - #dp.plot_cost2go() - #dp.plot_cost2go_3D() - dp.animate_cost2go() - - - \ No newline at end of file diff --git a/dev/reinforcement_learning/approximate_dp_tests/cost2go_lqr_vs_vi.py b/dev/reinforcement_learning/approximate_dp_tests/cost2go_lqr_vs_vi.py deleted file mode 100644 index cf1d8be7..00000000 --- a/dev/reinforcement_learning/approximate_dp_tests/cost2go_lqr_vs_vi.py +++ /dev/null @@ -1,81 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import massspringdamper -from pyro.control import controller -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming -from pyro.planning import discretizer - -from scipy.linalg import solve_continuous_are - -from functionapproximation import QuadraticFunctionApproximator - -sys = massspringdamper.FloatingSingleMass() - -sys.x_ub[0] = 10.0 -sys.x_lb[0] = -10.0 -sys.x_lb[1] = -5.0 -sys.x_ub[1] = 5.0 -sys.u_ub[0] = 5.0 -sys.u_lb[0] = -5.0 - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [101,101] , [11] , 0.05) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -0 , 0 ]) # target -qcf.INF = 300 - -qcf.R[0,0] = 10.0 - -qcf.S[0,0] = 10.0 -qcf.S[1,1] = 10.0 - -#################### -# Value iteration solution -#################### - -dp = dynamicprogramming.DynamicProgrammingWithLookUpTable( grid_sys, qcf) - -dp.solve_bellman_equation( tol = 0.5 ) - -ctl = dp.get_lookup_table_controller() - -dp.plot_cost2go() -dp.plot_cost2go_3D() - -#################### -# Quadratic Approx -#################### - -qfa = QuadraticFunctionApproximator( sys.n , x0 = qcf.xbar ) - -Xs = grid_sys.state_from_node_id # All state on the grid - -P = qfa.compute_all_kernel( Xs ) - -w , J_hat = qfa.least_square_fit( dp.J , P ) - -grid_sys.plot_grid_value_3D( J_hat , None , 'Quadratic approx') - -grid_sys.plot_grid_value_3D( dp.J , J_hat , 'J vs. J_hat') - -#################### -# Riccati solution -#################### - -S = solve_continuous_are( sys.A , sys.B , qcf.Q , qcf.R ) - -w_riccati = np.array([0, 0, 0, S[0,0], S[1,1], S[0,1]]) -J_riccati = P.T @ w_riccati -grid_sys.plot_grid_value_3D( J_riccati , J_hat , 'J_riccati vs. J_hat') \ No newline at end of file diff --git a/dev/reinforcement_learning/approximate_dp_tests/fitted_vi_pendulum_21x21_working.py b/dev/reinforcement_learning/approximate_dp_tests/fitted_vi_pendulum_21x21_working.py deleted file mode 100644 index 4e296141..00000000 --- a/dev/reinforcement_learning/approximate_dp_tests/fitted_vi_pendulum_21x21_working.py +++ /dev/null @@ -1,53 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Mar 31 11:35:09 2023 - -@author: alex -""" -import numpy as np - -from pyro.dynamic import pendulum -from pyro.planning import discretizer -from pyro.analysis import costfunction -from approximatedynamicprogramming import LinearApproximateDynamicProgramming -from functionapproximation import QuadraticFunctionApproximator -from functionapproximation import MultipleGaussianFunctionApproximator - -sys = pendulum.SinglePendulum() - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [101,101] , [3] ) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.INF = 300 - -# Approx - -fa = QuadraticFunctionApproximator( sys.n , x0 = qcf.xbar ) - -# Discrete world -grid_sys_gaussian = discretizer.GridDynamicSystem( sys , [21,21] , [3] , 0.05) -X0 = grid_sys_gaussian.state_from_node_id - -#fa = MultipleGaussianFunctionApproximator( X0 , 3.0 ) + fa - -fa = MultipleGaussianFunctionApproximator( X0 , 0.5 ) - -# DP algo -dp = LinearApproximateDynamicProgramming( grid_sys, qcf, fa ) - -dp.alpha = 0.95 -dp.gamma = 0.2 - -dp.w = dp.w + 20 - -#dp.solve_bellman_equation( tol = 0.1 ) -dp.compute_steps(100) - -#dp.plot_cost2go() -#dp.plot_cost2go_3D() -dp.animate_cost2go() \ No newline at end of file diff --git a/dev/reinforcement_learning/approximate_dp_tests/fitted_vi_pendulum_lowdef_convergence.py b/dev/reinforcement_learning/approximate_dp_tests/fitted_vi_pendulum_lowdef_convergence.py deleted file mode 100644 index 8f9e7a56..00000000 --- a/dev/reinforcement_learning/approximate_dp_tests/fitted_vi_pendulum_lowdef_convergence.py +++ /dev/null @@ -1,53 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Mar 31 11:35:09 2023 - -@author: alex -""" -import numpy as np - -from pyro.dynamic import pendulum -from pyro.planning import discretizer -from pyro.analysis import costfunction -from approximatedynamicprogramming import LinearApproximateDynamicProgramming -from functionapproximation import QuadraticFunctionApproximator -from functionapproximation import MultipleGaussianFunctionApproximator - -sys = pendulum.SinglePendulum() - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [101,101] , [3] ) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.INF = 300 - -# Approx - -fa = QuadraticFunctionApproximator( sys.n , x0 = qcf.xbar ) - -# Discrete world -grid_sys_gaussian = discretizer.GridDynamicSystem( sys , [11,11] , [3] , 0.05) -X0 = grid_sys_gaussian.state_from_node_id - -#fa = MultipleGaussianFunctionApproximator( X0 , 3.0 ) + fa - -fa = MultipleGaussianFunctionApproximator( X0 , 1.0 ) - -# DP algo -dp = LinearApproximateDynamicProgramming( grid_sys, qcf, fa ) - -dp.alpha = 0.8 -dp.gamma = 1.0 - -dp.w = dp.w + 20 - -#dp.solve_bellman_equation( tol = 0.1 ) -dp.compute_steps(100) - -#dp.plot_cost2go() -#dp.plot_cost2go_3D() -dp.animate_cost2go() \ No newline at end of file diff --git a/dev/reinforcement_learning/approximate_dp_tests/fitted_vi_pendulum_test.py b/dev/reinforcement_learning/approximate_dp_tests/fitted_vi_pendulum_test.py deleted file mode 100644 index 916252da..00000000 --- a/dev/reinforcement_learning/approximate_dp_tests/fitted_vi_pendulum_test.py +++ /dev/null @@ -1,53 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Mar 31 11:35:09 2023 - -@author: alex -""" -import numpy as np - -from pyro.dynamic import pendulum -from pyro.planning import discretizer -from pyro.analysis import costfunction -from approximatedynamicprogramming import LinearApproximateDynamicProgramming -from functionapproximation import QuadraticFunctionApproximator -from functionapproximation import MultipleGaussianFunctionApproximator - -sys = pendulum.SinglePendulum() - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [101,101] , [3] , 0.1 ) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.INF = 300 - -# Approx - -fa = QuadraticFunctionApproximator( sys.n , x0 = qcf.xbar ) - -# Discrete world -grid_sys_gaussian = discretizer.GridDynamicSystem( sys , [41,41] , [3] , 0.05) -X0 = grid_sys_gaussian.state_from_node_id - -#fa = MultipleGaussianFunctionApproximator( X0 , 3.0 ) + fa - -fa = MultipleGaussianFunctionApproximator( X0 , 0.30 ) - -# DP algo -dp = LinearApproximateDynamicProgramming( grid_sys, qcf, fa ) - -dp.alpha = 0.95 -dp.gamma = 1.0 - -dp.w = dp.w + 20 - -#dp.solve_bellman_equation( tol = 0.1 ) -dp.compute_steps(100) - -#dp.plot_cost2go() -#dp.plot_cost2go_3D() -dp.animate_cost2go() \ No newline at end of file diff --git a/dev/reinforcement_learning/approximate_dp_tests/functionapproximation.py b/dev/reinforcement_learning/approximate_dp_tests/functionapproximation.py deleted file mode 100644 index 383387c2..00000000 --- a/dev/reinforcement_learning/approximate_dp_tests/functionapproximation.py +++ /dev/null @@ -1,335 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Tue Mar 28 15:41:59 2023 - -@author: alex -""" - -import numpy as np - -############################################################################### -### Function approximator -############################################################################### - -class FunctionApproximator(): - - ############################ - def __init__(self, n = 10 ): - - self.n = n # number of parameters - - #self.w = np.zeros( self.n ) - - - ############################ - def J_hat( self , x , w ): - """ Compute J approx given state x and param w """ - - return 0 - - - ############################ - def dJ_dw( self , x ): - """ Compute dJ/dw given state x """ - - return np.zeros( self.n ) - - -############################################################################### -### Linear Function approximator -############################################################################### - -class LinearFunctionApproximator( FunctionApproximator ): - - ############################ - def __init__(self, n = 10 ): - - FunctionApproximator.__init__(self,n) - - - ############################ - def J_hat( self , x , w ): - """ Compute J approx given state x and param w """ - - phi = self.compute_kernel( x ) - - J_hat = phi.T @ w - - return J_hat - - - ############################ - def dJ_dw( self , x ): - """ Compute dJ/dw given state x """ - - phi = self.compute_kernel( x ) - - return phi - - - ############################ - def compute_kernel( self , x ): - """ Compute kernel functions phi given a state x """ - - phi = np.zeros( self.n ) - - return phi - - - ############################ - def compute_all_kernel( self , Xs ): - """ Compute all kernel functions phi given a m state x """ - - m = Xs.shape[0] # number of data point - n = self.n # number of params - - P = np.zeros( ( m , n )) - - for i in range(m): - - P[i,:] = self.compute_kernel( Xs[i,:] ) - - return P.T - - - ############################ - def least_square_fit( self , Js , P ): - """ solve J_d = P w """ - - #P = self.compute_all_kernel( Xs ) - - w = np.linalg.lstsq( P.T , Js , rcond=None)[0] - - J_hat = P.T @ w - - return w , J_hat - - ############################# - def __add__(self, other ): - """ - return new function approx with all kernels - """ - - return CombinedLinearFunctionApproximator( self , other ) - - - -############################################################################### -### Combined Linear Function approximator -############################################################################### - -class CombinedLinearFunctionApproximator( LinearFunctionApproximator ): - - ############################ - def __init__(self, FA1 , FA2 ): - - FunctionApproximator.__init__(self, FA1.n + FA2.n ) - - self.FA1 = FA1 - self.FA2 = FA2 - - - - ############################ - def compute_kernel( self , x ): - """ Compute kernel functions phi given a state x """ - - phi1 = self.FA1.compute_kernel( x ) - phi2 = self.FA2.compute_kernel( x ) - - phi = np.concatenate(( phi1, phi2 )) - - return phi - - - - - - -############################################################################### -### Quadratic Function approximator -############################################################################### - -class QuadraticFunctionApproximator( LinearFunctionApproximator ): - - ############################ - def __init__(self, sys_n = 2 , x0 = None): - """ - J_hat = C + B x + x' A x = w' phi - - """ - - self.sys_n = sys_n - - if x0 is not None: - - self.x0 = x0 - - else: - - self.x0 = np.zeros( sys_n ) - - self.n_2_diag = sys_n - self.n_2_off = int((sys_n**2-sys_n)/2) - self.n_2 = +self.n_2_diag + self.n_2_off # 2nd order number of weight - self.n_1 = sys_n # 1nd order number of weight - self.n_0 = 1 # 0nd order number ofweight - - # Total number of parameters - self.n = int(self.n_2 + self.n_1 + self.n_0) - - - ############################ - def compute_kernel( self , x ): - """ return approx a state x """ - - phi = np.zeros( self.n ) - - x = x - self.x0 - - xxT = np.outer( x , x ) - - #indices - n0 = self.n_0 - n1 = self.n_0 + self.n_1 - n2 = self.n_0 + self.n_1 + self.n_2_diag - n3 = self.n_0 + self.n_1 + self.n_2_diag + self.n_2_off - - phi[0] = 1 - phi[n0:n1] = x - phi[n1:n2] = np.diag( xxT ) - phi[n2:n3] = xxT[np.triu_indices( self.sys_n, k = 1)] - - return phi - - -############################################################################### -### Radial basis function Function approximator -############################################################################### - -class GaussianFunctionApproximator( LinearFunctionApproximator ): - - ############################ - def __init__(self, x0 , sig = 6.0 ): - """ - J_hat = exp( - || x - x0 || / 2 sig^2 ) - - """ - - self.x0 = x0 - self.sys_n = x0.shape[0] - self.n = 1 - self.a = -0.5 / (sig**2) - - - ############################ - def compute_kernel( self , x ): - """ return approx a state x """ - - phi = np.array([0.]) - - e = x - self.x0 - r = e.T @ e - - phi[0] = np.exp( self.a * r ) - - return phi - - -############################################################################### -class MultipleGaussianFunctionApproximator( LinearFunctionApproximator ): - - ############################ - def __init__(self, Xs , sig = 1.0 ): - """ - J_hat = sum exp( - || x - x0 || / 2 sig^2 ) - - """ - - self.Xs = Xs - self.sys_n = Xs.shape[1] - self.n = Xs.shape[0] # number of data point - self.a = -0.5 / (sig**2) - - - ############################ - def compute_kernel( self , x ): - """ return approx a state x """ - - phi = np.zeros(self.n) - - for i in range(self.n): - - e = x - self.Xs[i,:] - r = e.T @ e - - phi[i] = np.exp( self.a * r ) - - return phi - - - -############################################################################### -### Main -############################################################################### - - -if __name__ == "__main__": - """ MAIN TEST """ - - from pyro.dynamic import pendulum - from pyro.dynamic import massspringdamper - from pyro.planning import discretizer - from pyro.analysis import costfunction - from pyro.planning import dynamicprogramming - - sys = pendulum.SinglePendulum() - #sys = - - # Discrete world - grid_sys = discretizer.GridDynamicSystem( sys , [101,101] , [3] ) - - # Cost Function - qcf = costfunction.QuadraticCostFunction.from_sys(sys) - - qcf.xbar = np.array([ -3.14 , 0 ]) # target - qcf.INF = 200 - - # DP algo - dp = dynamicprogramming.DynamicProgrammingWithLookUpTable(grid_sys, qcf) - - dp.solve_bellman_equation( tol = 1.0 ) - dp.plot_cost2go_3D() - - - - # Approx - qfa0 = QuadraticFunctionApproximator( sys.n , x0 = qcf.xbar ) - - sig = 2.0 - - qfa1 = GaussianFunctionApproximator( x0 = np.array([0,0]), sig = sig ) - qfa2 = GaussianFunctionApproximator( x0 = qcf.xbar , sig = sig) - qfa3 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([1,0]) , sig = sig) - qfa4 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([1,1]) , sig = sig) - qfa5 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0,1]) , sig = sig) - qfa6 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([-1,0]) , sig = sig) - qfa7 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([-1,-1]) , sig = sig) - qfa8 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0,-1]) , sig = sig) - qfa9 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0.5,0]) , sig = sig) - qfa10 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0.5,0.5]) , sig = sig) - qfa11 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0,0.5]) , sig = sig) - qfa = qfa0 + qfa1 + qfa2 + qfa3 + qfa4 + qfa5 + qfa6 + qfa7 + qfa8 + qfa9 + qfa10 + qfa11 - - Xs = grid_sys.state_from_node_id # All state on the grid - - P = qfa.compute_all_kernel( Xs ) - - w , J_hat = qfa.least_square_fit( dp.J , P ) - - grid_sys.plot_grid_value_3D( J_hat , None , ' J_hat') - grid_sys.plot_grid_value_3D( dp.J , J_hat , 'J vs. J_hat') - - - \ No newline at end of file diff --git a/dev/reinforcement_learning/approximate_dp_tests/pendulum_cost2go_guassian_approx.py b/dev/reinforcement_learning/approximate_dp_tests/pendulum_cost2go_guassian_approx.py deleted file mode 100644 index 5242bd7f..00000000 --- a/dev/reinforcement_learning/approximate_dp_tests/pendulum_cost2go_guassian_approx.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.dynamic import massspringdamper -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming - -from scipy.linalg import solve_continuous_are - -from functionapproximation import QuadraticFunctionApproximator -from functionapproximation import GaussianFunctionApproximator -from functionapproximation import MultipleGaussianFunctionApproximator - -sys = pendulum.SinglePendulum() - -sys.x_ub[0] = 10.0 -sys.x_lb[0] = -10.0 -sys.x_lb[1] = -5.0 -sys.x_ub[1] = 5.0 -sys.u_ub[0] = 5.0 -sys.u_lb[0] = -5.0 - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [201,201] , [11] , 0.05) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.INF = 200 - -#################### -# Value iteration solution -#################### - -dp = dynamicprogramming.DynamicProgrammingWithLookUpTable( grid_sys, qcf) - -dp.solve_bellman_equation( tol = 0.5 ) - -#ctl = dp.get_lookup_table_controller() - -#dp.plot_cost2go() -dp.plot_cost2go_3D() - - - -#################### -# Gaussian Approx -#################### - -# ============================================================================= -# qfa1 = GaussianFunctionApproximator( x0 = np.array([0,0]) ) -# qfa2 = GaussianFunctionApproximator( x0 = qcf.xbar ) -# qfa3 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([1,0]) ) -# qfa4 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([1,1]) ) -# qfa5 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0,1]) ) -# qfa6 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([-1,0]) ) -# qfa7 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([-1,-1]) ) -# qfa8 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0,-1]) ) -# qfa9 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0.5,0]) ) -# qfa10 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0.5,0.5]) ) -# qfa11 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0,0.5]) ) -# -# qfa = qfa1 + qfa2 + qfa3 + qfa4 + qfa5 + qfa6 + qfa7 + qfa8 + qfa9 + qfa10 + qfa11 -# ============================================================================= - - -# Discrete world -grid_sys_gaussian = discretizer.GridDynamicSystem( sys , [21,21] , [3] , 0.05) -X0 = grid_sys_gaussian.state_from_node_id - -qfa = MultipleGaussianFunctionApproximator( X0 ) + QuadraticFunctionApproximator( sys.n ) - -Xs = grid_sys.state_from_node_id # All state on the grid - -P = qfa.compute_all_kernel( Xs ) - -w , J_hat = qfa.least_square_fit( dp.J , P ) - -grid_sys.plot_grid_value_3D( J_hat , None , 'Gaussian approx') - -grid_sys.plot_grid_value_3D( dp.J , J_hat , 'J vs. J_hat') diff --git a/dev/reinforcement_learning/approximate_dp_tests/pendulum_cost2go_guassian_approx_test.py b/dev/reinforcement_learning/approximate_dp_tests/pendulum_cost2go_guassian_approx_test.py deleted file mode 100644 index cae2a367..00000000 --- a/dev/reinforcement_learning/approximate_dp_tests/pendulum_cost2go_guassian_approx_test.py +++ /dev/null @@ -1,92 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.dynamic import massspringdamper -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming - -from scipy.linalg import solve_continuous_are - -from functionapproximation import GaussianFunctionApproximator -from functionapproximation import MultipleGaussianFunctionApproximator - -sys = pendulum.SinglePendulum() - -# ============================================================================= -# sys.x_ub[0] = 10.0 -# sys.x_lb[0] = -10.0 -# sys.x_lb[1] = -5.0 -# sys.x_ub[1] = 5.0 -# sys.u_ub[0] = 5.0 -# sys.u_lb[0] = -5.0 -# ============================================================================= - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [101,101] , [3] , 0.05) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.INF = 200 - -#################### -# Value iteration solution -#################### - -dp = dynamicprogramming.DynamicProgrammingWithLookUpTable( grid_sys, qcf) - -dp.solve_bellman_equation( tol = 0.5 ) - -#ctl = dp.get_lookup_table_controller() - -#dp.plot_cost2go() -dp.plot_cost2go_3D() - - - -#################### -# Gaussian Approx -#################### - -# ============================================================================= -# qfa1 = GaussianFunctionApproximator( x0 = np.array([0,0]) ) -# qfa2 = GaussianFunctionApproximator( x0 = qcf.xbar ) -# qfa3 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([1,0]) ) -# qfa4 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([1,1]) ) -# qfa5 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0,1]) ) -# qfa6 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([-1,0]) ) -# qfa7 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([-1,-1]) ) -# qfa8 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0,-1]) ) -# qfa9 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0.5,0]) ) -# qfa10 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0.5,0.5]) ) -# qfa11 = GaussianFunctionApproximator( x0 = qcf.xbar + np.array([0,0.5]) ) -# -# qfa = qfa1 + qfa2 + qfa3 + qfa4 + qfa5 + qfa6 + qfa7 + qfa8 + qfa9 + qfa10 + qfa11 -# ============================================================================= - - -# Discrete world -grid_sys_gaussian = discretizer.GridDynamicSystem( sys , [11,11] , [3] , 0.05) -X0 = grid_sys_gaussian.state_from_node_id - -qfa = MultipleGaussianFunctionApproximator( X0 ) - -Xs = grid_sys.state_from_node_id # All state on the grid - -P = qfa.compute_all_kernel( Xs ) - -w , J_hat = qfa.least_square_fit( dp.J , P ) - -grid_sys.plot_grid_value_3D( J_hat , None , 'Gaussian approx') - -grid_sys.plot_grid_value_3D( dp.J , J_hat , 'J vs. J_hat') diff --git a/dev/reinforcement_learning/approximate_dp_tests/pendulum_cost2go_quadratic_approx.py b/dev/reinforcement_learning/approximate_dp_tests/pendulum_cost2go_quadratic_approx.py deleted file mode 100644 index 9452dd71..00000000 --- a/dev/reinforcement_learning/approximate_dp_tests/pendulum_cost2go_quadratic_approx.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.dynamic import massspringdamper -from pyro.planning import discretizer -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming - -from scipy.linalg import solve_continuous_are - -from functionapproximation import QuadraticFunctionApproximator - -sys = pendulum.SinglePendulum() - -# ============================================================================= -# sys.x_ub[0] = 10.0 -# sys.x_lb[0] = -10.0 -# sys.x_lb[1] = -5.0 -# sys.x_ub[1] = 5.0 -# sys.u_ub[0] = 5.0 -# sys.u_lb[0] = -5.0 -# ============================================================================= - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [101,101] , [3] , 0.05) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.INF = 200 - -#################### -# Value iteration solution -#################### - -dp = dynamicprogramming.DynamicProgrammingWithLookUpTable( grid_sys, qcf) - -dp.solve_bellman_equation( tol = 0.5 ) - -#ctl = dp.get_lookup_table_controller() - -#dp.plot_cost2go() -#dp.plot_cost2go_3D() - -#################### -# Quadratic Approx -#################### - -qfa = QuadraticFunctionApproximator( sys.n , x0 = qcf.xbar ) - -Xs = grid_sys.state_from_node_id # All state on the grid - -P = qfa.compute_all_kernel( Xs ) - -w , J_hat = qfa.least_square_fit( dp.J , P ) - -#grid_sys.plot_grid_value_3D( J_hat , None , 'Quadratic approx') - -grid_sys.plot_grid_value_3D( dp.J , J_hat , 'J vs. J_hat') - diff --git a/dev/reinforcement_learning/dp_tests/dev_test_double_pendulum.py b/dev/reinforcement_learning/dp_tests/dev_test_double_pendulum.py deleted file mode 100644 index 2504db26..00000000 --- a/dev/reinforcement_learning/dp_tests/dev_test_double_pendulum.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -Note: Is this exemple working or a work in progress??? - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.control import controller -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming -from pyro.planning import discretizer - -sys = pendulum.DoublePendulum() - -sys.I1 = 5 -sys.I2 = 5 - -dx1 = 4.0 -dx2 = 4.0 -ddx1 = 5.0 -ddx2 = 5.0 - -sys.x_ub = np.array([ dx1 , dx2, ddx1 , ddx2 ]) -sys.x_lb = np.array([ -dx1 , -dx2, -ddx1 , -ddx2 ]) - -sys.u_ub = np.array([ 10.0 , 5.0 ]) -sys.u_lb = np.array([ -10.0 , -5.0 ]) - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [41,41,21,21] , [3,3] , 0.2 ) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ 0 , 0, 0 , 0 ]) # target -qcf.INF = 100000 -qcf.EPS = 0.2 - -qcf.Q[0,0] = 0.1 -qcf.Q[1,1] = 0.1 -qcf.Q[2,2] = 0.1 -qcf.Q[3,3] = 0.1 - -qcf.S[0,0] = 1 -qcf.S[1,1] = 1 -qcf.S[2,2] = 1 -qcf.S[3,3] = 1 - - -# DP algo -dp = dynamicprogramming.DynamicProgrammingWithLookUpTable( grid_sys, qcf ) - -#dp.plot_cost2go() -#dp.compute_steps( 10 , animate_policy = True ) -dp.compute_steps(100) - - -ctl = dp.get_lookup_table_controller() - - - -#asign controller -cl_sys = controller.ClosedLoopSystem( sys , ctl ) - -############################################################################## - -# Simulation and animation -cl_sys.x0 = np.array([0,0.1,0,0.0]) -cl_sys.compute_trajectory( 10, 10001, 'euler') -cl_sys.plot_trajectory('xu') -cl_sys.plot_phase_plane_trajectory() -cl_sys.animate_simulation() diff --git a/dev/reinforcement_learning/dp_tests/test_hidef.py b/dev/reinforcement_learning/dp_tests/test_hidef.py deleted file mode 100644 index 5941e9e9..00000000 --- a/dev/reinforcement_learning/dp_tests/test_hidef.py +++ /dev/null @@ -1,61 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.control import controller -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming -from pyro.planning import discretizer - -sys = pendulum.SinglePendulum() - -sys.x_ub[0] = +2.1 -sys.x_lb[0] = -3.3 -sys.x_ub[1] = +4.0 -sys.x_lb[1] = -5.0 - -sys.u_ub[0] = +2.5 -sys.u_lb[0] = -2.5 - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [401,401] , [31] ) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.R[0,0] = 10 -qcf.INF = 300 - -qcf.S[0,0] = 10.0 -qcf.S[1,1] = 10.0 - -# DP algo - -dp = dynamicprogramming.DynamicProgrammingWithLookUpTable( grid_sys, qcf) - -dp.solve_bellman_equation( tol = 0.1 ) - -dp.plot_cost2go() -dp.plot_policy() - - -#asign controller -ctl = dp.get_lookup_table_controller() -cl_sys = controller.ClosedLoopSystem( sys , ctl ) - -############################################################################## - -# Simulation and animation -cl_sys.x0 = np.array([0,0]) -cl_sys.compute_trajectory( 30, 10001, 'euler') -cl_sys.plot_trajectory('xu') -cl_sys.plot_phase_plane_trajectory() -cl_sys.animate_simulation() \ No newline at end of file diff --git a/dev/reinforcement_learning/dp_tests/test_lowdef.py b/dev/reinforcement_learning/dp_tests/test_lowdef.py deleted file mode 100644 index e90e4667..00000000 --- a/dev/reinforcement_learning/dp_tests/test_lowdef.py +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.control import controller -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming -from pyro.planning import discretizer - -sys = pendulum.SinglePendulum() - -sys.x_ub[0] = +4.0 -sys.x_lb[0] = -4.0 -sys.x_ub[1] = +5.0 -sys.x_lb[1] = -5.0 - -#sys.u_ub[0] = +5.0 -#sys.u_lb[0] = -5.0 - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [41,21] , [3] , dt = 0.2 ) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.INF = 300 -qcf.EPS = 1.0 - -# DP algo - -dp = dynamicprogramming.DynamicProgrammingWithLookUpTable( grid_sys, qcf) - -dp.solve_bellman_equation( tol = 1.0 ) - -dp.plot_cost2go() -dp.plot_policy() - - -#asign controller -ctl = dp.get_lookup_table_controller() -cl_sys = controller.ClosedLoopSystem( sys , ctl ) - -############################################################################## - -# Simulation and animation -cl_sys.x0 = np.array([0,0]) -cl_sys.compute_trajectory( 30, 10001, 'euler') -cl_sys.plot_trajectory('xu') -cl_sys.plot_phase_plane_trajectory() -cl_sys.animate_simulation() \ No newline at end of file diff --git a/dev/reinforcement_learning/dp_tests/test_policy_evaluator.py b/dev/reinforcement_learning/dp_tests/test_policy_evaluator.py deleted file mode 100644 index 49862bc5..00000000 --- a/dev/reinforcement_learning/dp_tests/test_policy_evaluator.py +++ /dev/null @@ -1,56 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming -from pyro.planning import discretizer - -sys = pendulum.SinglePendulum() - -sys.x_ub = np.array([+6, +6]) -sys.x_lb = np.array([-9, -6]) - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [101,101] , [11] ) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.INF = 300 - -qcf.S[0,0] = 10.0 -qcf.S[1,1] = 10.0 - - -# DP algo -dp = dynamicprogramming .DynamicProgrammingWithLookUpTable( grid_sys, qcf) - -dp.solve_bellman_equation( tol = 0.1 ) -dp.plot_cost2go() - -# Get optimal ctl -ctl = dp.get_lookup_table_controller() - - -# Evaluate on same grid -#evaluator = dprog.PolicyEvaluator(ctl, grid_sys, qcf) -#evaluator.solve_bellman_equation() -#evaluator.plot_cost2go() - - -# Evaluate on new grid -grid_sys2 = discretizer.GridDynamicSystem( sys , [151,151] , [11] ) - -#evaluator2 = dynamicprogramming.PolicyEvaluatorWithLookUpTable( ctl, grid_sys2, qcf) -evaluator2 = dynamicprogramming.PolicyEvaluator(ctl, grid_sys2, qcf) -evaluator2.solve_bellman_equation() -evaluator2.plot_cost2go() \ No newline at end of file diff --git a/dev/reinforcement_learning/dp_tests/test_policy_evaluator_with_lookuptable.py b/dev/reinforcement_learning/dp_tests/test_policy_evaluator_with_lookuptable.py deleted file mode 100644 index 45a69709..00000000 --- a/dev/reinforcement_learning/dp_tests/test_policy_evaluator_with_lookuptable.py +++ /dev/null @@ -1,68 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.control import controller -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming -from pyro.planning import discretizer - -sys = pendulum.SinglePendulum() - -sys.x_ub = np.array([+6, +6]) -sys.x_lb = np.array([-9, -6]) - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [101,101] , [11] ) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.INF = 300 - -qcf.S[0,0] = 10.0 -qcf.S[1,1] = 10.0 - - -# DP algo -dp = dynamicprogramming.DynamicProgrammingWithLookUpTable( grid_sys, qcf) - -dp.solve_bellman_equation( tol = 0.1 ) -dp.plot_cost2go() - -# Get optimal ctl -ctl = dp.get_lookup_table_controller() - - -# Evaluate on same grid -#evaluator = dprog.PolicyEvaluatorWithLookUpTable(ctl, grid_sys, qcf) -#evaluator.solve_bellman_equation() -#evaluator.plot_cost2go() - -# Evaluate on new grid -grid_sys2 = discretizer.GridDynamicSystem( sys , [301,301] , [11] , ) - -evaluator2 = dynamicprogramming.PolicyEvaluatorWithLookUpTable(ctl, grid_sys2, qcf) -evaluator2.solve_bellman_equation() -evaluator2.plot_cost2go() - - -# Evaluate on new grid - -# Raise the max torque to avoid hitting the min-max boundary with interpolation -sys.u_ub[0] = +10 -sys.u_lb[0] = -10 - -grid_sys2 = discretizer.GridDynamicSystem( sys , [301,301] , [11] ) - -evaluator2 = dynamicprogramming.PolicyEvaluatorWithLookUpTable(ctl, grid_sys2, qcf) -evaluator2.solve_bellman_equation() -evaluator2.plot_cost2go() diff --git a/dev/reinforcement_learning/dp_tests/test_speed.py b/dev/reinforcement_learning/dp_tests/test_speed.py deleted file mode 100644 index 88eca689..00000000 --- a/dev/reinforcement_learning/dp_tests/test_speed.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.control import controller -from pyro.planning import dynamicprogramming as dp -from pyro.planning import discretizer -from pyro.analysis import costfunction - -sys = pendulum.SinglePendulum() - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [51,51] , [11] ) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.INF = 1000000 - - -# DP algo -dp1 = dp.DynamicProgramming( grid_sys, qcf ) -dp1.compute_steps(10) - -dp2 = dp.DynamicProgrammingWithLookUpTable( grid_sys, qcf) -dp2.compute_steps(10) - -dp4 = dp.DynamicProgramming2DRectBivariateSpline(grid_sys, qcf) -dp4.compute_steps(10) \ No newline at end of file diff --git a/dev/reinforcement_learning/dp_tests/validation_double_pendulum_local_controller.py b/dev/reinforcement_learning/dp_tests/validation_double_pendulum_local_controller.py deleted file mode 100644 index 6c1993f5..00000000 --- a/dev/reinforcement_learning/dp_tests/validation_double_pendulum_local_controller.py +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -Note: Proof of concept with local convergence - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.control import controller -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming -from pyro.planning import discretizer - -sys = pendulum.DoublePendulum() - -sys.I1 = 5 -sys.I2 = 5 - -dx1 = 2.0 -dx2 = 2.0 -ddx1 = 3.0 -ddx2 = 6.0 - -sys.x_ub = np.array([ dx1 , dx2, ddx1 , ddx2 ]) -sys.x_lb = np.array([ -dx1 , -dx2, -ddx1 , -ddx2 ]) - -sys.u_ub = np.array([ 10.0 , 5.0 ]) -sys.u_lb = np.array([ -10.0 , -5.0 ]) - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [31,31,31,31] , [3,3] , 0.05 ) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ 0 , 0, 0 , 0 ]) # target -qcf.INF = 1000 -qcf.EPS = 0.2 - -qcf.Q[0,0] = 0.1 -qcf.Q[1,1] = 0.1 -qcf.Q[2,2] = 0.1 -qcf.Q[3,3] = 0.1 - -qcf.S[0,0] = 100 -qcf.S[1,1] = 100 -qcf.S[2,2] = 100 -qcf.S[3,3] = 100 - - -# DP algo -dp = dynamicprogramming.DynamicProgrammingWithLookUpTable( grid_sys, qcf ) - -dp.plot_cost2go() -#dp.compute_steps( 10 , animate_policy = True ) -dp.compute_steps( 200 ) - - -ctl = dp.get_lookup_table_controller() - - - -#asign controller -cl_sys = controller.ClosedLoopSystem( sys , ctl ) - -############################################################################## - -# Simulation and animation -cl_sys.x0 = np.array([-0.5,0.5,0,0.0]) -cl_sys.compute_trajectory( 10, 10001, 'euler') -cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation() diff --git a/dev/reinforcement_learning/dp_tests/validation_dp_4D_with_two_pendulum.py b/dev/reinforcement_learning/dp_tests/validation_dp_4D_with_two_pendulum.py deleted file mode 100644 index 8035957d..00000000 --- a/dev/reinforcement_learning/dp_tests/validation_dp_4D_with_two_pendulum.py +++ /dev/null @@ -1,57 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -Note: Is this exemple working or a work in progress??? - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.control import controller -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming -from pyro.planning import discretizer - -sys = pendulum.TwoIndependentSinglePendulum() - -sys.x_ub = np.array([ 4.0 , 4.0, 5.0 , 5.0 ]) -sys.x_lb = np.array([ -4.0 , -4.0, -5.0 , -5.0 ]) - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [41,41,21,21] , [3,3] , 0.2 ) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , -3.14, 0 , 0 ]) # target -qcf.INF = 1000000 -qcf.EPS = 2.0 - - -# DP algo -dp = dynamicprogramming.DynamicProgrammingWithLookUpTable( grid_sys, qcf) -#dp = dynamicprogramming .DynamicProgrammingFast2DGrid(grid_sys, qcf) - - -#dp.interpol_method = 'nearest' #12 sec -#dp.interpol_method = 'linear' #18 sec -#dp.interpol_method = 'linear' # - -#dp.plot_dynamic_cost2go = False -#dp.compute_steps(1,True) -dp.compute_steps(80) -#dp.save_latest('test4d_2') - -ctl = dp.get_lookup_table_controller() - -cl_sys = ctl + sys - -cl_sys.x0 = np.array([-0.5,0.1,0,0]) -cl_sys.compute_trajectory( 30, 10001, 'euler') -cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation() - diff --git a/dev/reinforcement_learning/pytorch_tests/pytorch_nn_test.py b/dev/reinforcement_learning/pytorch_tests/pytorch_nn_test.py deleted file mode 100644 index 854add62..00000000 --- a/dev/reinforcement_learning/pytorch_tests/pytorch_nn_test.py +++ /dev/null @@ -1,100 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Mon Apr 3 21:52:17 2023 - -@author: alex -""" - -import torch -import math - -import numpy as np -import matplotlib -import matplotlib.pyplot as plt - - -# Get cpu or gpu device for training. -device = "cuda" if torch.cuda.is_available() else "mps" if torch.backends.mps.is_available() else "cpu" -print(f"Using {device} device") - -show = True - - -class NN(torch.nn.Module): - def __init__(self): - """ - In the constructor we instantiate four parameters and assign them as - member parameters. - """ - super().__init__() - self.linear_relu_stack = torch.nn.Sequential( - torch.nn.Linear(1, 20), - torch.nn.ReLU(), - torch.nn.Linear(20, 20), - torch.nn.ReLU(), - torch.nn.Linear(20, 1) - ) - - self.test_layer = torch.nn.Linear(1, 1) - - def forward(self, x): - - return self.linear_relu_stack( x ) - - - -# Create Tensors to hold input and outputs. -x = torch.linspace(-math.pi*2.0, math.pi*2.0, 4000).reshape(-1,1) -y = torch.sin(x).reshape(-1,1) - -fig = plt.figure(figsize=(4,3), dpi=300) -ax = fig.add_subplot(111, autoscale_on=False ) -ax.grid(True) -ax.tick_params( labelsize = 5) -ax.set_xlim( [-4,4] ) -ax.set_ylim( [-1,1] ) -line1 = ax.plot( x, y, '-b' ) - -if show: - fig.show() - plt.ion() - plt.pause( 0.001 ) - - -# Construct our model by instantiating the class defined above -#model = Polynomial3() -model = NN() - -y_hat = model( x ) - -line2 = ax.plot( x, y_hat.detach().numpy() , '-r' ) - -# Construct our loss function and an Optimizer. The call to model.parameters() -# in the SGD constructor will contain the learnable parameters (defined -# with torch.nn.Parameter) which are members of the model. -criterion = torch.nn.MSELoss(reduction='sum') -optimizer = torch.optim.SGD(model.parameters(), lr=1e-5) -for t in range(1000): - # Forward pass: Compute predicted y by passing x to the model - y_hat = model(x) - - # Compute and print loss - loss = criterion(y_hat, y) - - # Zero gradients, perform a backward pass, and update the weights. - optimizer.zero_grad() - loss.backward() - optimizer.step() - - if t % 100 == 99: - print(t, loss.item()) - if show: - line2[0].set_data( x, y_hat.detach().numpy() ) - plt.pause( 0.001 ) - - - - - - diff --git a/dev/reinforcement_learning/pytorch_tests/pytorch_polynomial_test.py b/dev/reinforcement_learning/pytorch_tests/pytorch_polynomial_test.py deleted file mode 100644 index e0011c72..00000000 --- a/dev/reinforcement_learning/pytorch_tests/pytorch_polynomial_test.py +++ /dev/null @@ -1,107 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Mon Apr 3 21:52:17 2023 - -@author: alex -""" - -import torch -import math - -import numpy as np -import matplotlib -import matplotlib.pyplot as plt - - -# Get cpu or gpu device for training. -device = "cuda" if torch.cuda.is_available() else "mps" if torch.backends.mps.is_available() else "cpu" -print(f"Using {device} device") - -show = True - - -class Polynomial3(torch.nn.Module): - def __init__(self): - """ - In the constructor we instantiate four parameters and assign them as - member parameters. - """ - super().__init__() - self.a = torch.nn.Parameter(torch.randn(())) - self.b = torch.nn.Parameter(torch.randn(())) - self.c = torch.nn.Parameter(torch.randn(())) - self.d = torch.nn.Parameter(torch.randn(())) - - def forward(self, x): - """ - In the forward function we accept a Tensor of input data and we must return - a Tensor of output data. We can use Modules defined in the constructor as - well as arbitrary operators on Tensors. - """ - return self.a + self.b * x + self.c * x ** 2 + self.d * x ** 3 - - def string(self): - """ - Just like any class in Python, you can also define custom method on PyTorch modules - """ - return f'y = {self.a.item()} + {self.b.item()} x + {self.c.item()} x^2 + {self.d.item()} x^3' - - - - - -# Create Tensors to hold input and outputs. -x = torch.linspace(-math.pi*1.0, math.pi*1.0, 4000).reshape(-1,1) -y = torch.sin(x).reshape(-1,1) - -fig = plt.figure(figsize=(4,3), dpi=300) -ax = fig.add_subplot(111, autoscale_on=False ) -ax.grid(True) -ax.tick_params( labelsize = 5) -ax.set_xlim( [-4,4] ) -ax.set_ylim( [-1,1] ) -line1 = ax.plot( x, y, '-b' ) - -if show: - fig.show() - plt.ion() - plt.pause( 0.001 ) - - -# Construct our model by instantiating the class defined above -model = Polynomial3() - -y_hat = model( x ) - -line2 = ax.plot( x, y_hat.detach().numpy() , '-r' ) - -criterion = torch.nn.MSELoss(reduction='sum') -optimizer = torch.optim.SGD(model.parameters(), lr=1e-6) - -for t in range(2000): - # Forward pass: Compute predicted y by passing x to the model - y_hat = model(x) - - # Compute and print loss - loss = criterion(y_hat, y) - - # Zero gradients, perform a backward pass, and update the weights. - optimizer.zero_grad() - loss.backward() - optimizer.step() - - if t % 100 == 99: - print(t, loss.item()) - if show: - line2[0].set_data( x, y_hat.detach().numpy() ) - plt.pause( 0.001 ) - - - -print(f'Result: {model.string()}') - - - - - diff --git a/dev/reinforcement_learning/pytorch_tests/pytorch_polynomial_vs_nn_fit.py b/dev/reinforcement_learning/pytorch_tests/pytorch_polynomial_vs_nn_fit.py deleted file mode 100644 index 966f6ed4..00000000 --- a/dev/reinforcement_learning/pytorch_tests/pytorch_polynomial_vs_nn_fit.py +++ /dev/null @@ -1,143 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Mon Apr 3 21:52:17 2023 - -@author: alex -""" - -import torch -import math - -import numpy as np -import matplotlib -import matplotlib.pyplot as plt - - -# Get cpu or gpu device for training. -device = "cuda" if torch.cuda.is_available() else "mps" if torch.backends.mps.is_available() else "cpu" -print(f"Using {device} device") - -show = True - - -class Polynomial3(torch.nn.Module): - def __init__(self): - """ - In the constructor we instantiate four parameters and assign them as - member parameters. - """ - super().__init__() - self.a = torch.nn.Parameter(torch.randn(())) - self.b = torch.nn.Parameter(torch.randn(())) - self.c = torch.nn.Parameter(torch.randn(())) - self.d = torch.nn.Parameter(torch.randn(())) - #self.e = torch.nn.Parameter(torch.randn(())) - #self.f = torch.nn.Parameter(torch.randn(())) - #self.g = torch.nn.Parameter(torch.randn(())) - - def forward(self, x): - """ - In the forward function we accept a Tensor of input data and we must return - a Tensor of output data. We can use Modules defined in the constructor as - well as arbitrary operators on Tensors. - """ - out = ( self.a + self.b * x + self.c * x ** 2 + self.d * x ** 3 ) - #+ self.e * x ** 4 + self.f * x ** 5 + self.g * x ** 6 ) - - return out - - def string(self): - """ - Just like any class in Python, you can also define custom method on PyTorch modules - """ - return f'y = {self.a.item()} + {self.b.item()} x + {self.c.item()} x^2 + {self.d.item()} x^3' - - -class NN(torch.nn.Module): - def __init__(self): - """ - In the constructor we instantiate four parameters and assign them as - member parameters. - """ - super().__init__() - self.linear_relu_stack = torch.nn.Sequential( - torch.nn.Linear(1, 50), - torch.nn.ReLU(), - torch.nn.Linear(50, 20), - torch.nn.ReLU(), - torch.nn.Linear(20, 1) - ) - - def forward(self, x): - - return self.linear_relu_stack( x ) - - - - -# Create Tensors to hold input and outputs. -period = 1 -x = torch.linspace(-math.pi*period, math.pi*period, 4000).reshape(-1,1) -y = torch.sin(x).reshape(-1,1) - -fig = plt.figure(figsize=(4,3), dpi=300) -ax = fig.add_subplot(111, autoscale_on=False ) -ax.grid(True) -ax.tick_params( labelsize = 5) -ax.set_xlim( [-math.pi*period,math.pi*period] ) -ax.set_ylim( [-1,1] ) -line1 = ax.plot( x, y, '-b' ) - -if show: - fig.show() - plt.ion() - plt.pause( 0.001 ) - - -# Construct our model by instantiating the class defined above -model = Polynomial3() -model2 = NN() - -y_hat = model( x ) -y_hat2 = model2( x ) - -line2 = ax.plot( x, y_hat.detach().numpy() , '-r' ) -line3 = ax.plot( x, y_hat2.detach().numpy() , '--g' ) - -criterion = torch.nn.MSELoss(reduction='sum') -criterion2 = torch.nn.MSELoss(reduction='sum') - -optimizer = torch.optim.SGD(model.parameters(), lr=1e-6) -#optimizer2 = torch.optim.SGD(model2.parameters(), lr=1e-6) -optimizer2 = torch.optim.Adam( model2.parameters(), lr=1e-4 ) - -for t in range(5000): - # Forward pass: Compute predicted y by passing x to the model - y_hat = model(x) - y_hat2 = model2(x) - - # Compute and print loss - loss = criterion(y_hat, y) - loss2 = criterion2(y_hat2, y) - - # Zero gradients, perform a backward pass, and update the weights. - optimizer.zero_grad() - loss.backward() - optimizer.step() - - optimizer2.zero_grad() - loss2.backward() - optimizer2.step() - - if t % 100 == 99: - print(t, loss.item()) - if show: - line2[0].set_data( x, y_hat.detach().numpy() ) - line3[0].set_data( x, y_hat2.detach().numpy() ) - plt.pause( 0.001 ) - - - - - diff --git a/dev/reinforcement_learning/pytorch_tests/pytorch_rl_demo.py b/dev/reinforcement_learning/pytorch_tests/pytorch_rl_demo.py deleted file mode 100644 index 2cc18de6..00000000 --- a/dev/reinforcement_learning/pytorch_tests/pytorch_rl_demo.py +++ /dev/null @@ -1,258 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Tue Apr 4 14:14:37 2023 - -@author: alex -""" - -#import gymnasium as gym -import math -import random -import matplotlib -import numpy as np -import matplotlib.pyplot as plt -from collections import namedtuple, deque -from itertools import count - -import torch -import torch.nn as nn -import torch.optim as optim -import torch.nn.functional as F - - -from pyro.dynamic import cartpole - -sys = cartpole.CartPole() -sys.xbar[1] = np.pi # Up-right position -#env = gym.make("CartPole-v1") - -# set up matplotlib -plt.ion() - -# if gpu is to be used -device = "cuda" if torch.cuda.is_available() else "mps" if torch.backends.mps.is_available() else "cpu" - -device = 'cpu' - -Transition = namedtuple('Transition', - ('state', 'action', 'next_state', 'reward')) - - -class ReplayMemory(object): - - def __init__(self, capacity): - self.memory = deque([], maxlen=capacity) - - def push(self, *args): - """Save a transition""" - self.memory.append(Transition(*args)) - - def sample(self, batch_size): - return random.sample(self.memory, batch_size) - - def __len__(self): - return len(self.memory) - - -class DQN(nn.Module): - - def __init__(self, n_observations, n_actions): - super(DQN, self).__init__() - self.layer1 = nn.Linear(n_observations, 128) - self.layer2 = nn.Linear(128, 128) - self.layer3 = nn.Linear(128, n_actions) - - # Called with either one element to determine next action, or a batch - # during optimization. Returns tensor([[left0exp,right0exp]...]). - def forward(self, x): - x = F.relu(self.layer1(x)) - x = F.relu(self.layer2(x)) - return self.layer3(x) - - -BATCH_SIZE = 128 -GAMMA = 0.99 -EPS_START = 0.9 -EPS_END = 0.05 -EPS_DECAY = 1000 -TAU = 0.005 -LR = 1e-4 - -# Get number of actions from gym action space -n_actions = 3 -# Get the number of state observations -n_observations = sys.n - -policy_net = DQN(n_observations, n_actions).to(device) -target_net = DQN(n_observations, n_actions).to(device) -target_net.load_state_dict(policy_net.state_dict()) - -optimizer = optim.AdamW(policy_net.parameters(), lr=LR, amsgrad=True) -memory = ReplayMemory(10000) - - -steps_done = 0 - - -def select_action(state): - global steps_done - sample = random.random() - eps_threshold = EPS_END + (EPS_START - EPS_END) * \ - math.exp(-1. * steps_done / EPS_DECAY) - steps_done += 1 - if sample > eps_threshold: - with torch.no_grad(): - # t.max(1) will return the largest column value of each row. - # second column on max result is index of where max element was - # found, so we pick action with the larger expected reward. - return policy_net(state).max(1)[1].view(1, 1) - else: - return torch.tensor([[ int(np.random.uniform( 0 , 3)) ]], device=device, dtype=torch.long) - - -episode_durations = [] - - -def plot_durations(show_result=False): - plt.figure(1) - durations_t = torch.tensor(episode_durations, dtype=torch.float) - if show_result: - plt.title('Result') - else: - plt.clf() - plt.title('Training...') - plt.xlabel('Episode') - plt.ylabel('Duration') - plt.plot(durations_t.numpy()) - # Take 100 episode averages and plot them too - if len(durations_t) >= 100: - means = durations_t.unfold(0, 100, 1).mean(1).view(-1) - means = torch.cat((torch.zeros(99), means)) - plt.plot(means.numpy()) - - plt.pause(0.001) # pause a bit so that plots are updated - - -def optimize_model(): - if len(memory) < BATCH_SIZE: - return - transitions = memory.sample(BATCH_SIZE) - # Transpose the batch (see https://stackoverflow.com/a/19343/3343043 for - # detailed explanation). This converts batch-array of Transitions - # to Transition of batch-arrays. - batch = Transition(*zip(*transitions)) - - # Compute a mask of non-final states and concatenate the batch elements - # (a final state would've been the one after which simulation ended) - non_final_mask = torch.tensor(tuple(map(lambda s: s is not None, - batch.next_state)), device=device, dtype=torch.bool) - non_final_next_states = torch.cat([s for s in batch.next_state - if s is not None]) - state_batch = torch.cat(batch.state) - action_batch = torch.cat(batch.action) - reward_batch = torch.cat(batch.reward) - - # Compute Q(s_t, a) - the model computes Q(s_t), then we select the - # columns of actions taken. These are the actions which would've been taken - # for each batch state according to policy_net - state_action_values = policy_net(state_batch).gather(1, action_batch) - - # Compute V(s_{t+1}) for all next states. - # Expected values of actions for non_final_next_states are computed based - # on the "older" target_net; selecting their best reward with max(1)[0]. - # This is merged based on the mask, such that we'll have either the expected - # state value or 0 in case the state was final. - next_state_values = torch.zeros(BATCH_SIZE, device=device) - with torch.no_grad(): - next_state_values[non_final_mask] = target_net(non_final_next_states).max(1)[0] - # Compute the expected Q values - expected_state_action_values = (next_state_values * GAMMA) + reward_batch - - # Compute Huber loss - criterion = nn.SmoothL1Loss() - loss = criterion(state_action_values, expected_state_action_values.unsqueeze(1)) - - # Optimize the model - optimizer.zero_grad() - loss.backward() - # In-place gradient clipping - torch.nn.utils.clip_grad_value_(policy_net.parameters(), 100) - optimizer.step() - - - -num_episodes = 600 - -for i_episode in range(num_episodes): - # Initialize the environment and get it's state - - start = sys.x0 - - start[1] = np.random.uniform( 1, -4 ) - - state = start - - state = torch.tensor(state, dtype=torch.float32, device=device).unsqueeze(0) - - x = state.numpy()[0] - - for t in count(): - - action = select_action( state ) - - if action == 0: - - u = np.array([ 20. ]) - - elif action ==1: - - u = np.array([ -20 ]) - - else: - - u = np.array([ 0. ]) - - dx = sys.f( x , u ) - - x_next = dx * 0.1 + x - - reward = -sys.cost_function.g( x , u , 0 ) - observation = x_next - truncated = (not sys.isavalidstate( x_next ) ) or ( t > 199 ) - terminated = np.linalg.norm(sys.xbar - sys.x0) < 0.5 - - reward = torch.tensor([reward], device=device) - done = terminated or truncated - - if terminated: - next_state = None - else: - next_state = torch.tensor(observation, dtype=torch.float32, device=device).unsqueeze(0) - - # Store the transition in memory - memory.push(state, action, next_state, reward) - - # Move to the next state - state = next_state - - # Perform one step of the optimization (on the policy network) - optimize_model() - - # Soft update of the target network's weights - # θ′ ← τ θ + (1 −τ )θ′ - target_net_state_dict = target_net.state_dict() - policy_net_state_dict = policy_net.state_dict() - for key in policy_net_state_dict: - target_net_state_dict[key] = policy_net_state_dict[key]*TAU + target_net_state_dict[key]*(1-TAU) - target_net.load_state_dict(target_net_state_dict) - - if done: - episode_durations.append(t + 1) - plot_durations() - break - -print('Complete') -plot_durations(show_result=True) -plt.ioff() -plt.show() \ No newline at end of file diff --git a/dev/reinforcement_learning/pytorch_tests/pytorch_rl_demo_2.py b/dev/reinforcement_learning/pytorch_tests/pytorch_rl_demo_2.py deleted file mode 100644 index 10d98fa5..00000000 --- a/dev/reinforcement_learning/pytorch_tests/pytorch_rl_demo_2.py +++ /dev/null @@ -1,258 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Tue Apr 4 14:14:37 2023 - -@author: alex -""" - -#import gymnasium as gym -import math -import random -import matplotlib -import numpy as np -import matplotlib.pyplot as plt -from collections import namedtuple, deque -from itertools import count - -import torch -import torch.nn as nn -import torch.optim as optim -import torch.nn.functional as F - - -from pyro.dynamic import pendulum - -sys = pendulum.SinglePendulum() #TODO -sys.xbar[0] = 3.14 -#env = gym.make("CartPole-v1") - -# set up matplotlib -plt.ion() - -# if gpu is to be used -device = "cuda" if torch.cuda.is_available() else "mps" if torch.backends.mps.is_available() else "cpu" - -device = 'cpu' - -Transition = namedtuple('Transition', - ('state', 'action', 'next_state', 'reward')) - - -class ReplayMemory(object): - - def __init__(self, capacity): - self.memory = deque([], maxlen=capacity) - - def push(self, *args): - """Save a transition""" - self.memory.append(Transition(*args)) - - def sample(self, batch_size): - return random.sample(self.memory, batch_size) - - def __len__(self): - return len(self.memory) - - -class DQN(nn.Module): - - def __init__(self, n_observations, n_actions): - super(DQN, self).__init__() - self.layer1 = nn.Linear(n_observations, 128) - self.layer2 = nn.Linear(128, 128) - self.layer3 = nn.Linear(128, n_actions) - - # Called with either one element to determine next action, or a batch - # during optimization. Returns tensor([[left0exp,right0exp]...]). - def forward(self, x): - x = F.relu(self.layer1(x)) - x = F.relu(self.layer2(x)) - return self.layer3(x) - - -BATCH_SIZE = 128 -GAMMA = 0.99 -EPS_START = 0.9 -EPS_END = 0.05 -EPS_DECAY = 1000 -TAU = 0.005 -LR = 1e-4 - -# Get number of actions from gym action space -n_actions = 3 -# Get the number of state observations -n_observations = sys.n - -policy_net = DQN(n_observations, n_actions).to(device) -target_net = DQN(n_observations, n_actions).to(device) -target_net.load_state_dict(policy_net.state_dict()) - -optimizer = optim.AdamW(policy_net.parameters(), lr=LR, amsgrad=True) -memory = ReplayMemory(10000) - - -steps_done = 0 - - -def select_action(state): - global steps_done - sample = random.random() - eps_threshold = EPS_END + (EPS_START - EPS_END) * \ - math.exp(-1. * steps_done / EPS_DECAY) - steps_done += 1 - if sample > eps_threshold: - with torch.no_grad(): - # t.max(1) will return the largest column value of each row. - # second column on max result is index of where max element was - # found, so we pick action with the larger expected reward. - return policy_net(state).max(1)[1].view(1, 1) - else: - return torch.tensor([[ int(np.random.uniform( 0 , 3)) ]], device=device, dtype=torch.long) - - -episode_durations = [] - - -def plot_durations(show_result=False): - plt.figure(1) - durations_t = torch.tensor(episode_durations, dtype=torch.float) - if show_result: - plt.title('Result') - else: - plt.clf() - plt.title('Training...') - plt.xlabel('Episode') - plt.ylabel('Duration') - plt.plot(durations_t.numpy()) - # Take 100 episode averages and plot them too - if len(durations_t) >= 100: - means = durations_t.unfold(0, 100, 1).mean(1).view(-1) - means = torch.cat((torch.zeros(99), means)) - plt.plot(means.numpy()) - - plt.pause(0.001) # pause a bit so that plots are updated - - -def optimize_model(): - if len(memory) < BATCH_SIZE: - return - transitions = memory.sample(BATCH_SIZE) - # Transpose the batch (see https://stackoverflow.com/a/19343/3343043 for - # detailed explanation). This converts batch-array of Transitions - # to Transition of batch-arrays. - batch = Transition(*zip(*transitions)) - - # Compute a mask of non-final states and concatenate the batch elements - # (a final state would've been the one after which simulation ended) - non_final_mask = torch.tensor(tuple(map(lambda s: s is not None, - batch.next_state)), device=device, dtype=torch.bool) - non_final_next_states = torch.cat([s for s in batch.next_state - if s is not None]) - state_batch = torch.cat(batch.state) - action_batch = torch.cat(batch.action) - reward_batch = torch.cat(batch.reward) - - # Compute Q(s_t, a) - the model computes Q(s_t), then we select the - # columns of actions taken. These are the actions which would've been taken - # for each batch state according to policy_net - state_action_values = policy_net(state_batch).gather(1, action_batch) - - # Compute V(s_{t+1}) for all next states. - # Expected values of actions for non_final_next_states are computed based - # on the "older" target_net; selecting their best reward with max(1)[0]. - # This is merged based on the mask, such that we'll have either the expected - # state value or 0 in case the state was final. - next_state_values = torch.zeros(BATCH_SIZE, device=device) - with torch.no_grad(): - next_state_values[non_final_mask] = target_net(non_final_next_states).max(1)[0] - # Compute the expected Q values - expected_state_action_values = (next_state_values * GAMMA) + reward_batch - - # Compute Huber loss - criterion = nn.SmoothL1Loss() - loss = criterion(state_action_values, expected_state_action_values.unsqueeze(1)) - - # Optimize the model - optimizer.zero_grad() - loss.backward() - # In-place gradient clipping - torch.nn.utils.clip_grad_value_(policy_net.parameters(), 100) - optimizer.step() - - - -num_episodes = 600 - -for i_episode in range(num_episodes): - # Initialize the environment and get it's state - - start = sys.x0 - - start[0] = np.random.uniform( 3. , -1. ) - - state = start - - state = torch.tensor(state, dtype=torch.float32, device=device).unsqueeze(0) - - x = state.numpy()[0] - - for t in count(): - - action = select_action( state ) - - if action == 0: - - u = np.array([ 10. ]) - - elif action ==1: - - u = np.array([ -10. ]) - - else: - - u = np.array([ 0. ]) - - dx = sys.f( x , u ) - - x_next = dx * 0.1 + x - - reward = 10 - sys.cost_function.g( x , u , 0 ) - observation = x_next - truncated = (not sys.isavalidstate( x_next ) ) or ( t > 99 ) - terminated = np.linalg.norm(sys.xbar - sys.x0) < 0.5 - - reward = torch.tensor([reward], device=device) - done = terminated or truncated - - if terminated: - next_state = None - else: - next_state = torch.tensor(observation, dtype=torch.float32, device=device).unsqueeze(0) - - # Store the transition in memory - memory.push(state, action, next_state, reward) - - # Move to the next state - state = next_state - - # Perform one step of the optimization (on the policy network) - optimize_model() - - # Soft update of the target network's weights - # θ′ ← τ θ + (1 −τ )θ′ - target_net_state_dict = target_net.state_dict() - policy_net_state_dict = policy_net.state_dict() - for key in policy_net_state_dict: - target_net_state_dict[key] = policy_net_state_dict[key]*TAU + target_net_state_dict[key]*(1-TAU) - target_net.load_state_dict(target_net_state_dict) - - if done: - episode_durations.append(t + 1) - plot_durations() - break - -print('Complete') -plot_durations(show_result=True) -plt.ioff() -plt.show() \ No newline at end of file diff --git a/dev/reinforcement_learning/rl_tests/pendulum_test_rl.py b/dev/reinforcement_learning/rl_tests/pendulum_test_rl.py deleted file mode 100644 index a60ee8e2..00000000 --- a/dev/reinforcement_learning/rl_tests/pendulum_test_rl.py +++ /dev/null @@ -1,50 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 16 22:27:47 2022 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic import pendulum -from pyro.control import controller -from pyro.analysis import costfunction -from pyro.planning import dynamicprogramming -from pyro.planning import discretizer - -from reinforcementlearning import QLearning - -sys = pendulum.SinglePendulum() - -sys.x_ub[0] = 4.0 -sys.x_lb[0] = -7.0 -sys.x_lb[1] = -6.0 -sys.x_ub[1] = 6.0 - -# Discrete world -grid_sys = discretizer.GridDynamicSystem( sys , [101,101] , [3] , 0.1) - -# Cost Function -qcf = costfunction.QuadraticCostFunction.from_sys(sys) - -qcf.xbar = np.array([ -3.14 , 0 ]) # target -qcf.INF = 300 - -qcf.R[0,0] = 1.0 - -qcf.S[0,0] = 10.0 -qcf.S[1,1] = 10.0 - - -# DP algo -dp = QLearning( grid_sys , qcf ) - -dp.compute_J_from_Q() - -dp.compute_policy_from_Q() - -dp.compute_episodes(1000) - -dp.plot_cost2go() \ No newline at end of file diff --git a/dev/reinforcement_learning/rl_tests/reinforcementlearning.py b/dev/reinforcement_learning/rl_tests/reinforcementlearning.py deleted file mode 100644 index 3dcfd188..00000000 --- a/dev/reinforcement_learning/rl_tests/reinforcementlearning.py +++ /dev/null @@ -1 +0,0 @@ -#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Created on Wed Oct 12 20:13:17 2022 @author: alex """ import numpy as np import matplotlib.pyplot as plt from scipy.interpolate import RectBivariateSpline as interpol2D from scipy.interpolate import RegularGridInterpolator as rgi from pyro.analysis import costfunction from pyro.planning import dynamicprogramming from pyro.planning import discretizer ################################################################## class EpsilonGreedyController( dynamicprogramming.LookUpTableController ): ############################ def __init__(self, grid_sys , pi_star , epsilon = 0.7 ): """ Pyro controller based on a discretized lookpup table of optimal control inputs where the optimal action is taken with probability espsilon, else a random action is taken. Parameters ---------- grid_sys : pyro GridDynamicSystem class A discretized dynamic system pi_star : numpy array, dim = self.grid_sys.nodes_n , dtype = int A list of optimal action index for each node id """ super().__init__( grid_sys , pi_star ) self.name = 'Epsilon Greedy Controller' self.epsilon = epsilon ############################# def c( self , y , r , t = 0 ): """ State feedback (y=x) - no reference - time independent """ x = y if np.random.uniform(0,1) < self.epsilon: # greedy behavior u = self.lookup_table_selection( x ) else: # Random exploration random_index = int(np.random.uniform( 0 , self.grid_sys.actions_n )) u = self.grid_sys.input_from_action_id[ random_index ] # TODO add domain check for random actions? return u ############################################################################### ### RL algo ############################################################################### class QLearning( dynamicprogramming.DynamicProgramming ): """ """ ############################ def __init__(self, grid_sys , cost_function ): # Dynamic system self.grid_sys = grid_sys # Discretized Dynamic system class self.sys = grid_sys.sys # Base Dynamic system class # Cost function self.cf = cost_function self.dt = self.grid_sys.dt self.t = 0 # Q Learning Parameters self.alpha = 0.99 # Discount factor self.eta = 0.8 # Learning rate self.interpol_method ='linear' # "linear”, “nearest”, “slinear”, “cubic”, and “quintic” self.initialize() # Trainer self.trajectories = [] # COntroller self.eps = 0.7 ############################## def initialize(self): """ initialize Q values """ self.J = np.zeros( self.grid_sys.nodes_n , dtype = float ) self.pi = np.zeros( self.grid_sys.nodes_n , dtype = int ) self.Q = np.zeros( ( self.grid_sys.nodes_n , self.grid_sys.actions_n ) , dtype = float ) # Initial cost-to-go evaluation for s in range( self.grid_sys.nodes_n ): x = self.grid_sys.state_from_node_id[ s , : ] # Final Cost of all states self.J[ s ] = self.cf.h( x , self.t ) # For all control actions for a in range( self.grid_sys.actions_n ): # Set all Q values to the terminal cost self.Q[ s , a ] = self.J[ s ] ############################## def compute_J_from_Q(self): """ update the J table from Q values """ for s in range( self.grid_sys.nodes_n ): self.J[ s ] = self.Q[s,:].min() # Create interpol function self.J_interpol = self.grid_sys.compute_interpolation_function( self.J , self.interpol_method , bounds_error = False , fill_value = 0 ) ############################## def compute_policy_from_Q(self): """ update the J table from Q values """ for s in range( self.grid_sys.nodes_n ): self.pi[ s ] = self.Q[s,:].argmin() ############################################### def Q_update(self, x , u , x_next ): """ """ s = self.grid_sys.get_nearest_node_id_from_state( x ) a = self.grid_sys.get_nearest_action_id_from_input( u ) if self.sys.isavalidstate( x_next ): J_next = self.J_interpol( x_next ) Q_sample = self.cf.g( x , u, self.t ) * self.dt + self.alpha * J_next else: Q_sample = self.cf.INF # Q update error = Q_sample - self.Q[ s , a ] self.Q[ s , a ] = self.Q[ s , a ] + self.eta * error ############################################### def learn_from_traj( self, traj , pass_number = 1 ): steps = traj.x.shape[0] - 1 for p in range(pass_number): self.compute_J_from_Q() for i in range(steps-1,-1,-1): x = traj.x[i,:] u = traj.u[i,:] x_next = traj.x[i+1,:] self.Q_update( x , u , x_next ) ############################################### def generate_traj( self ): self.compute_policy_from_Q() ctl = EpsilonGreedyController( self.grid_sys, self.pi , self.eps ) cl_sys = ctl + self.sys tf = 10 n = int( tf / self.dt ) cl_sys.x0 = np.random.uniform(-1,1, self.sys.n ) cl_sys.compute_trajectory( tf , n , 'euler') return cl_sys.traj ############################################### def compute_episodes( self, n = 1 ): for i in range(n): print('Episode:',i) new_traj = self.generate_traj() self.learn_from_traj( new_traj ) #self.trajectories.append(new_traj) #for traj in self.trajectories: # self.learn_from_traj( traj , 10 ) ############################################### def show_episode( self ): self.compute_policy_from_Q() ctl = EpsilonGreedyController( self.grid_sys, self.pi , 1.0 ) cl_sys = ctl + self.sys tf = 10 cl_sys.x0 = np.random.uniform(-1,1, self.sys.n ) cl_sys.compute_trajectory( tf , 10001 ) cl_sys.plot_trajectory('xu') cl_sys.animate_simulation() \ No newline at end of file diff --git a/dev/tests/old_tests/saveloadtest.py b/dev/tests/old_tests/saveloadtest.py deleted file mode 100755 index 281469ba..00000000 --- a/dev/tests/old_tests/saveloadtest.py +++ /dev/null @@ -1,18 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sat Mar 21 14:25:38 2020 - -@author: alex -""" - -import numpy as np -from pyro.dynamic import pendulum -from pyro.control import nonlinear -from pyro.analysis import simulation - -sys = pendulum.DoublePendulum() - -sys.x0 = np.array([0.0,1.,1.,0.]) -sys.compute_trajectory() - diff --git a/dev/tests/old_tests/test_all_examples.py b/dev/tests/old_tests/test_all_examples.py deleted file mode 100644 index 580f674f..00000000 --- a/dev/tests/old_tests/test_all_examples.py +++ /dev/null @@ -1,128 +0,0 @@ -""" -Run all examples - -Does not check for correct results, only that there are no exceptions. - -Running this: - -* Using pytest: `pytest -ra examples/` (replace examples with path to the - examples folder where this script lives). Pytest will produce a report - of failed/succeeded tests - -* As a script: `python ./examples/test_all_examples.py`. Will stop at the - exception. - -TODO: Add actual tests that check for correct results - -""" - -from pathlib import Path - -from importlib.util import spec_from_file_location, module_from_spec - -import sys - -import inspect - -from matplotlib import pyplot as plt - -_all_examples = [ - "./bicycle/bicycle.py", - "./bicycle/bicycle_exploration_with_rrt.py", - "./bicycle/bicycle_parallel_parking_with_rrt.py", - "./cartpole/cartpole_natural_behavior.py", - "./cartpole/cartpole_with_computed_torque.py", - "./cartpole/cartpole_with_rrt_and_computed_torque.py", - "./cartpole/underactuated_cartpole_swingup.py", - "./cartpole/underactuated_cartpole_with_partialfeedbacklinearization.py", - "./cartpole/underactuated_cartpole_with_rrt.py", - "./double_pendulum/double_pendulum.py", - "./double_pendulum/double_pendulum_with_computed_torque.py", - "./double_pendulum/double_pendulum_with_rrt.py", - "./double_pendulum/double_pendulum_with_rrt_and_computed_torque.py", - "./double_pendulum/double_pendulum_with_sliding_mode.py", - "./double_pendulum/double_pendulum_with_trajectory_following_computed_torque.py", - "./double_pendulum/double_pendulum_with_trajectory_following_open_loop_controller.py", - "./double_pendulum/double_pendulum_with_trajectory_following_sliding_mode_controller.py", - "./holonomic_mobile_robot/holonomic_mobile_robot_exploration_with_obstacles_with_rrt.py", - "./holonomic_mobile_robot/holonomic_mobile_robot_exploration_with_rrt.py", - "./holonomic_mobile_robot/holonomic_mobile_robot_with_valueiteration.py", - "./integrators/double_integrator.py", - "./integrators/double_integrator_optimal_controller.py", - "./integrators/integrators_with_closed_loops.py", - "./integrators/simple_integrator.py", - "./integrators/triple_integrator.py", - "./robot_arms/fivelinkrobot_kinematic_controller.py", - "./robot_arms/threelinkrobot_computed_torque_controller.py", - "./robot_arms/threelinkrobot_kinematic_controller.py", - "./robot_arms/twolinkrobot_computed_torque_controller.py", - "./robot_arms/twolinkrobot_effector_impedance_controller.py", - "./robot_arms/twolinkrobot_joint_impedance_controller.py", - "./robot_arms/twolinkrobot_kinematic_controller.py", - "./robot_arms/twolinkrobot_kinematic_vs_dynamic_openloop.py", - "./robot_arms/twolinkrobot_quasi_static_controllers.py", - "./simple_pendulum/custom_simple_pendulum.py", - "./simple_pendulum/simple_pendulum_with_computed_torque.py", - "./simple_pendulum/simple_pendulum_with_open_loop_controller.py", - "./simple_pendulum/simple_pendulum_with_pid.py", - "./simple_pendulum/simple_pendulum_with_rrt.py", - "./simple_pendulum/simple_pendulum_with_sliding_mode_controller.py", - "./simple_pendulum/simple_pendulum_with_trajectory_following_computed_torque.py", - "./simple_pendulum/simple_pendulum_with_trajectory_following_sliding_mode_controller.py", - "./simple_pendulum/simple_pendulum_with_valueiteration.py", -] - -this_script_dir = Path(__file__).parent -this_module = sys.modules[__name__] -#print(_all_examples) - -def import_from_file(modulename, filepath): - """import a file as a module and return the module object - - Everything will be executed, except if it's conditional to - __name__ == "__main__" - - """ - - spec = spec_from_file_location(modulename, filepath) - mod = module_from_spec(spec) - spec.loader.exec_module(mod) - - return mod - -def gettestfunc(modname, fullpath): - """Create a function that imports a file and runs the main function - - """ - - def run_example_main(): - ex_module = import_from_file(modname, fullpath) - - # Call function `main()` if it exists - if hasattr(ex_module, "main"): - ex_main_fun = getattr(ex_module, "main") - ex_main_fun() - - # Close all figs to reclaim memory - plt.close('all') - - return run_example_main - -_all_test_funs = [] - -for example_file in _all_examples: - relpath = Path(example_file) - fullpath = this_script_dir.joinpath(relpath) - modname = relpath.stem # file name without extension - - # Define a new function with a name starting with test_ so it is ran - # by pytest. - setattr(this_module, "test_" + modname, - gettestfunc(modname, fullpath)) - -def main(): - for fun in _all_test_funs: - fun() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/dev/tests/old_tests/test_all_pendulums.py b/dev/tests/old_tests/test_all_pendulums.py deleted file mode 100644 index 5a585e2b..00000000 --- a/dev/tests/old_tests/test_all_pendulums.py +++ /dev/null @@ -1,94 +0,0 @@ -""" -Run all examples - -Does not check for correct results, only that there are no exceptions. - -Running this: - -* Using pytest: `pytest -ra examples/` (replace examples with path to the - examples folder where this script lives). Pytest will produce a report - of failed/succeeded tests - -* As a script: `python ./examples/test_all_examples.py`. Will stop at the - exception. - -TODO: Add actual tests that check for correct results - -""" - -from pathlib import Path - -from importlib.util import spec_from_file_location, module_from_spec - -import sys - -import inspect - -from matplotlib import pyplot as plt - -_all_examples = [ - "./simple_pendulum/custom_simple_pendulum.py", - "./simple_pendulum/simple_pendulum_with_computed_torque.py", - "./simple_pendulum/simple_pendulum_with_open_loop_controller.py", - "./simple_pendulum/simple_pendulum_with_pid.py", - "./simple_pendulum/simple_pendulum_with_rrt.py", - "./simple_pendulum/simple_pendulum_with_sliding_mode_controller.py", - "./simple_pendulum/simple_pendulum_with_trajectory_following_computed_torque.py", - "./simple_pendulum/simple_pendulum_with_trajectory_following_sliding_mode_controller.py", - "./simple_pendulum/simple_pendulum_with_valueiteration.py" -] - -this_script_dir = Path(__file__).parent -this_module = sys.modules[__name__] -#print(_all_examples) - -def import_from_file(modulename, filepath): - """import a file as a module and return the module object - - Everything will be executed, except if it's conditional to - __name__ == "__main__" - - """ - - spec = spec_from_file_location(modulename, filepath) - mod = module_from_spec(spec) - spec.loader.exec_module(mod) - - return mod - -def gettestfunc(modname, fullpath): - """Create a function that imports a file and runs the main function - - """ - - def run_example_main(): - ex_module = import_from_file(modname, fullpath) - - # Call function `main()` if it exists - if hasattr(ex_module, "main"): - ex_main_fun = getattr(ex_module, "main") - ex_main_fun() - - # Close all figs to reclaim memory - plt.close('all') - - return run_example_main - -_all_test_funs = [] - -for example_file in _all_examples: - relpath = Path(example_file) - fullpath = this_script_dir.joinpath(relpath) - modname = relpath.stem # file name without extension - - # Define a new function with a name starting with test_ so it is ran - # by pytest. - setattr(this_module, "test_" + modname, - gettestfunc(modname, fullpath)) - -def main(): - for fun in _all_test_funs: - fun() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/dev/tests/old_tests/test_integrators.py b/dev/tests/old_tests/test_integrators.py deleted file mode 100644 index 545c720e..00000000 --- a/dev/tests/old_tests/test_integrators.py +++ /dev/null @@ -1,121 +0,0 @@ - -from collections import namedtuple - -import numpy as np - -import pytest - -from pyro.dynamic import integrator - - -SystemTestCase = namedtuple('SystemTestCase', ['sut', 'x0', 'get_ref_sol']) -SysSolution = namedtuple('SysSolution', ['t', 'x', 'y']) - -# Fixtures - -@pytest.fixture -def double_integ(): - """Generate a DoubleIntegrator CDS and accompanying solution - computed from analytical solution. - """ - - # System under test - sut = integrator.DoubleIntegrator() - sut.ubar = np.array([4.83,]) - - # Initial conditions - x0 = np.array([4.37, -3.74]) - - # Calculate analytical solution - def get_ref_sol(t): - npts = t.shape[0] - x_ref = np.empty((npts, 2)) - x_ref[:, 0] = (0.5 * sut.ubar * t**2) + (x0[1] * t) + x0[0] - x_ref[:, 1] = x0[1] + sut.ubar * t - - y_ref = x_ref[:, 0].reshape((npts, 1)) - - return SysSolution(t, x_ref, y_ref) - - return SystemTestCase(sut, x0, get_ref_sol) - -def test_simple_integrator_constant(): - # Setup simple integrator with constant input - I = integrator.SimpleIntegrator() - I.ubar = np.array([4.13,]) - - # Solution params - tf = 10 - x0 = 15.2 - npts = 100 - - # Reference solution - t_ref = np.linspace(0, tf, npts) - x_ref = (x0 + t_ref * I.ubar).reshape((npts, 1)) - - sim = I.compute_trajectory(x0, tf=tf, n=npts) - - assert(np.allclose(t_ref, sim.t)) - assert(np.allclose(x_ref, sim.x)) - assert(np.allclose(x_ref, sim.y)) - -def test_simple_integrator_ramp(): - """Simple integrator with ramp input""" - I = integrator.SimpleIntegrator() - - # Solution params - tf = 10 - x0 = 39.4 - npts = 100 - - # Ramp input - ramp_cst = np.pi - def u(t): - return np.asarray(t * ramp_cst) - - # Reference solution - t_ref = np.linspace(0, tf, npts) - x_ref = (x0 + 0.5 * ramp_cst * t_ref**2).reshape((npts, 1)) - - sim = I.compute_trajectory(x0, tf=tf, n=npts, u=u) - - assert(np.allclose(t_ref, sim.t)) - assert(np.allclose(x_ref, sim.x)) - assert(np.allclose(x_ref, sim.y)) - -def test_double_integrator_constant_ode(double_integ): - # Solution params - tf = 10 - npts = 10 - t_ref = np.linspace(0, tf, npts) - - # Reference solution - ref_sol = double_integ.get_ref_sol(t_ref) - - # Solution computed by SUT - I = double_integ.sut - sim = I.compute_trajectory(double_integ.x0, tf=tf, n=npts, solver='ode') - - assert(np.allclose(t_ref, sim.t)) - assert(np.allclose(ref_sol.x, sim.x)) - assert(np.allclose(ref_sol.y, sim.y)) - -def test_double_integrator_constant_euler(double_integ): - # Solution params - tf = 10 - npts = 100 - t_ref = np.linspace(0, tf, npts) - - # Reference solution - ref_sol = double_integ.get_ref_sol(t_ref) - - # Solution computed by SUT - I = double_integ.sut - sim = I.compute_trajectory(double_integ.x0, tf=tf, n=npts, solver='euler') - - # Euler's method has low-order convergence, so we tolerate 1% error - atol, rtol = 1, 0.01 - - assert(np.allclose(t_ref, sim.t)) - assert(np.allclose(ref_sol.x, sim.x, atol=atol, rtol=rtol)) - assert(np.allclose(ref_sol.y, sim.y, atol=atol, rtol=rtol)) \ No newline at end of file diff --git a/dev/tests/old_tests/test_pidcontrol.py b/dev/tests/old_tests/test_pidcontrol.py deleted file mode 100644 index 3dc2b948..00000000 --- a/dev/tests/old_tests/test_pidcontrol.py +++ /dev/null @@ -1,187 +0,0 @@ - -import numpy as np - -from scipy import integrate, signal - -import pytest - -from pyro.control.linear import PIDController, ProportionalController - -from pyro.dynamic.system import ContinuousDynamicSystem - -from pyro.dynamic.manipulator import TwoLinkManipulator - -class FirstOrder(ContinuousDynamicSystem): - def __init__(self, tau): - super().__init__(1, 1, 1) - self.tau = tau - - def f(self, x, u, t): - return (u - x) / self.tau - -def step(a, delay=0): - def u(t): - if t < delay: - return 0 - else: - return a - return u - -def filtered_deriv(y, x, tau=0): - """Numerical derivative with optional lowpass filter. - - tau is the filter time constant expressed in same units as x (eg seconds if x is - time). - """ - dy = np.empty(y.shape) - dy[0] = (y[1] - y[0]) / (x[1] - x[0]) - for i in range(1, (dy.shape[0] - 1)): - dy[i] = (y[i+1] - y[i-1]) / (x[i+1] - x[i-1]) - dy[-1] = (y[-1] - y[-2]) / (x[-1] - x[-2]) - - # No filter - if tau <= 0: - return dy - - # sample freq - fs = x.shape[0] / (x[-1] - x[0]) - nyqfreq = fs / 2 - w0 = 1 / (tau*2*np.pi*nyqfreq) - - lowpass = signal.iirfilter(1, w0, btype='lowpass', analog=False) - - # Create initial conditions so y=0 at t=0 - zi = signal.lfiltic(*lowpass, y=[0], x=dy[:1]) - - filtered, _ = signal.lfilter(*lowpass, dy, zi=-zi) - return filtered - -def test_sdof_prop(): - """Test single DOF system with proportional control""" - tau_p = 2 - tf = 1.5 - kp = 20 - - sys = FirstOrder(tau_p) - ctl = ProportionalController(20) - clsys = ctl + sys - - sim = clsys.compute_trajectory(x0=[0], r=step(1), tf=tf, n=100) - - # analytic solution - kcl = kp / (kp + 1) # steady state asymptote value - tau_cl = tau_p / (kp + 1) # Closed loop time constant - x_ref = kcl * (1-np.exp(-sim.t/tau_cl)) - - assert np.allclose(sim.x[:, 0], x_ref) - -def test_sdof_pid(): - """Check PID controller outputs""" - tf = 2 # simulation time - npts = 500 # simulation samples - tau = 1E-2 # time constant of derivative filter - - # Create system/controller and run simulation - sys = FirstOrder(1) - ctl = PIDController([[3]], [[2]], [[0.6]], dv_tau=tau) - clsys = ctl + sys - sim = clsys.compute_trajectory(0, r=step(1), tf=tf, n=npts) - - # error - e = (sim.r - sim.y)[:, 0] - - # integral of error - e_int = integrate.cumtrapz(e, sim.t, initial=0) - - # filtered derivative of error - e_fd = filtered_deriv(e, sim.t, tau) - - # Check controller output - ctl_ref = (e * ctl.KP + e_int * ctl.KI + e_fd * ctl.KD).reshape((sim.n,)) - - assert np.allclose(sim.u[:, 0], ctl_ref, atol=0, rtol=1E-2) - -def test_nd_pid(): - tf = 2 - npts = 500 - tau = 1E-2 - - class DummyManipulator(TwoLinkManipulator): - """Manipulator with null response to inputs""" - def __init__(self): - super().__init__() - self.p = 2 - - def B(self, q): - return np.zeros(self.dof) - - def h(self, x, u, t): - """Read joint positions""" - return x[:2] - - sys = DummyManipulator() - ctl = PIDController( - KP = np.ones((2,2)), - KI = np.ones((2,2)), - KD = np.ones((2,2)), - dv_tau=tau - ) - clsys = ctl + sys - - x0 = [np.pi-0.5, np.pi/2, 0, 0] - sim = clsys.compute_trajectory(x0=x0, tf=tf, n=npts) - - errors = sim.r - sim.y - - e_int = integrate.cumtrapz(errors, sim.t, axis=0, initial=0) - - e_der = np.empty(errors.shape) - for j in range(errors.shape[1]): - e_der[:, j] = filtered_deriv(errors[:, j], sim.t, tau=tau) - - ctl_ref = errors.dot(ctl.KP.T) + e_int.dot(ctl.KI.T) + e_der.dot(ctl.KD.T) - - assert np.allclose(sim.u, ctl_ref, atol=0.05) - -def test_check_and_normalize(): - # Check normalization of scalars to 2D array - ctl = PIDController(1, 1, 1) - for arr in [ctl.KP, ctl.KI, ctl.KD]: - assert arr.ndim == 2 - assert arr.shape == (1, 1) - assert arr[0, 0] == 1 - - # Check normalization of 1D array to 2D array - vals = [1, 2, 3] - ctl = PIDController(vals, vals, vals) - for arr in [ctl.KP, ctl.KI, ctl.KD]: - assert arr.ndim == 2 - assert arr.shape == (1, 3) - assert np.all(vals == arr) - - # Check that 2D arrays should be unchanged - vals = np.ones((3, 4)) * 2.3 - ctl = PIDController(vals, vals, vals) - for arr in [ctl.KP, ctl.KI, ctl.KD]: - assert arr.ndim == 2 - assert arr.shape == vals.shape - assert np.all(vals == arr) - - # Check default zero values for KI and KD - for testval in [1, [1, 2, 3], np.ones((8, 10))]: - ctl = PIDController(testval) - for arr in [ctl.KI, ctl.KD]: - assert arr.shape == ctl.KP.shape - assert np.all(arr == np.zeros(ctl.KP.shape)) - - with pytest.raises(ValueError): - ctl = PIDController(1, KI=np.ones(2)) - with pytest.raises(ValueError): - ctl = PIDController(1, KD=np.ones(2)) - with pytest.raises(ValueError): - ctl = PIDController(np.ones((2, 3)), KI=np.ones((3, 2))) - with pytest.raises(ValueError): - ctl = PIDController(np.ones((2, 3)), KD=np.ones((3, 2))) - -if __name__ == "__main__": - test_check_and_normalize() \ No newline at end of file diff --git a/dev/tests/old_tests/test_state_space.py b/dev/tests/old_tests/test_state_space.py deleted file mode 100644 index 917540c4..00000000 --- a/dev/tests/old_tests/test_state_space.py +++ /dev/null @@ -1,204 +0,0 @@ - -import numpy as np - -import pytest - -from pyro.dynamic import StateSpaceSystem, linearize - -from pyro.dynamic.pendulum import SinglePendulum - -from pyro.dynamic.manipulator import TwoLinkManipulator - -class SdofOscillator(StateSpaceSystem): - """Single DOF mass-spring-damper system - - Defined using state-space matrices - - """ - - def __init__(self, m, k, b): - """ - m: mass - k: spring constant - b: damping constant - """ - - self.mass = m - self.k = k - self.b = b - - A = np.array([[0, 1], [-k/m, -b/m]]) - B = np.array([0, 1/m]).reshape((2, 1)) - - C = np.identity(2) - D = np.zeros((2, 1)) - - super().__init__(A, B, C, D) - - def solve_analytical_free(self, x0, dx0, t): - """ - Free vibration equations from: - Rao (2019) Vibrations of Continuous Systems (2 ed) - eq 2.16 - 2.21 - - x0: initial position - dx0: initial velocity - """ - - # undamped natural frequency - wn = np.sqrt(self.k / self.mass) - - # damping ratio - xi = self.b / 2 / self.mass / wn - - if xi < 1: - wd = np.sqrt(1 - xi**2) * wn - return np.exp(-xi * wn * t) *\ - ((x0 * np.cos(wd * t)) + (((dx0 + xi*wn*x0) / wd) * np.sin(wd*t))) - else: - raise NotImplementedError("Only underdamped implemented") - - def solve_analytical_forced(self, x0, dx0, f0, w0, t): - """ - Solution of *undamped* oscillator with harmonic excitation of the form - - F = f0*cos(w0*t) - - ref: Rao (2019) Vibrations of Continuous Systems (2 ed) - eq 2.27 - """ - - if self.b != 0: - raise NotImplementedError("Only implemented for undamped system") - - wn = np.sqrt(self.k / self.mass) - return (x0 - (f0 / (self.k - self.mass * w0**2))) * np.cos(wn*t) \ - + dx0 / wn * np.sin(wn*t) \ - + (f0 / (self.k - self.mass * w0**2)) * np.cos(w0*t) - -def test_1dof_oscillator_free(): - sys = SdofOscillator(2.0, 100.0, 10.0) - x0 = np.array([0.2, 0]) - - # Simulate time solution - sim = sys.compute_trajectory(x0) - - # Reference analytical time solution - ref = sys.solve_analytical_free(x0[0], x0[1], sim.t) - - assert np.allclose(sim.x[:, 0], ref) - assert np.allclose(sim.y[:, 0], ref) - -def test_1dof_oscillator_forced(): - sys = SdofOscillator(m=2.0, k=100.0, b=0.0) - x0 = np.array([0.2, 0]) - - # External force - w0 = 2 - f0 = 4.5 - def u(t): - t = np.asarray(t) - return (np.cos(w0*t) * f0).reshape((t.size, 1)) - - # Simulate time solution - sim = sys.compute_trajectory(x0, u=u, n=1000) - - # Reference analytical time solution - ref = sys.solve_analytical_forced(x0[0], x0[1], f0, w0, sim.t) - - assert np.allclose(sim.x[:, 0], ref, atol=1e-5, rtol=0) - assert np.allclose(sim.y[:, 0], ref, atol=1e-5, rtol=0) - assert np.allclose(sim.u, u(sim.t)) - -def test_dimension_checks(): - # Valid dimensions for system with 5 states, 2 inputs and 4 outputs - A = np.zeros((5, 5)) - B = np.zeros((5, 2)) - C = np.zeros((4, 5)) - D = np.zeros((4, 2)) - - sys = StateSpaceSystem(A, B, C, D) - assert sys.n == 5 - assert sys.m == 2 - assert sys.p == 4 - - # Unsquare A matrix - with pytest.raises(ValueError): - StateSpaceSystem(A[:4, :], B, C, D) - - # B does not match number of states - with pytest.raises(ValueError): - StateSpaceSystem(A, B[:4, :], C, D) - - # C does not match number of states - with pytest.raises(ValueError): - StateSpaceSystem(A, B, C[:, :4], D) - - # mismatch number of inputs between B and D - with pytest.raises(ValueError): - StateSpaceSystem(A, B[:, :1], C, D) - - # mismatch number of outputs between C and D - with pytest.raises(ValueError): - StateSpaceSystem(A, B, C, D[:2, :]) - -def test_linearize_identity(): - """Linearization of linear system should be identical""" - - # ensure repeatable random matrices - np.random.seed(0) - - A = np.random.rand(5, 5) - B = np.random.rand(5, 2) - C = np.random.rand(4, 5) - D = np.random.rand(4, 2) - - sys = StateSpaceSystem(A, B, C, D) - - x0 = np.random.rand(5, 1) - u0 = np.random.rand(2, 1) - - linearized = linearize(sys, x0, u0, 1e-3) - - assert np.allclose(sys.A, linearized.A) - assert np.allclose(sys.B, linearized.B) - assert np.allclose(sys.C, linearized.C) - assert np.allclose(sys.D, linearized.D) - -def test_linearize_pendulum(): - class Cartesian1Pendulum(SinglePendulum): - """Single pendulum with cartesian coordinate ouput (y)""" - def h(self, x, u, t): - return np.array([np.sin(x[0]), -np.cos(x[0])]) * self.l1 - - nlsys = Cartesian1Pendulum() - nlsys.lc1 = 0.3 - nlsys.l1 = nlsys.lc1 - nlsys.m1 = 1.2 - nlsys.d1 = 0.1 - - # Linearization point - x0 = np.zeros((2, 1)) - u0 = np.zeros((1,)) - y0 = nlsys.h(x0, 0, 0).reshape((2,)) - - # linearize with epsilon = 0.01 rad (~1 deg) - linsys = linearize(nlsys, x0, u0, epsilon_x=0.01) - - # Simulate with 5 degree initial position and zero velocity - x_init = np.array([0.087, 0]) - - nlsim = nlsys.compute_trajectory(x_init, tf=10) - linsim = linsys.compute_trajectory(x_init, tf=10) - - rtol = 0.0 - atol = x_init[0] * 0.02 # 2% error tolerance - - assert np.allclose(nlsim.t, linsim.t) - assert np.allclose(nlsim.x, linsim.x, rtol, atol) - assert np.allclose(nlsim.dx, linsim.dx, rtol, atol) - assert np.allclose(nlsim.u, linsim.u) - assert np.allclose(nlsim.y, (linsim.y + y0), rtol, atol) - -if __name__ == "__main__": - pass \ No newline at end of file diff --git a/dev/tests/old_tests/test_stateobserver.py b/dev/tests/old_tests/test_stateobserver.py deleted file mode 100644 index a7a7e1a6..00000000 --- a/dev/tests/old_tests/test_stateobserver.py +++ /dev/null @@ -1,200 +0,0 @@ - -import pytest - -import numpy as np - -from pyro.dynamic.statespace import StateSpaceSystem, StateObserver - -from io import StringIO - -class TwoDofSs(StateSpaceSystem): - def __init__(self): - A = np.array([ - [-0.00054584, 0.0002618 ], - [ 0.0002618 , -0.00061508] - ]) - - B = np.array([ - [0.00078177, 0.00028403], - [0., 0.00041951]] - ) - - C = np.array([[0., 1.]]) - - D = np.array([[0., 0.]]) - - super().__init__(A, B, C, D) - -class Test_Obs_TwoDofSs(): - """ - Test data (expected Kalman gains, sim results, etc) were generated with Matlab - script "observer_twodofss.m". - """ - def test_obs_dimcheck(self): - sys = TwoDofSs() - with pytest.raises(ValueError): - L = np.empty([2, 2]) - StateObserver.from_ss(sys, L) - - with pytest.raises(ValueError): - L = np.empty([4, 1]) - obs = StateObserver.from_ss(sys, L) - - L = np.empty([2, 1]) - obs = StateObserver.from_ss(sys, L) - - def test_kalman_gain_from_ss(self): - sys = TwoDofSs() - Q = np.array([[0.6324, 0.1880], [0.1880, 0.5469]]); - R = 0.9575; - kf = StateObserver.kalman_from_ss(sys, Q, R) - - # Kalman gain - L_matlab = np.array([0.241879988531013, 0.163053708572278]).reshape(2, 1) * 1E-3 - - # Oberserver covariance matrix - P_matlab = np.array([ - [0.530701428966605, 0.231600089018445], - [0.231600089018445, 0.156123925957956] - ]) * 1E-3 - np.testing.assert_array_almost_equal(kf.L, L_matlab) - np.testing.assert_array_almost_equal(kf.P, P_matlab) - - def test_kalman_gain_from_ABCD(self): - sys = TwoDofSs() - Q = np.array([[0.6324, 0.1880], [0.1880, 0.5469]]); - R = 0.9575; - kf = StateObserver.kalman(sys.A, sys.B, sys.C, sys.D, Q, R) - - # Kalman gain - L_matlab = np.array([0.241879988531013, 0.163053708572278]).reshape(2, 1) * 1E-3 - - # Oberserver covariance matrix - P_matlab = np.array([ - [0.530701428966605, 0.231600089018445], - [0.231600089018445, 0.156123925957956] - ]) * 1E-3 - - np.testing.assert_array_almost_equal(kf.L, L_matlab) - np.testing.assert_array_almost_equal(kf.P, P_matlab) - - def test_kalman_check_dims(self): - sys = TwoDofSs() - - with pytest.raises(ValueError): - Q = np.empty([3, 3]) - R = np.empty(1) - kf = StateObserver.kalman_from_ss(sys, Q, R) - - with pytest.raises(ValueError): - Q = np.empty([2, 3]) - R = np.empty(1) - kf = StateObserver.kalman_from_ss(sys, Q, R) - - with pytest.raises(ValueError): - Q = np.empty([3, 2]) - R = np.empty(1) - kf = StateObserver.kalman_from_ss(sys, Q, R) - - with pytest.raises(ValueError): - Q = np.empty([2, 2]) - R = np.empty([2, 1]) - kf = StateObserver.kalman_from_ss(sys, Q, R) - - Q = np.empty([2, 2]) - R = np.empty(1) - kf = StateObserver.kalman_from_ss(sys, Q, R) - - - def test_sim_observed_sys(self): - """Simulate ObservedSystem and compare result against matlab""" - sys = TwoDofSs() - Q = np.array([[0.6324, 0.1880], [0.1880, 0.5469]]); - R = 0.9575; - kf = StateObserver.kalman_from_ss(sys, Q, R) - - tt = np.linspace(0, 10_000, 50) - - def t2u(t): - if (t % 2000) < 1000: return [100, 20] - else: return [150, 20] - - sys.t2u = t2u - sys.x0 = np.array([50, 20]) - kf.x0 = np.array([0, 0]) - - osys = kf + sys - tf = 10_000 - traj = osys.compute_trajectory(tf=tf, n=50, method="DOP853", rtol=1E-4, atol=1E-3) - - # Matlab generated data - expected_x_est_txt = """ - 0,0 - 17.1816933112253,2.67356729335786 - 32.6690586962738,5.82859520370164 - 46.6546237710204,9.32442222148156 - 59.3067122681008,13.0471771432147 - 71.5276536671465,16.9072478692853 - 89.4100863270359,21.0605927899578 - 105.581133740505,25.5464167418339 - 120.235163744847,30.2450926073082 - 133.54083561347,35.0605541856489 - 144.145342049008,39.908533059689 - 147.782605315062,44.4764402774289 - 151.218496987128,48.6570058723352 - 154.457796350395,52.4874078787767 - 157.506432493375,56.0004668309786 - 162.611520978705,59.2429648674815 - 172.620730276353,62.5084875703597 - 181.714021581887,65.8486940494296 - 189.992743238983,69.2127999649183 - 197.544222204844,72.5606793538811 - 201.474865866373,75.8310112090093 - 200.549189229051,78.7251242885965 - 199.838307324667,81.2244129101249 - 199.302580326945,83.3864237545624 - 198.90932687086,85.2596270743574 - 202.325759488412,86.9320759701282 - 209.310514157894,88.7129350344944 - 215.639957552738,90.6092996685361 - 221.389332577766,92.5772594675247 - 226.623132747702,94.5816224562129 - 226.987428251076,96.5278400916074 - 224.255676111288,98.1279656932221 - 221.876950861141,99.406279861416 - 219.799635130726,100.417827326237 - 217.980439792433,101.208381543566 - 221.501471929412,101.90611387318 - 227.117466778374,102.793385710491 - 232.185808843378,103.840649870868 - 236.771941112517,105.003439669918 - 240.931870147046,106.245677667186 - 238.893939424458,107.421823001517 - 235.388555299305,108.281100091856 - 232.286339967808,108.871642671038 - 229.532653928748,109.244248870842 - 227.08108262064,109.441075730228 - 231.406190089678,109.643118035093 - 236.327062319301,110.07665108912 - 240.75426253915,110.692120331065 - 244.748083523848,111.444937451339 - 248.360465903124,112.298568351518 - """.strip() - - expected_x_est = np.loadtxt(StringIO(expected_x_est_txt), delimiter=",") - - # plots for debugging - # - # from matplotlib import pyplot as plt - # plt.figure() - # plt.plot(traj.t, expected_x_est, "-o"); - # plt.plot(traj.t, traj.y, "-x"); - # plt.plot(traj.t, traj.x[:, :2], color="k"); - # plt.show() - - # Compare against matlab with 0.1 % error - np.testing.assert_allclose(expected_x_est, traj.y, rtol=1E-3, atol=0.01) - -if __name__ == "__main__": - import scipy.io - Test_Obs_TwoDofSs().test_sim_observed_sys() diff --git a/dev/tests/old_tests/test_sys b/dev/tests/old_tests/test_sys deleted file mode 100644 index 4e674040..00000000 --- a/dev/tests/old_tests/test_sys +++ /dev/null @@ -1,31 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Thu Jan 23 12:08:36 2020 - -@author: alex -""" - -import numpy as np -from pyro.dynamic import integrator - - -################################### -# Simple integrator -sys = integrator.SimpleIntegrator() - -# Default input signal -sys.ubar = np.array([1]) - -# Phase plane behavior -sys.plot_phase_plane(0,0) # only one state for two axis! - -# Initial conditions -sys.x0 = np.array([2]) - -# Simulation -traj = sys.compute_trajectory() - -# Plot output -sys.plot_trajectory() -sys.plot_phase_plane_trajectory(0,0) \ No newline at end of file diff --git a/dev/tests/old_tests/test_utils.py b/dev/tests/old_tests/test_utils.py deleted file mode 100644 index 02d4d29c..00000000 --- a/dev/tests/old_tests/test_utils.py +++ /dev/null @@ -1,13 +0,0 @@ -import matplotlib.pyplot as plt - -def compare_signals(x1, y1, x2, y2): - fig = plt.figure() - - num_subfigs = y1.shape[1] - for j in range(num_subfigs): - ax = fig.add_subplot(num_subfigs, 1, j+1) - ax.plot(x1, y1[:, j], label="y1") - ax.plot(x2, y2[:, j], label="y2") - ax.legend() - - return fig diff --git a/dev/tests/old_tests/test_value_iteration.py b/dev/tests/old_tests/test_value_iteration.py deleted file mode 100644 index bf62d768..00000000 --- a/dev/tests/old_tests/test_value_iteration.py +++ /dev/null @@ -1,87 +0,0 @@ -""" -Run all examples - -Does not check for correct results, only that there are no exceptions. - -Running this: - -* Using pytest: `pytest -ra examples/` (replace examples with path to the - examples folder where this script lives). Pytest will produce a report - of failed/succeeded tests - -* As a script: `python ./examples/test_all_examples.py`. Will stop at the - exception. - -TODO: Add actual tests that check for correct results - -""" - -from pathlib import Path - -from importlib.util import spec_from_file_location, module_from_spec - -import sys - -import inspect - -from matplotlib import pyplot as plt - -_all_examples = [ - "./simple_pendulum/simple_pendulum_with_valueiteration_nd.py", - "./holonomic_mobile_robot/holonomic_mobile_robot_with_valueiteration_nd.py", -] - -this_script_dir = Path(__file__).parent -this_module = sys.modules[__name__] -#print(_all_examples) - -def import_from_file(modulename, filepath): - """import a file as a module and return the module object - - Everything will be executed, except if it's conditional to - __name__ == "__main__" - - """ - - spec = spec_from_file_location(modulename, filepath) - mod = module_from_spec(spec) - spec.loader.exec_module(mod) - - return mod - -def gettestfunc(modname, fullpath): - """Create a function that imports a file and runs the main function - - """ - - def run_example_main(): - ex_module = import_from_file(modname, fullpath) - - # Call function `main()` if it exists - if hasattr(ex_module, "main"): - ex_main_fun = getattr(ex_module, "main") - ex_main_fun() - - # Close all figs to reclaim memory - plt.close('all') - - return run_example_main - -_all_test_funs = [] - -for example_file in _all_examples: - relpath = Path(example_file) - fullpath = this_script_dir.joinpath(relpath) - modname = relpath.stem # file name without extension - - # Define a new function with a name starting with test_ so it is ran - # by pytest. - setattr(this_module, "test_" + modname, - gettestfunc(modname, fullpath)) - -def main(): - for fun in _all_test_funs: - fun() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/dev/tests/old_tests/traj.npz b/dev/tests/old_tests/traj.npz deleted file mode 100644 index 49c75ddb..00000000 Binary files a/dev/tests/old_tests/traj.npz and /dev/null differ diff --git a/dev/tests/test_graphical_settings.py b/dev/tests/test_graphical_settings.py deleted file mode 100644 index f044a6c4..00000000 --- a/dev/tests/test_graphical_settings.py +++ /dev/null @@ -1,35 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Fri Nov 16 12:05:08 2018 - -@author: Alexandre -""" - - -import matplotlib -import matplotlib.pyplot as plt -import sys as python_system - -############################################################################### -import numpy as np -############################################################################### -from pyro.dynamic import pendulum -############################################################################### - -#matplotlib.use('Qt5Agg') -#plt.ion() - -print('The current matplotlib backend is:', matplotlib.get_backend() ) -print('Matplotlib interactive mode is activated: ', plt.isinteractive() ) -print('The python script is running interactive', hasattr(python_system, 'ps1') ) - -sys = pendulum.DoublePendulum() - -# Simultation -sys.x0 = np.array([-0.1,0,0,0]) -sys.plot_trajectory() -sys.plot_phase_plane_trajectory(0, 2) -sys.animate_simulation() -#sys.animate_simulation( is_3d = True ) -sys.show3( [1,1] ) - diff --git a/examples/courses/udes_gmc714/demo_simple_pendulum_multiple_controller_options.py b/examples/courses/udes_gmc714/demo_simple_pendulum_multiple_controller_options.py index 887c8aff..dce7dc22 100644 --- a/examples/courses/udes_gmc714/demo_simple_pendulum_multiple_controller_options.py +++ b/examples/courses/udes_gmc714/demo_simple_pendulum_multiple_controller_options.py @@ -98,7 +98,7 @@ cl_sys.x0[0] = q0 cl_sys.compute_trajectory(tf,10001,'euler') cl_sys.plot_trajectory('xu') +sys.plot_phase_plane() +cl_sys.plot_phase_plane_trajectory_closed_loop() cl_sys.animate_simulation() -sys.plot_phase_plane() -cl_sys.plot_phase_plane_trajectory_closed_loop() \ No newline at end of file diff --git a/examples/courses/udes_gro640/prob/demo_crash_commande_en_position.py b/examples/courses/udes_gro640/prob/demo_crash_commande_en_position.py index f9906466..11eb23ba 100755 --- a/examples/courses/udes_gro640/prob/demo_crash_commande_en_position.py +++ b/examples/courses/udes_gro640/prob/demo_crash_commande_en_position.py @@ -26,7 +26,7 @@ # Configurations de départs clsys.x0 = np.array([0,0.5,0]) # crash -#clsys.x0 = np.array([0,0.7,0]) # fonctionne +# clsys.x0 = np.array([0,0.7,0]) # fonctionne # Simulation clsys.compute_trajectory( solver = 'odeint' ) diff --git a/examples/demos_by_system/acrobot/acrobot_with_lqr.py b/examples/demos_by_system/acrobot/acrobot_with_lqr.py index 77908675..da56bf17 100644 --- a/examples/demos_by_system/acrobot/acrobot_with_lqr.py +++ b/examples/demos_by_system/acrobot/acrobot_with_lqr.py @@ -34,6 +34,6 @@ # Simulation Closed-Loop Non-linear with LQR controller cl_sys = ctl + sys cl_sys.x0 = np.array([-0.1,0.2,0,0]) -cl_sys.compute_trajectory( tf = 2.0 ) +cl_sys.compute_trajectory( tf = 5.0 ) cl_sys.plot_trajectory('xu') -cl_sys.animate_simulation( time_factor_video=0.02 ) \ No newline at end of file +cl_sys.animate_simulation( time_factor_video=1.5 ) \ No newline at end of file diff --git a/examples/demos_by_system/boat/boat_position_controller.py b/examples/demos_by_system/boat/boat_position_controller.py new file mode 100644 index 00000000..34de6910 --- /dev/null +++ b/examples/demos_by_system/boat/boat_position_controller.py @@ -0,0 +1,281 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Sun Oct 3 08:27:06 2021 + +@author: alex +""" + +import numpy as np + +from scipy.linalg import solve_continuous_are + +from pyro.dynamic.boat import Boat2D +from pyro.analysis.costfunction import QuadraticCostFunction +from pyro.dynamic.statespace import linearize +from pyro.control import controller + +############################################################################### +def na( theta ): + """ + Normalize angle to [-pi,pi] + """ + + theta = ( theta + np.pi ) % (2*np.pi) - np.pi + + return theta + + +############################################################################### + +class BoatController( controller.StaticController ) : + """ + A simple boat controller based on a cascade control structure + + High-level position control: + heading and velocity setpoint are based on the + position error computed in the global frame + + Low-level velocity control: + LQR velocity control computed in the body frame + + """ + + ############################ + def __init__( self , sys ): + """ """ + # Dimensions of signals + self.k = 3 + self.m = 2 + self.p = 6 + + super().__init__(self.k, self.m, self.p) + + # Label + self.name = 'Boat Controller' + + # Dynamic model available to the controller + self.sys = sys + + # Velocity inner loop parameters + # only used if the position control loop is deactivated + self.reference_velocity = np.array([ 5.0, 0.0, 0.0]) + + # Linearized model for LQR controller + ss = linearize( sys , 0.01 ) + self.A = ss.A[3:,3:] + self.B = ss.B[3:,:] + self.A[ abs(self.A) < 0.00001 ] = 0.0 + self.B[ abs(self.B) < 0.00001 ] = 0.0 + print('Velocity linearized dynamic') + print('----------------------------') + print('A =\n', self.A) + print('B =\n', self.B) + + # Cost function for LQR controller + cf = QuadraticCostFunction(3,2) + cf.Q[0,0] = 10000 + cf.Q[1,1] = 10000 + cf.Q[2,2] = 50000 + cf.R[0,0] = 0.0001 + cf.R[1,1] = 0.0001 + + # LQR solution + self.S = solve_continuous_are( self.A , self.B , cf.Q , cf.R ) + self.K = np.linalg.inv( cf.R ) @ self.B.T @ self.S + self.K[ abs(self.K) < 0.00001 ] = 0.0 + print('Velocity inner-loop LQR gain matrix =\n', self.K) + + # Outer position loop parameters + self.position_control_isactivated = True + self.KP = np.array([[ 0.5 , 0 , 0], + [ 0 , 0.5 , 0], + [ 0 , 0 , 2]]) + self.d_max = 2.0 + + # Trajectory following parameters + self.trajectory_following_isactivated = False + self.traj_amplitude = 12.0 + self.traj_frequency = 0.1 + + # Saturations + self.f_max = np.array([100000,10000]) + self.f_min = np.array([-10000,-10000]) + self.v_max = np.array([5.0,1.0,1.0]) + self.v_min = np.array([-1.0,-1.0,-1.0]) + + ############################# + def q_d( self , t = 0 ): + """ Return the desired position """ + + if self.trajectory_following_isactivated: + + a = self.traj_amplitude + w = self.traj_frequency + + q_d = np.array([ a * np.cos(w*t), + a * np.sin(w*t), + w * t + np.pi/2]) + + else: + + q_d = np.array([0,0,0.0]) + + return q_d + + ############################# + def dq_d( self , t = 0 ): + """ Return the time derivative of the desired position """ + + if self.trajectory_following_isactivated: + + a = self.traj_amplitude + w = self.traj_frequency + + dq_d = np.array([ a * w * - np.sin(w*t) , a * w * np.cos(w * t ), w ]) + + else: + + dq_d = np.array([0,0,0.0]) + + return dq_d + + + ############################# + def c( self , y , r , t = 0 ): + """ + Control law + """ + + # Avilable observations + q = y[0:3] # position feedback + v = y[3:] # velocity feedback + + if self.position_control_isactivated: + + # Desired position and velocity + q_d = self.q_d(t) + dq_d = self.dq_d(t) + + # Configuration error + q_e = q_d - q + d_e = np.linalg.norm(q_e[0:2]) # absolute distance to target + + # Angular position error withtout cyclic fuck-up + q_e[2] = na( na(q_d[2]) - na(q[2]) ) + + # Dynamic heading ref + # If far from target, reference orientation is overided to + # instead making the boat head toward the desired position + if d_e > self.d_max: + actual = na( q[2] ) + desired = na( np.arctan2( q_e[1] , q_e[0] ) ) + q_e[2] = na( desired - actual ) + + # Position outter loop in inertial frame + dq_r = self.KP @ q_e + dq_d + + # Desired velocity in body frame + v_d = self.sys.N( q ).T @ dq_r + + # Velocity setpoint limits + v_d = np.clip( v_d , self.v_min , self.v_max ) + + else: + + # Direct Velocity control for debugging + v_d = self.reference_velocity + + # Velocity error + v_e = v_d - v + + # Velocity inner loop + u = self.K @ v_e + + # Max/min propulsion force + u = np.clip( u , self.f_min , self.f_max ) + + return u + + ######################################################################### + def forward_kinematic_lines_plus( self, x , u , t ): + """ + Graphical output for the controller + ----------------------------------- + plot the desired boat pose + + x,u,t are the state, input and time of the global closed-loop system + + """ + + if self.position_control_isactivated: + + # desired boat pose, from model forward kinematics + pts, style, color = self.sys.forward_kinematic_lines( self.q_d(t) ) + + # Change the line style and color + style[0] = '--' + color[0] = 'c' + color[1] = 'c' + + else: + + pts = None + style = None + color = None + + return pts, style, color + + +# Non-linear model +sys = Boat2D() + +# Cascade controller +ctl = BoatController( sys ) + +# Simulation of Non-linear Boat Model with LQR linear controller +cl_sys = ctl + sys + +################################ +# Test velocity innerloop +################################ + +# ctl.position_control_isactivated = False +# ctl.reference_velocity = np.array([ 1.0, 0.1, 0.2]) +# cl_sys.x0 = np.array([3,3,1.0,5,0,0]) +# cl_sys.compute_trajectory(10) +# cl_sys.plot_trajectory('xu') +# cl_sys.animate_simulation( time_factor_video = 1.0 ) + + +################################ +# Test fixed targets position control +################################ + +ctl.position_control_isactivated = True + +cl_sys.x0 = np.array([3,3,1.0,5,0,0]) +cl_sys.compute_trajectory(20) +cl_sys.plot_trajectory('xu') +cl_sys.animate_simulation( time_factor_video = 1.0 ) + +cl_sys.x0 = np.array([-20,10,-2.5,0,0,0]) +cl_sys.compute_trajectory(30) +cl_sys.plot_trajectory('xu') +cl_sys.animate_simulation( time_factor_video = 1.0 ) + +cl_sys.x0 = np.array([50,50,0.0,0,0,0]) +cl_sys.compute_trajectory(80) +cl_sys.plot_trajectory('xu') +cl_sys.animate_simulation( time_factor_video = 1.0 ) + +################################ +# Test trajectory following control +################################ + +ctl.trajectory_following_isactivated = True + +cl_sys.x0 = np.array([0,0,0,0,0,0]) +cl_sys.compute_trajectory(60) +cl_sys.plot_trajectory('xu') +cl_sys.animate_simulation( time_factor_video = 1.0 ) \ No newline at end of file diff --git a/examples/demos_by_system/car_propulsion/longitudinal_car_braking_value_iteration.py b/examples/demos_by_system/car_propulsion/longitudinal_car_braking_value_iteration.py index ecaf9265..0defc280 100644 --- a/examples/demos_by_system/car_propulsion/longitudinal_car_braking_value_iteration.py +++ b/examples/demos_by_system/car_propulsion/longitudinal_car_braking_value_iteration.py @@ -9,14 +9,14 @@ ############################################################################### import numpy as np ############################################################################### -from pyro.dynamic import longitudinal_vehicule +from pyro.dynamic import vehicle_propulsion from pyro.planning import discretizer from pyro.analysis import costfunction from pyro.planning import dynamicprogramming from pyro.control import controller ############################################################################### -sys = longitudinal_vehicule.LongitudinalFrontWheelDriveCarWithWheelSlipInput() +sys = vehicle_propulsion.LongitudinalFrontWheelDriveCarWithWheelSlipInput() ############################################################################### diff --git a/examples/demos_by_system/car_propulsion/longitudinal_car_with_torque_input.py b/examples/demos_by_system/car_propulsion/longitudinal_car_with_torque_input.py index 4a57dc15..95f5c198 100644 --- a/examples/demos_by_system/car_propulsion/longitudinal_car_with_torque_input.py +++ b/examples/demos_by_system/car_propulsion/longitudinal_car_with_torque_input.py @@ -8,10 +8,10 @@ ############################################################################### import numpy as np ############################################################################### -from pyro.dynamic import longitudinal_vehicule +from pyro.dynamic import vehicle_propulsion #################################### -sys = longitudinal_vehicule.LongitudinalFrontWheelDriveCarWithTorqueInput() +sys = vehicle_propulsion.LongitudinalFrontWheelDriveCarWithTorqueInput() sys.x0 = np.array([0,0.1,0,0]) diff --git a/examples/demos_by_system/car_steering/bicycle.py b/examples/demos_by_system/car_steering/bicycle.py index 6f804f71..83458308 100644 --- a/examples/demos_by_system/car_steering/bicycle.py +++ b/examples/demos_by_system/car_steering/bicycle.py @@ -8,11 +8,11 @@ ############################################################################### import numpy as np ############################################################################### -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering ############################################################################### # Vehicule dynamical system -sys = vehicle.KinematicBicyleModel() +sys = vehicle_steering.KinematicBicyleModel() # Set default wheel velocity and steering angle sys.ubar = np.array([2,0.1]) diff --git a/examples/demos_by_system/car_steering/bicycle_exploration_with_rrt.py b/examples/demos_by_system/car_steering/bicycle_exploration_with_rrt.py index b2ed2604..42131f18 100644 --- a/examples/demos_by_system/car_steering/bicycle_exploration_with_rrt.py +++ b/examples/demos_by_system/car_steering/bicycle_exploration_with_rrt.py @@ -8,11 +8,11 @@ ############################################################################### import numpy as np ############################################################################### -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering from pyro.planning import randomtree ############################################################################### -sys = vehicle.KinematicBicyleModel() +sys = vehicle_steering.KinematicBicyleModel() ############################################################################### diff --git a/examples/demos_by_system/car_steering/bicycle_parallel_parking_with_rrt.py b/examples/demos_by_system/car_steering/bicycle_parallel_parking_with_rrt.py index f81744f9..24a5aa24 100644 --- a/examples/demos_by_system/car_steering/bicycle_parallel_parking_with_rrt.py +++ b/examples/demos_by_system/car_steering/bicycle_parallel_parking_with_rrt.py @@ -7,11 +7,11 @@ ############################################################################### import numpy as np ############################################################################### -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering from pyro.planning import randomtree ############################################################################### -sys = vehicle.KinematicBicyleModel() +sys = vehicle_steering.KinematicBicyleModel() sys.dynamic_domain = False ############################################################################### diff --git a/examples/demos_by_system/car_steering/car.py b/examples/demos_by_system/car_steering/car.py index 25c61a02..00d41ba1 100644 --- a/examples/demos_by_system/car_steering/car.py +++ b/examples/demos_by_system/car_steering/car.py @@ -9,12 +9,12 @@ ############################################################################### import numpy as np ############################################################################### -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering ############################################################################### # Vehicule dynamical system #sys = vehicle.KinematicCarModel() -sys = vehicle.KinematicCarModelwithObstacles() +sys = vehicle_steering.KinematicCarModelwithObstacles() # Set default wheel velocity and steering angle sys.ubar = np.array([2,0.2]) diff --git a/examples/demos_by_system/car_steering/car_trajectory_optimisation.py b/examples/demos_by_system/car_steering/car_trajectory_optimisation.py index 72af987b..e4e08112 100644 --- a/examples/demos_by_system/car_steering/car_trajectory_optimisation.py +++ b/examples/demos_by_system/car_steering/car_trajectory_optimisation.py @@ -8,7 +8,7 @@ import numpy as np -from pyro.dynamic.vehicle import KinematicCarModel +from pyro.dynamic.vehicle_steering import KinematicCarModel from pyro.planning.trajectoryoptimisation import DirectCollocationTrajectoryOptimisation @@ -34,7 +34,8 @@ planner.x_goal = np.array([ 0,0,0]) planner.maxiter = 1000 +# planner.ini planner.compute_optimal_trajectory() -planner.show_solution() +# planner.show_solution() planner.animate_solution() diff --git a/examples/demos_by_system/car_steering/car_trajectory_with_rrt.py b/examples/demos_by_system/car_steering/car_trajectory_with_rrt.py index 6980b562..50c8870d 100644 --- a/examples/demos_by_system/car_steering/car_trajectory_with_rrt.py +++ b/examples/demos_by_system/car_steering/car_trajectory_with_rrt.py @@ -8,11 +8,11 @@ ############################################################################### import numpy as np ############################################################################### -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering from pyro.planning import randomtree ############################################################################### -sys = vehicle.KinematicCarModelwithObstacles() +sys = vehicle_steering.KinematicCarModelwithObstacles() ############################################################################### diff --git a/examples/demos_by_system/car_steering/car_with_custom_lateral_controller.py b/examples/demos_by_system/car_steering/car_with_custom_lateral_controller.py index deb3f72f..81d5532d 100644 --- a/examples/demos_by_system/car_steering/car_with_custom_lateral_controller.py +++ b/examples/demos_by_system/car_steering/car_with_custom_lateral_controller.py @@ -9,12 +9,12 @@ ############################################################################### import numpy as np ############################################################################### -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering from pyro.control import controller ############################################################################### # Vehicule dynamical system -sys = vehicle.ConstantSpeedKinematicCarModel() +sys = vehicle_steering.ConstantSpeedKinematicCarModel() class CarController( controller.StaticController ) : diff --git a/examples/demos_by_system/car_steering/car_with_valueiteration_minimum_time.py b/examples/demos_by_system/car_steering/car_with_valueiteration_minimum_time.py index d3447744..83932915 100755 --- a/examples/demos_by_system/car_steering/car_with_valueiteration_minimum_time.py +++ b/examples/demos_by_system/car_steering/car_with_valueiteration_minimum_time.py @@ -12,10 +12,10 @@ from pyro.analysis import costfunction from pyro.planning import dynamicprogramming from pyro.planning import discretizer -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering ############################################################################### -sys = vehicle.KinematicCarModelwithObstacles() +sys = vehicle_steering.KinematicCarModelwithObstacles() # Set domain sys.x_ub = np.array([+35, +3, +3]) diff --git a/examples/demos_by_system/car_steering/car_with_valueiteration_quadratic_cost.py b/examples/demos_by_system/car_steering/car_with_valueiteration_quadratic_cost.py index ee0fd70d..e7992e25 100755 --- a/examples/demos_by_system/car_steering/car_with_valueiteration_quadratic_cost.py +++ b/examples/demos_by_system/car_steering/car_with_valueiteration_quadratic_cost.py @@ -11,10 +11,10 @@ from pyro.analysis import costfunction from pyro.planning import dynamicprogramming from pyro.planning import discretizer -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering ############################################################################### -sys = vehicle.KinematicCarModelwithObstacles() +sys = vehicle_steering.KinematicCarModelwithObstacles() # Set domain sys.x_ub = np.array([+35, +3, +3]) diff --git a/examples/demos_by_system/cartpole/cartpole_with_trajectory_optimization.py b/examples/demos_by_system/cartpole/cartpole_with_trajectory_optimization.py index 2327a207..467343c7 100644 --- a/examples/demos_by_system/cartpole/cartpole_with_trajectory_optimization.py +++ b/examples/demos_by_system/cartpole/cartpole_with_trajectory_optimization.py @@ -43,7 +43,8 @@ planner.x_start = np.array([0,0,0,0]) planner.x_goal = np.array([0,np.pi,0,0]) +planner.init_dynamic_plot() planner.maxiter = 500 planner.compute_optimal_trajectory() -planner.show_solution() +# planner.show_solution() planner.animate_solution() \ No newline at end of file diff --git a/examples/demos_by_system/drone/planar_drone_trajectory_optimisation.py b/examples/demos_by_system/drone/planar_drone_trajectory_optimisation.py index 4046621b..2899c344 100644 --- a/examples/demos_by_system/drone/planar_drone_trajectory_optimisation.py +++ b/examples/demos_by_system/drone/planar_drone_trajectory_optimisation.py @@ -14,12 +14,16 @@ sys = Drone2D() +sys.u_ub = np.array([10.0, 10.0]) + planner = DirectCollocationTrajectoryOptimisation( sys ) planner.x_start = np.array([-5,0,0,0,0,0]) planner.x_goal = np.array([0,0,0,0,0,0]) +planner.maxiter = 200 +planner.init_dynamic_plot() planner.compute_optimal_trajectory() -planner.show_solution() +# planner.show_solution() planner.animate_solution() diff --git a/examples/demos_by_system/drone/planar_drone_trajectory_optimisation_linearized.py b/examples/demos_by_system/drone/planar_drone_trajectory_optimisation_linearized.py index cf0ae964..5b2c2be1 100644 --- a/examples/demos_by_system/drone/planar_drone_trajectory_optimisation_linearized.py +++ b/examples/demos_by_system/drone/planar_drone_trajectory_optimisation_linearized.py @@ -12,6 +12,8 @@ from pyro.planning.trajectoryoptimisation import DirectCollocationTrajectoryOptimisation from pyro.dynamic.statespace import linearize +from pyro.analysis.costfunction import QuadraticCostFunctionVectorized + # Non-linear model sys = Drone2D() @@ -22,8 +24,9 @@ # Linear model ss = linearize( sys , 0.01 ) +cf = QuadraticCostFunctionVectorized( sys.n, sys.m ) -planner = DirectCollocationTrajectoryOptimisation( ss ) +planner = DirectCollocationTrajectoryOptimisation( ss , cost_function = cf ) planner.x_start = np.array([-5,0,0,0,0,0]) planner.x_goal = np.array([0,0,0,0,0,0]) diff --git a/examples/demos_by_system/holonomic_mobile_robot/holonomic_mobile_robot_exploration_with_obstacles_with_rrt.py b/examples/demos_by_system/holonomic_mobile_robot/holonomic_mobile_robot_exploration_with_obstacles_with_rrt.py index 2e398ec0..8b403799 100644 --- a/examples/demos_by_system/holonomic_mobile_robot/holonomic_mobile_robot_exploration_with_obstacles_with_rrt.py +++ b/examples/demos_by_system/holonomic_mobile_robot/holonomic_mobile_robot_exploration_with_obstacles_with_rrt.py @@ -7,10 +7,10 @@ import numpy as np -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering from pyro.planning import randomtree -sys = vehicle.HolonomicMobileRobotwithObstacles() +sys = vehicle_steering.HolonomicMobileRobotwithObstacles() sys.obstacles = [ [ (2,2),(4,10)], diff --git a/examples/demos_by_system/holonomic_mobile_robot/holonomic_mobile_robot_exploration_with_rrt.py b/examples/demos_by_system/holonomic_mobile_robot/holonomic_mobile_robot_exploration_with_rrt.py index af8efabb..e8be27ef 100644 --- a/examples/demos_by_system/holonomic_mobile_robot/holonomic_mobile_robot_exploration_with_rrt.py +++ b/examples/demos_by_system/holonomic_mobile_robot/holonomic_mobile_robot_exploration_with_rrt.py @@ -7,10 +7,10 @@ import numpy as np -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering from pyro.planning import randomtree -sys = vehicle.HolonomicMobileRobot() +sys = vehicle_steering.HolonomicMobileRobot() x_start = np.array([0,0]) x_goal = np.array([8,8]) diff --git a/examples/demos_by_system/holonomic_mobile_robot/holonomic_mobile_robot_with_valueiteration.py b/examples/demos_by_system/holonomic_mobile_robot/holonomic_mobile_robot_with_valueiteration.py index 4d7c4c07..6ad0d221 100644 --- a/examples/demos_by_system/holonomic_mobile_robot/holonomic_mobile_robot_with_valueiteration.py +++ b/examples/demos_by_system/holonomic_mobile_robot/holonomic_mobile_robot_with_valueiteration.py @@ -7,12 +7,12 @@ import numpy as np -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering from pyro.planning import discretizer from pyro.analysis import costfunction from pyro.planning import dynamicprogramming -sys = vehicle.HolonomicMobileRobotwithObstacles() +sys = vehicle_steering.HolonomicMobileRobotwithObstacles() # Discrete world grid_sys = discretizer.GridDynamicSystem( sys , (51,51) , (3,3) ) diff --git a/examples/demos_by_system/pendulum_double/double_pendulum.py b/examples/demos_by_system/pendulum_double/double_pendulum.py index 6fe77040..80675298 100644 --- a/examples/demos_by_system/pendulum_double/double_pendulum.py +++ b/examples/demos_by_system/pendulum_double/double_pendulum.py @@ -15,7 +15,9 @@ sys = pendulum.DoublePendulum() # Simultation -sys.x0 = np.array([-0.1,0,0,0]) +sys.x0 = np.array([-0.1,0,0,0]) +sys.ubar = np.array([-0.1,0.0]) + sys.plot_trajectory() sys.plot_phase_plane_trajectory(0, 2) sys.animate_simulation() \ No newline at end of file diff --git a/examples/demos_by_system/pendulum_double/double_pendulum_with_trajectory_optimization.py b/examples/demos_by_system/pendulum_double/double_pendulum_with_trajectory_optimization.py index 955f3e41..6de3dcd5 100644 --- a/examples/demos_by_system/pendulum_double/double_pendulum_with_trajectory_optimization.py +++ b/examples/demos_by_system/pendulum_double/double_pendulum_with_trajectory_optimization.py @@ -26,6 +26,8 @@ planner.x_goal = np.array([0,0,0,0]) planner.maxiter = 500 +planner.set_linear_initial_guest(True) +planner.init_dynamic_plot() planner.compute_optimal_trajectory() -planner.show_solution() +# planner.show_solution() planner.animate_solution() \ No newline at end of file diff --git a/examples/demos_by_system/pendulum_simple/simple_pendulum_with_valueiteration_minimum_time.py b/examples/demos_by_system/pendulum_simple/simple_pendulum_with_valueiteration_minimum_time.py index fdb47404..ae9668d5 100755 --- a/examples/demos_by_system/pendulum_simple/simple_pendulum_with_valueiteration_minimum_time.py +++ b/examples/demos_by_system/pendulum_simple/simple_pendulum_with_valueiteration_minimum_time.py @@ -46,7 +46,7 @@ #dp.animate_policy( show = True , save = False ) #dp.animate_cost2go( show = False , save = True ) -#dp.animate_policy( show = False , save = True ) +dp.animate_policy( show = False , save = True ) ctl = dp.get_lookup_table_controller() diff --git a/examples/demos_by_system/plane/plane_cobra.py b/examples/demos_by_system/plane/plane_cobra.py new file mode 100644 index 00000000..6e3f9ea8 --- /dev/null +++ b/examples/demos_by_system/plane/plane_cobra.py @@ -0,0 +1 @@ +#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Created on Fri Sep 8 10:38:30 2023 @author: alex """ import numpy as np from pyro.dynamic import plane sys = plane.Plane2D() sys.plot_alpha2Cl() sys.x0 = np.array([0,0,0.2,15,0,0]) sys.inertia = 1.0 def t2u(t): u = np.array([ 2 * t , -0.12 * t ]) return u sys.t2u = t2u #sys.gravity = 0 sys.compute_trajectory( 10 , 20001 , 'euler' ) sys.plot_trajectory('x') # sys.dynamic_domain = False sys.animate_simulation( time_factor_video=0.5 ) \ No newline at end of file diff --git a/examples/demos_by_system/plane/plane_simple_controller.py b/examples/demos_by_system/plane/plane_simple_controller.py new file mode 100644 index 00000000..1c48dfb1 --- /dev/null +++ b/examples/demos_by_system/plane/plane_simple_controller.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Fri Sep 8 10:38:30 2023 + +@author: alex +""" + +import numpy as np + +from pyro.dynamic import plane + +from pyro.control import controller + + + + +############################################################################### + +class PLaneController( controller.StaticController ) : + + ############################ + def __init__( self ): + """ """ + + # Dimensions + self.k = 1 + self.m = 2 + self.p = 6 + + super().__init__(self.k, self.m, self.p) + + # Label + self.name = 'Plane Controller' + + self.v_ref = 40 + self.y_ref = 0.0 + + + ############################# + def c( self , y , r , t = 0 ): + """ + Feedback static computation u = c(y,r,t) + + INPUTS + y : sensor signal_proc vector p x 1 + r : reference signal_proc vector k x 1 + t : time 1 x 1 + + OUPUTS + u : control inputs vector m x 1 + + """ + + v = y[3] + theta = y[2] + y = y[1] + + T = +10 * ( self.v_ref - v ) + + theta_ref = +0.1 * ( self.y_ref - y ) + + delta = -0.5 * ( theta_ref - theta ) + + u = np.array([ T , delta ]) + + + return u + + +sys = plane.Plane2D() +sys.l_w = 0.0 + +sys.x0 = np.array([0,0,0.2,15,0,0]) + +ctl = PLaneController() + +cl_sys = ctl + sys + +cl_sys.compute_trajectory( 2 ) + +cl_sys.plot_trajectory() +cl_sys.animate_simulation( time_factor_video = 0.2 ) + + + diff --git a/examples/demos_by_system/suspension/suspension.py b/examples/demos_by_system/suspension/suspension.py new file mode 100644 index 00000000..8fe9ee27 --- /dev/null +++ b/examples/demos_by_system/suspension/suspension.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Sun Oct 16 22:27:47 2022 + +@author: alex +""" + +import numpy as np + +from pyro.dynamic import suspension + +sys = suspension.QuarterCarOnRoughTerrain() + +sys.k = 10.0 +sys.vx = 10.0 + + +# Simulation and animation +sys.compute_trajectory( 10, 10001, 'euler') +sys.plot_trajectory('xu') +sys.animate_simulation() \ No newline at end of file diff --git a/examples/demos_by_tool/dynamicprogramming/2D_navigation.py b/examples/demos_by_tool/dynamicprogramming/2D_navigation.py index 9caadf92..5492248a 100644 --- a/examples/demos_by_tool/dynamicprogramming/2D_navigation.py +++ b/examples/demos_by_tool/dynamicprogramming/2D_navigation.py @@ -8,12 +8,12 @@ import numpy as np -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering from pyro.analysis import costfunction from pyro.planning import dynamicprogramming from pyro.planning import discretizer -sys = vehicle.HolonomicMobileRobotwithObstacles() +sys = vehicle_steering.HolonomicMobileRobotwithObstacles() #sys.obstacles[1][0] = (5,5) diff --git a/examples/demos_by_tool/dynamicprogramming/braking_reachability.py b/examples/demos_by_tool/dynamicprogramming/braking_reachability.py index c4a752b8..291b14ab 100644 --- a/examples/demos_by_tool/dynamicprogramming/braking_reachability.py +++ b/examples/demos_by_tool/dynamicprogramming/braking_reachability.py @@ -8,12 +8,12 @@ import numpy as np -from pyro.dynamic import longitudinal_vehicule +from pyro.dynamic import vehicle_propulsion from pyro.analysis import costfunction from pyro.planning import dynamicprogramming from pyro.planning import discretizer -sys = longitudinal_vehicule.LongitudinalFrontWheelDriveCarWithWheelSlipInput() +sys = vehicle_propulsion.LongitudinalFrontWheelDriveCarWithWheelSlipInput() sys.x_ub[0] = 60 sys.x_lb[0] = 0 diff --git a/examples/demos_by_tool/dynamicprogramming/car_braking.py b/examples/demos_by_tool/dynamicprogramming/car_braking.py index 6abd3e28..82b52c3c 100644 --- a/examples/demos_by_tool/dynamicprogramming/car_braking.py +++ b/examples/demos_by_tool/dynamicprogramming/car_braking.py @@ -8,12 +8,12 @@ import numpy as np -from pyro.dynamic import longitudinal_vehicule +from pyro.dynamic import vehicle_propulsion from pyro.analysis import costfunction from pyro.planning import dynamicprogramming from pyro.planning import discretizer -sys = longitudinal_vehicule.LongitudinalFrontWheelDriveCarWithWheelSlipInput() +sys = vehicle_propulsion.LongitudinalFrontWheelDriveCarWithWheelSlipInput() sys.x_ub[1] = 15 sys.x_lb[1] = 0 diff --git a/examples/demos_by_tool/dynamicprogramming/car_parking.py b/examples/demos_by_tool/dynamicprogramming/car_parking.py index e5d80437..0bc8c9a0 100644 --- a/examples/demos_by_tool/dynamicprogramming/car_parking.py +++ b/examples/demos_by_tool/dynamicprogramming/car_parking.py @@ -11,9 +11,9 @@ from pyro.analysis import costfunction from pyro.planning import dynamicprogramming from pyro.planning import discretizer -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering -sys = vehicle.KinematicCarModelwithObstacles() +sys = vehicle_steering.KinematicCarModelwithObstacles() # Set domain sys.x_ub = np.array([+35, +3, +3]) diff --git a/examples/demos_by_tool/dynamicprogramming/pendulum_optimal_swingup_demo.py b/examples/demos_by_tool/dynamicprogramming/pendulum_optimal_swingup_demo.py new file mode 100644 index 00000000..cee002f8 --- /dev/null +++ b/examples/demos_by_tool/dynamicprogramming/pendulum_optimal_swingup_demo.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Sun Oct 16 22:27:47 2022 + +@author: alex +""" + +import numpy as np + +from pyro.dynamic import pendulum +from pyro.analysis import costfunction +from pyro.planning import dynamicprogramming +from pyro.planning import discretizer + +sys = pendulum.SinglePendulum() + +sys.x_ub = np.array([+10, +10]) +sys.x_lb = np.array([-10, -10]) + +# Discrete world +grid_sys = discretizer.GridDynamicSystem( sys , [201,201] , [21] ) + +# Cost Function +qcf = costfunction.QuadraticCostFunction.from_sys(sys) + +qcf.xbar = np.array([ -3.14 , 0 ]) # target +qcf.INF = 500 + +qcf.R[0,0] = 1.0 + +qcf.S[0,0] = 10.0 +qcf.S[1,1] = 10.0 + + +# DP algo +dp = dynamicprogramming.DynamicProgrammingWithLookUpTable( grid_sys, qcf) +#dp = dprog.DynamicProgramming2DRectBivariateSpline(grid_sys, qcf) + +#dp.solve_bellman_equation( animate_cost2go = True ) + +#dp.compute_steps(200) +# dp.plot_policy() + +#dp.solve_bellman_equation( tol = 1) +dp.solve_bellman_equation( tol = 0.1 , animate_cost2go = True ) +# dp.solve_bellman_equation( tol = 1 , animate_policy = True ) +#dp.plot_cost2go(150) + +#dp.animate_cost2go( show = False , save = True ) +#dp.animate_policy( show = False , save = True ) + +dp.clean_infeasible_set() +dp.plot_cost2go_3D() +dp.plot_policy() + +ctl = dp.get_lookup_table_controller() + + +#ctl.plot_control_law( sys = sys , n = 100) + + +#asign controller +cl_sys = ctl + sys +cl_sys.x0 = np.array([0., 0.]) +cl_sys.compute_trajectory( 10, 10001, 'euler') +cl_sys.plot_trajectory('xu') +cl_sys.plot_phase_plane_trajectory() +cl_sys.animate_simulation() diff --git a/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_dp_vs_ppo_bangbang.py b/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_dp_vs_ppo_bangbang.py new file mode 100644 index 00000000..adb5c282 --- /dev/null +++ b/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_dp_vs_ppo_bangbang.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Fri Mar 1 14:08:03 2024 + +@author: alex +""" + +import numpy as np +import matplotlib.pyplot as plt + +from pyro.dynamic import pendulum +from pyro.control import controller +from stable_baselines3 import PPO +from stable_baselines3.common.env_util import make_vec_env + +sys = pendulum.InvertedPendulum() + +# Physical parameters +sys.gravity = 10.0 +sys.m1 = 1.0 +sys.l1 = 1.0 +sys.lc1 = 0.5 * sys.l1 +sys.I1 = (1.0 / 12.0) * sys.m1 * sys.l1**2 + +sys.l_domain = 2 * sys.l1 # graphical domain + +# Min/max state and control inputs +sys.x_ub = np.array([+2.0 * np.pi, +12]) +sys.x_lb = np.array([-2.0 * np.pi, -12]) +sys.u_ub = np.array([+8.0]) +sys.u_lb = np.array([-8.0]) + +# Time constant +dt = 0.05 + +# Cost Function +# The reward function is defined as: r = -(theta2 + 0.1 * theta_dt2 + 0.001 * torque2) +sys.cost_function.xbar = np.array([0, 0]) # target +sys.cost_function.R[0, 0] = 0.001 / dt +sys.cost_function.Q[0, 0] = 1.0 / dt +sys.cost_function.Q[1, 1] = 0.1 / dt + +sys.x0 = np.array([-np.pi, 0.0]) + +# DP solution +from pyro.planning import discretizer +from pyro.planning import dynamicprogramming + +grid_sys = discretizer.GridDynamicSystem(sys, [101, 101], [11]) + +dp = dynamicprogramming.DynamicProgrammingWithLookUpTable(grid_sys, sys.cost_function) + +dp.solve_bellman_equation(tol=0.01) +dp.clean_infeasible_set() +dp.plot_policy() +dp.plot_cost2go_3D(jmax=5000) + +# Learning +gym_env = sys.convert_to_gymnasium(dt=dt, render_mode=None) + +gym_env.clipping_states = True # To reproduce the behavior of gym pendulum + +gym_env.reset_mode = "uniform" +gym_env.x0_lb = np.array([-np.pi , -1.0]) +gym_env.x0_ub = np.array([+np.pi , +1.0]) + +model = PPO("MlpPolicy", gym_env, verbose=1) + +#model.load('pendulum_dp_vs_ppo_bangbang') + +from pyro.control.reinforcementlearning import stable_baseline3_controller + +ppo_ctl = stable_baseline3_controller(model) + + +ppo_ctl.plot_control_law(sys=sys, n=100) +plt.show() +plt.pause(0.001) + +n_time_steps = 2.5E5 +batches = 3 +gym_env.render_mode = None +for batch in range(batches): + model.learn(total_timesteps=int(n_time_steps / batches)) + ppo_ctl.plot_control_law(sys=sys, n=100) + plt.show() + plt.pause(0.001) + +# Save the model +model.save('pendulum_dp_vs_ppo_bangbang') + +# Animating rl closed-loop +cl_sys = ppo_ctl + sys + +cl_sys.x0 = np.array([-3.2, -0.0]) +cl_sys.compute_trajectory(tf=10.0, n=10000, solver="euler") +cl_sys.plot_trajectory("xu") +cl_sys.animate_simulation() diff --git a/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_dp_vs_ppo_pump.py b/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_dp_vs_ppo_pump.py new file mode 100644 index 00000000..074488c8 --- /dev/null +++ b/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_dp_vs_ppo_pump.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Fri Mar 1 14:08:03 2024 + +@author: alex +""" + +import numpy as np +import matplotlib.pyplot as plt + +from pyro.dynamic import pendulum +from pyro.control import controller +from stable_baselines3 import PPO +from stable_baselines3.common.env_util import make_vec_env + +sys = pendulum.InvertedPendulum() + +# Physical parameters +sys.gravity = 10.0 +sys.m1 = 1.0 +sys.l1 = 1.0 +sys.lc1 = 0.5 * sys.l1 +sys.I1 = (1.0 / 12.0) * sys.m1 * sys.l1**2 + +sys.l_domain = 2 * sys.l1 # graphical domain + +# Min/max state and control inputs +sys.x_ub = np.array([+2.0 * np.pi, +12]) +sys.x_lb = np.array([-2.0 * np.pi, -12]) +sys.u_ub = np.array([+2.0]) +sys.u_lb = np.array([-2.0]) + +# Time constant +dt = 0.05 + +# Cost Function +# The reward function is defined as: r = -(theta2 + 0.1 * theta_dt2 + 0.001 * torque2) +sys.cost_function.xbar = np.array([0, 0]) # target +sys.cost_function.R[0, 0] = 0.0 / dt +sys.cost_function.Q[0, 0] = 1.0 / dt +sys.cost_function.Q[1, 1] = 0.1 / dt + +sys.x0 = np.array([-np.pi, 0.0]) + +# DP solution +from pyro.planning import discretizer +from pyro.planning import dynamicprogramming + +grid_sys = discretizer.GridDynamicSystem(sys, [201, 201], [21]) + +dp = dynamicprogramming.DynamicProgrammingWithLookUpTable(grid_sys, sys.cost_function) + +dp.solve_bellman_equation(tol=0.01) +dp.clean_infeasible_set() +dp.plot_policy() +dp.plot_cost2go_3D(jmax=5000) + +# Learning +gym_env = sys.convert_to_gymnasium(dt=dt, render_mode=None) + +gym_env.clipping_states = True # To reproduce the behavior of gym pendulum + +gym_env.reset_mode = "uniform" +gym_env.x0_lb = np.array([-np.pi , -1.0]) +gym_env.x0_ub = np.array([+np.pi , +1.0]) + +model = PPO("MlpPolicy", gym_env, verbose=1) + +#model.load('pendulum_dp_vs_ppo_bangbang') + +from pyro.control.reinforcementlearning import stable_baseline3_controller + +ppo_ctl = stable_baseline3_controller(model) + + +ppo_ctl.plot_control_law(sys=sys, n=100) +plt.show() +plt.pause(0.001) + +n_time_steps = 2.0E6 +batches = 4 +gym_env.render_mode = None +for batch in range(batches): + model.learn(total_timesteps=int(n_time_steps / batches)) + ppo_ctl.plot_control_law(sys=sys, n=100) + plt.show() + plt.pause(0.001) + +# Save the model +model.save('pendulum_dp_vs_ppo_bangbang') + +# Animating rl closed-loop +cl_sys = ppo_ctl + sys + +cl_sys.x0 = np.array([-3.2, -0.0]) +cl_sys.compute_trajectory(tf=50.0, n=10000, solver="euler") +cl_sys.plot_trajectory("xu") +cl_sys.animate_simulation() diff --git a/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_dp_vs_ppo_tmotor_bangbang.py b/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_dp_vs_ppo_tmotor_bangbang.py new file mode 100644 index 00000000..acb2517d --- /dev/null +++ b/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_dp_vs_ppo_tmotor_bangbang.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Fri Mar 1 14:08:03 2024 + +@author: alex +""" + +import numpy as np +import matplotlib.pyplot as plt + +from pyro.dynamic import pendulum +from pyro.control import controller +from stable_baselines3 import PPO +from stable_baselines3.common.env_util import make_vec_env + +sys = pendulum.InvertedPendulum() + +# Physical parameters of the prototype +sys.gravity = 9.8 +sys.m1 = 0.300 +sys.l1 = 0.4 +sys.lc1 = 0.4 +sys.I1 = 0.0 + +max_torque = 3.0 + +sys.l_domain = 2 * sys.l1 # graphical domain + +# Min/max state and control inputs +sys.x_ub = np.array([+2 * np.pi, +12]) +sys.x_lb = np.array([-2 * np.pi, -12]) +sys.u_ub = np.array([+max_torque]) +sys.u_lb = np.array([-max_torque]) + +# Cost Function +sys.cost_function.xbar = np.array([0, 0]) # target +sys.cost_function.R[0, 0] = 0.0 +sys.cost_function.Q[0, 0] = 1.0 +sys.cost_function.Q[1, 1] = 0.0 + +# DP solution +from pyro.planning import discretizer +from pyro.planning import dynamicprogramming + +grid_sys = discretizer.GridDynamicSystem(sys, [201, 201], [21]) + +dp = dynamicprogramming.DynamicProgrammingWithLookUpTable(grid_sys, sys.cost_function) + +dp.solve_bellman_equation(tol=0.01) +dp.clean_infeasible_set() +dp.plot_policy() +dp.plot_cost2go(jmax=10) + +# Learning +env = sys.convert_to_gymnasium(dt=0.05, render_mode=None) + +env.clipping_states = True # test + +model = PPO("MlpPolicy", env, verbose=1) + +from pyro.control.reinforcementlearning import stable_baseline3_controller + +ppo_ctl = stable_baseline3_controller(model) + + +ppo_ctl.plot_control_law(sys=sys, n=100) +plt.show() +plt.pause(0.001) + +n_time_steps = 250000 +batches = 5 +env.render_mode = None +for batch in range(batches): + model.learn(total_timesteps=int(n_time_steps / batches)) + ppo_ctl.plot_control_law(sys=sys, n=100) + plt.show() + plt.pause(0.001) + +# Animating rl closed-loop +cl_sys = ppo_ctl + sys + +cl_sys.x0 = np.array([-3.14, -0.0]) +cl_sys.compute_trajectory(tf=10.0, n=10000, solver="euler") +cl_sys.plot_trajectory("xu") +cl_sys.animate_simulation() diff --git a/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_with_PPO_baseline_gym_example.py b/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_with_PPO_baseline_gym_example.py new file mode 100644 index 00000000..e44987e5 --- /dev/null +++ b/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_with_PPO_baseline_gym_example.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Tue Feb 13 13:37:57 2024 + +@author: alex +""" + +import gymnasium as gym + +from stable_baselines3 import PPO +from stable_baselines3.common.env_util import make_vec_env + +# Parallel environments +vec_env = make_vec_env("Pendulum-v1", n_envs=1) + +model = PPO("MlpPolicy", vec_env, verbose=1) +model.learn(total_timesteps=250000) + +obs = vec_env.reset() + +while True: + action, _states = model.predict(obs) + obs, rewards, dones, info = vec_env.step(action) + vec_env.render("human") \ No newline at end of file diff --git a/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_with_PPO_baseline_pyro_reproduction.py b/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_with_PPO_baseline_pyro_reproduction.py new file mode 100644 index 00000000..b2bf8a89 --- /dev/null +++ b/examples/demos_by_tool/rl_with_stable_baseline3/pendulum_with_PPO_baseline_pyro_reproduction.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Fri Mar 1 14:08:03 2024 + +@author: alex +""" + +import numpy as np +import matplotlib.pyplot as plt + +import gymnasium as gym +from gymnasium import spaces + +from pyro.dynamic import pendulum + +from stable_baselines3 import PPO +from stable_baselines3.common.env_util import make_vec_env + + +class SysEnv(gym.Env): + + def __init__(self, sys, dt=0.1, tf=10.0, render_mode=None): + + # x-y ouputs + y_ub = np.array([+1, +1, sys.x_ub[1]]) + y_lb = np.array([-1, -1, sys.x_lb[1]]) + + self.observation_space = spaces.Box(y_lb, y_ub) + self.action_space = spaces.Box(sys.u_lb, sys.u_ub) + + self.sys = sys + self.dt = dt + + self.tf = tf + self.render_mode = render_mode + + # Memory + self.x = sys.x0 + self.u = sys.ubar + self.t = 0.0 + + if self.render_mode == "human": + + self.animator = self.sys.get_animator() + self.animator.show_plus(self.x, self.u, self.t) + plt.pause(0.001) + + def _get_obs(self): + + theta = self.x[0] + thetadot = self.x[1] + + y = np.array([np.cos(theta), np.sin(theta), thetadot], dtype=np.float32) + + return y + + def _get_info(self): + + return {"state": self.x, "action": self.u} + + def reset(self, seed=None, options=None): + + super().reset(seed=seed) + + self.x = np.random.uniform(np.array([-np.pi, -1]), np.array([np.pi, 1])) + self.u = self.sys.ubar + self.t = 0.0 + + observation = self._get_obs() + info = self._get_info() + + return observation, info + + def step(self, u): + + u = np.clip(u, self.sys.u_lb, self.sys.u_ub) + x = self.x + t = self.t + dt = self.dt + + # Derivatives + dx = self.sys.f(x, u, t) + + # Euler integration + x_new = x + dx * dt + t_new = t + dt + + x_new[0] = self.angle_normalize(x_new[0]) + + # Sat speed --> I hate they do this in gym env + if x_new[1] > sys.x_ub[1]: + x_new[1] = sys.x_ub[1] + if x_new[1] < sys.x_lb[1]: + x_new[1] = sys.x_lb[1] + + # Cost function + r = -self.sys.cost_function.g(x, u, t) + + terminated = False # t > self.tf + + truncated = t > self.tf # False #not self.sys.isavalidstate(x_new) + + # Memory update + self.x = x_new + self.t = t_new + self.u = u + + y = self._get_obs() + info = self._get_info() + + if self.render_mode == "human": + self._render_frame() + + return y, r, terminated, truncated, info + + def angle_normalize(self, x): + + return ((x + np.pi) % (2 * np.pi)) - np.pi + + def render(self): + if self.render_mode == "human": + return self._render_frame() + + def _render_frame(self): + + self.animator.show_plus_update(self.x, self.u, self.t) + plt.pause(0.001) + + +sys = pendulum.InvertedPendulum() + +# Setting physical parameter to reflect the gym environment + +# Physical parameters +sys.gravity = 10.0 +sys.m1 = 1.0 +sys.l1 = 1.0 +sys.lc1 = 0.5 * sys.l1 +sys.I1 = (1.0 / 12.0) * sys.m1 * sys.l1**2 + +sys.l_domain = 2 * sys.l1 # graphical domain + +# Min/max state and control inputs +sys.x_ub = np.array([+np.pi, +8]) +sys.x_lb = np.array([-np.pi, -8]) +sys.u_ub = np.array([+2.0]) +sys.u_lb = np.array([-2.0]) + +# Cost Function +# The reward function is defined as: r = -(theta2 + 0.1 * theta_dt2 + 0.001 * torque2) +sys.cost_function.xbar = np.array([0, 0]) # target +sys.cost_function.R[0, 0] = 0.001 +sys.cost_function.Q[0, 0] = 1.0 +sys.cost_function.Q[1, 1] = 0.1 + +# Learning +gym_env = SysEnv(sys, dt=0.05, render_mode=None) +model = PPO("MlpPolicy", gym_env, verbose=1) +model.learn(total_timesteps=250000) + +# Animation of the pendulum with the learned policy +gym_env = SysEnv(sys, render_mode="human") +y, info = gym_env.reset() +episodes = 10 +for episode in range(episodes): + y, info = gym_env.reset() + terminated = False + truncated = False + print("\n Episode:", episode) + while not (terminated or truncated): + u, _states = model.predict(y, deterministic=True) + y, r, terminated, truncated, info = gym_env.step(u) + + \ No newline at end of file diff --git a/examples/demos_by_tool/trajectory_planning/double_pendulum_with_trajectory_optimization.py b/examples/demos_by_tool/trajectory_planning/double_pendulum_with_trajectory_optimization.py new file mode 100644 index 00000000..6de3dcd5 --- /dev/null +++ b/examples/demos_by_tool/trajectory_planning/double_pendulum_with_trajectory_optimization.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Sat Oct 30 14:04:51 2021 + +@author: alex +""" + +import numpy as np + +from pyro.dynamic.pendulum import DoublePendulum +from pyro.planning.trajectoryoptimisation import DirectCollocationTrajectoryOptimisation + + +sys = DoublePendulum() + +#Max/Min torque +sys.u_ub[0] = +20 +sys.u_ub[1] = +20 +sys.u_lb[0] = -20 +sys.u_lb[1] = -20 + +planner = DirectCollocationTrajectoryOptimisation( sys , 0.2 , 20 ) + +planner.x_start = np.array([3.14,0,0,0]) +planner.x_goal = np.array([0,0,0,0]) + +planner.maxiter = 500 +planner.set_linear_initial_guest(True) +planner.init_dynamic_plot() +planner.compute_optimal_trajectory() +# planner.show_solution() +planner.animate_solution() \ No newline at end of file diff --git a/examples/demos_by_tool/trajectory_planning/double_pendulum_with_trajectory_optimization_CUSTOM.py b/examples/demos_by_tool/trajectory_planning/double_pendulum_with_trajectory_optimization_CUSTOM.py deleted file mode 100644 index 369c56ae..00000000 --- a/examples/demos_by_tool/trajectory_planning/double_pendulum_with_trajectory_optimization_CUSTOM.py +++ /dev/null @@ -1,174 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Sep 10 11:12:11 2021 - -@author: alexandregirard -""" - -import numpy as np -from scipy.optimize import minimize - -from pyro.dynamic import pendulum - - - -sys = pendulum.DoublePendulum() - -n = 4 -m = 2 -grid = 20 -dt = 0.2 - - -#dec = np.linspace(0,grid*3,grid*3) - - -def dec2xu(dec): - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - x[2,:] = dec[2*grid:3*grid] - x[3,:] = dec[3*grid:4*grid] - u[0,:] = dec[4*grid:5*grid] - u[1,:] = dec[5*grid:6*grid] - - return x,u - -def cost(dec): - - J = 0 - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - x[2,:] = dec[2*grid:3*grid] - x[3,:] = dec[3*grid:4*grid] - u[0,:] = dec[4*grid:5*grid] - u[1,:] = dec[5*grid:6*grid] - - for i in range(grid-1): - - #J = J + 0.5 * dt * ( (x[0,i]-target)**2 + (x[0,i+1]-target)**2 + x[1,i]**2 + x[1,i+1]**2 + u[0,i]**2 + u[0,i+1]**2 ) - - J = J + 0.5 * dt * ( x[0,i]**2 + x[0,i+1]**2 + x[1,i]**2 + x[1,i+1]**2 ) - J = J + 0.5 * dt * ( x[2,i]**2 + x[2,i+1]**2 + x[3,i]**2 + x[3,i+1]**2 ) - J = J + 0.5 * dt * ( u[0,i]**2 + u[0,i+1]**2 + u[1,i]**2 + u[1,i+1]**2 ) * 5 - - return J - -def constraints(dec): - - x,u = dec2xu(dec) - - vec=np.zeros(grid*4-4) - - for i in range(grid-1): - - vec[i+grid*0-0] = (x[0,i+1] - x[0,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[0] + sys.f(x[:,i+1],u[:,i+1])[0] ) - vec[i+grid*1-1] = (x[1,i+1] - x[1,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[1] + sys.f(x[:,i+1],u[:,i+1])[1] ) - vec[i+grid*2-2] = (x[2,i+1] - x[2,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[2] + sys.f(x[:,i+1],u[:,i+1])[2] ) - vec[i+grid*3-3] = (x[3,i+1] - x[3,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[3] + sys.f(x[:,i+1],u[:,i+1])[3] ) - - return vec - - -def compute_bounds(): - - bounds = [] - - #x0 - bounds.append( (-3.14,-3.13) ) - - for i in range(1,grid-1): - - bounds.append( (-5,2) ) - - bounds.append( (0.0,0.01) ) - - #x1 - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-3,3) ) - - bounds.append( (0,0.01) ) - - #x2 - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-3,6) ) - - bounds.append( (0,0.01) ) - - #x3 - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-10,10) ) - - bounds.append( (0,0.01) ) - - #u0 - - for i in range(grid): - - bounds.append( (-20,30) ) - - #u1 - - for i in range(grid): - - bounds.append( (-20,30) ) - - return bounds - - -def display_callback(a): - - - print('One iteration completed') - - return True - - - - -# Guess -dec = np.zeros(grid*(n+m)) -#dec[0:grid] = traj.x[:,0] -#dec[grid:2*grid] = traj.x[:,1] -#dec[2*grid:3*grid] = traj.u[:,0] - -bnds = compute_bounds() - -cons = {'type': 'eq', 'fun': constraints } -res = minimize( cost, dec, method='SLSQP', bounds=bnds, constraints=cons, callback=display_callback, options={'disp':True,'maxiter':1000}) # - - -sys.compute_trajectory(grid*dt,grid) - - -dec = res.x - -sys.traj.x[:,0] = dec[0:grid] -sys.traj.x[:,1] = dec[grid:2*grid] -sys.traj.x[:,2] = dec[2*grid:3*grid] -sys.traj.x[:,3] = dec[3*grid:4*grid] -sys.traj.u[:,0] = dec[4*grid:5*grid] -sys.traj.u[:,1] = dec[5*grid:6*grid] - - -sys.plot_trajectory('xu') -sys.animate_simulation() \ No newline at end of file diff --git a/examples/demos_by_tool/trajectory_planning/linear_trajectory_optimisation.py b/examples/demos_by_tool/trajectory_planning/linear_trajectory_optimisation.py deleted file mode 100644 index 192f12d8..00000000 --- a/examples/demos_by_tool/trajectory_planning/linear_trajectory_optimisation.py +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Sun Oct 3 08:27:06 2021 - -@author: alex -""" - -import numpy as np - -from pyro.dynamic.massspringdamper import TwoMass -from pyro.planning.trajectoryoptimisation import DirectCollocationTrajectoryOptimisation - - -sys = TwoMass() - -sys.u_ub[0] = +2 -sys.u_lb[0] = 0 - -planner = DirectCollocationTrajectoryOptimisation( sys , 0.1, 40 ) - -planner.x_start = np.array([0.5,0.5,0,0]) -planner.x_goal = np.array([0,0,0,0]) - -planner.compute_optimal_trajectory() -planner.show_solution() -planner.animate_solution() - - diff --git a/examples/demos_by_tool/trajectory_planning/mountain_car_trajectory_optimization.py b/examples/demos_by_tool/trajectory_planning/mountain_car_trajectory_optimization.py new file mode 100644 index 00000000..19fdf808 --- /dev/null +++ b/examples/demos_by_tool/trajectory_planning/mountain_car_trajectory_optimization.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Fri Sep 29 15:03:52 2023 + +@author: alex +""" + +############################################################################## +import numpy as np +############################################################################## +from pyro.dynamic import mountaincar +from pyro.planning import trajectoryoptimisation +from pyro.analysis import costfunction +############################################################################## + +sys = mountaincar.MountainCar() + +sys.x_ub = np.array([+0.2,+2.0]) +sys.x_lb = np.array([-1.7,-2.0]) + +sys.u_ub[0] = +1.5 +sys.u_lb[0] = -1.5 + + +# Cost Function +qcf = costfunction.QuadraticCostFunction.from_sys(sys) + +qcf.xbar = np.array([ 0 , 0 ]) # target +qcf.INF = 30 + +qcf.R[0,0] = 10.0 + +qcf.S[0,0] = 10.0 +qcf.S[1,1] = 10.0 + +planner = trajectoryoptimisation.DirectCollocationTrajectoryOptimisation( sys , 0.1, 40 ) + +planner.x_start = np.array([-1.0,+0.0]) +planner.x_goal = np.array([+0.0,+0.0]) + +planner.init_dynamic_plot() +planner.set_linear_initial_guest() +planner.compute_optimal_trajectory() +# planner.show_solution() +planner.animate_solution() diff --git a/examples/demos_by_tool/trajectory_planning/simple_pendulum_trajectory_optimization_CUSTOM.py b/examples/demos_by_tool/trajectory_planning/simple_pendulum_trajectory_optimization_CUSTOM.py deleted file mode 100644 index afa64a63..00000000 --- a/examples/demos_by_tool/trajectory_planning/simple_pendulum_trajectory_optimization_CUSTOM.py +++ /dev/null @@ -1,120 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Created on Fri Sep 10 11:12:11 2021 - -@author: alexandregirard -""" - -import numpy as np -from scipy.optimize import minimize - -from pyro.dynamic import pendulum - - - -sys = pendulum.SinglePendulum() - -n = 2 -m = 1 -grid = 100 -dt = 0.1 - - -#dec = np.linspace(0,grid*3,grid*3) - - -def dec2xu(dec): - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - u[0,:] = dec[2*grid:3*grid] - - return x,u - -def cost(dec): - - J = 0 - - x = np.zeros((n,grid)) - u = np.zeros((m,grid)) - - x[0,:] = dec[0:grid] - x[1,:] = dec[grid:2*grid] - u[0,:] = dec[2*grid:3*grid] - - for i in range(grid-1): - - #J = J + 0.5 * dt * ( (x[0,i]-target)**2 + (x[0,i+1]-target)**2 + x[1,i]**2 + x[1,i+1]**2 + u[0,i]**2 + u[0,i+1]**2 ) - - J = J + 0.5 * dt * ( u[0,i]**2 + u[0,i+1]**2 ) - - return J - -def constraints(dec): - - x,u = dec2xu(dec) - - vec=np.zeros(grid*2-2) - - for i in range(grid-1): - - vec[i] = (x[0,i+1] - x[0,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[0] + sys.f(x[:,i+1],u[:,i+1])[0] ) - vec[i+grid-1] = (x[1,i+1] - x[1,i]) - 0.5 * dt * ( sys.f(x[:,i],u[:,i])[1] + sys.f(x[:,i+1],u[:,i+1])[1] ) - return vec - - -def compute_bounds(): - bounds = [] - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-4,1) ) - - bounds.append( (-3.14,-3.13) ) - - bounds.append( (0,0.01) ) - - for i in range(1,grid-1): - - bounds.append( (-6,4) ) - - bounds.append( (0,0.01) ) - - for i in range(grid): - - bounds.append( (-10,10) ) - - return bounds - - - -# Guess -dec = np.zeros(grid*(n+m)) -#dec[0:grid] = traj.x[:,0] -#dec[grid:2*grid] = traj.x[:,1] -#dec[2*grid:3*grid] = traj.u[:,0] - -bnds = compute_bounds() - -cons = {'type': 'eq', 'fun': constraints } -res = minimize( cost, dec, method='SLSQP', bounds=bnds, constraints=cons, options={'disp':True,'maxiter':1000}) # - - -sys.compute_trajectory(grid*dt,grid) - - -dec = res.x - -sys.traj.x[:,0] = dec[0:grid] -sys.traj.x[:,1] = dec[grid:2*grid] -sys.traj.u[:,0] = dec[2*grid:3*grid] - - -sys.plot_trajectory('xu') -sys.animate_simulation() \ No newline at end of file diff --git a/examples/demos_by_tool/trajectory_stabilization/cartpole_swing_up_with_lqr_stabilization.py b/examples/demos_by_tool/trajectory_stabilization/cartpole_swing_up_with_lqr_stabilization.py new file mode 100644 index 00000000..8a1f2e9c --- /dev/null +++ b/examples/demos_by_tool/trajectory_stabilization/cartpole_swing_up_with_lqr_stabilization.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Thu May 4 10:01:30 2023 + +@author: alex +""" + +############################################################################### +import numpy as np +############################################################################### +from pyro.dynamic import cartpole +from pyro.analysis import costfunction +from pyro.planning import trajectoryoptimisation +from pyro.control import lqr +############################################################################### + +############## +# System +############## + +sys = cartpole.CartPole() + +sys.u_ub[0] = +20 +sys.u_lb[0] = -20 + +################ +# Cost function +################ + +cf = costfunction.QuadraticCostFunction.from_sys( sys ) + +cf.Q[0,0] = 1.0 +cf.Q[1,1] = 1.0 +cf.R[0,0] = 1.0 + +########################### +# Trajectory optimization +########################## + +planner = trajectoryoptimisation.DirectCollocationTrajectoryOptimisation( + sys , + dt = 0.1 , + grid = 40 , + cost_function = cf + ) + + +planner.x_start = np.array([0,0,0,0]) +planner.x_goal = np.array([0,np.pi,0,0]) + +planner.init_dynamic_plot() +planner.maxiter = 500 +planner.compute_optimal_trajectory() + +traj = planner.traj + + +########################### +# LQR Controller +########################## + +cf.R[0,0] = 5.0 + +ctl = lqr.TrajectoryLQRController( sys , traj , cf ) + + +########################### +# Simulation +########################## + +cl_sys = ctl + sys + +cl_sys.x0[0] = 0.5 +cl_sys.x0[1] = 0.5 + +cl_sys.compute_trajectory( tf = 10.0 ) +cl_sys.plot_trajectory('xu') +cl_sys.animate_simulation( time_factor_video=1.0 ) + diff --git a/examples/demos_by_tool/trajectory_stabilization/double_pendulum_with_trajectory_following_lqr_controller.py b/examples/demos_by_tool/trajectory_stabilization/double_pendulum_with_trajectory_following_lqr_controller.py new file mode 100644 index 00000000..316386bf --- /dev/null +++ b/examples/demos_by_tool/trajectory_stabilization/double_pendulum_with_trajectory_following_lqr_controller.py @@ -0,0 +1,29 @@ +# -*- coding: utf-8 -*- +""" +Created on Fri Nov 16 12:05:08 2018 + +@author: Alexandre +""" +############################################################################### +import numpy as np +############################################################################### +from pyro.dynamic import pendulum +from pyro.planning import plan +from pyro.analysis import simulation +from pyro.control import lqr +############################################################################### + +sys = pendulum.DoublePendulum() + +traj = simulation.Trajectory.load('double_pendulum_directcollocation_hires.npy') + +ctl = lqr.TrajectoryLQRController( sys , traj ) + +# New cl-dynamic +cl_sys = ctl + sys + +# Simultation +cl_sys.x0 = traj.x[0,:] + np.array([-0.2,0.1,0.1,-0.02]) +cl_sys.compute_trajectory(10) +cl_sys.plot_trajectory('xu') +cl_sys.animate_simulation() \ No newline at end of file diff --git a/examples/demos_by_tool/trajectory_stabilization/pendulum_swing_up_with_lqr_stabilization.py b/examples/demos_by_tool/trajectory_stabilization/pendulum_swing_up_with_lqr_stabilization.py new file mode 100644 index 00000000..1ceb6eaa --- /dev/null +++ b/examples/demos_by_tool/trajectory_stabilization/pendulum_swing_up_with_lqr_stabilization.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Thu May 4 10:01:30 2023 + +@author: alex +""" + +############################################################################### +import numpy as np +############################################################################### +from pyro.dynamic import pendulum +from pyro.analysis import costfunction +from pyro.planning import trajectoryoptimisation +from pyro.control import lqr +from pyro.analysis import simulation +############################################################################### + +sys = pendulum.SinglePendulum() + + +# Cost function +cf = costfunction.QuadraticCostFunction.from_sys( sys ) + +cf.Q[0,0] = 1.0 +cf.Q[1,1] = 1.0 +cf.R[0,0] = 0.1 + + +#Max/Min torque +sys.u_ub[0] = +4 +sys.u_lb[0] = -6 + +planner = trajectoryoptimisation.DirectCollocationTrajectoryOptimisation( sys ) + +planner.x_start = np.array([0,0]) +planner.x_goal = np.array([-3.14,0]) + +planner.init_dynamic_plot() +planner.compute_optimal_trajectory() + +traj = planner.traj + + +cf.R[0,0] = 1.0 + + +ctl = lqr.TrajectoryLQRController( sys , traj , cf ) + + +# Simulation +cl_sys = ctl + sys + +cl_sys.x0[0] = 0.0 + 0.2 +cl_sys.x0[1] = 0.0 + +cl_sys.compute_trajectory( tf = 5.0 ) +cl_sys.plot_trajectory('xu') +cl_sys.animate_simulation( time_factor_video=1.0 ) + diff --git a/examples/projects/vehicle_modeling/advanced_vehicles.py b/examples/projects/vehicle_modeling/advanced_vehicles.py index e45264fc..034162ed 100644 --- a/examples/projects/vehicle_modeling/advanced_vehicles.py +++ b/examples/projects/vehicle_modeling/advanced_vehicles.py @@ -11,7 +11,7 @@ import numpy as np ############################################################################## from pyro.dynamic import system -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering ############################################################################### @@ -20,7 +20,7 @@ ############################################################################### -class LateralDynamicBicycleModelwithSpeedInput( vehicle.KinematicCarModel ): +class LateralDynamicBicycleModelwithSpeedInput( vehicle_steering.KinematicCarModel ): """ Equations of Motion ------------------------- diff --git a/examples/projects/vehicle_modeling/kinematic_VI_bicycle.py b/examples/projects/vehicle_modeling/kinematic_VI_bicycle.py index 2672ec3d..f9081c1a 100644 --- a/examples/projects/vehicle_modeling/kinematic_VI_bicycle.py +++ b/examples/projects/vehicle_modeling/kinematic_VI_bicycle.py @@ -9,7 +9,7 @@ import numpy as np import matplotlib.pyplot as plt ############################################################################### -from pyro.dynamic import vehicle +from pyro.dynamic import vehicle_steering ############################################################################### import advanced_vehicles import test_vehicle_controllers @@ -18,7 +18,7 @@ # "Fake" controller - Varying inputs (delta, T_f, T_r) throughout time (change in linear.py) ctl = test_vehicle_controllers.kinematicInputs() # Vehicule dynamical system -sys = vehicle.KinematicBicyleModel() +sys = vehicle_steering.KinematicBicyleModel() # Set default wheel velocity and steering angle cl_sys = ctl+sys diff --git a/pyro/analysis/costfunction.py b/pyro/analysis/costfunction.py index 1e8691e2..ebef4a0f 100644 --- a/pyro/analysis/costfunction.py +++ b/pyro/analysis/costfunction.py @@ -97,9 +97,6 @@ def trajectory_evaluation(self, traj): ############################################################################### # Basic cost functions ############################################################################### - - -############################################################################# class QuadraticCostFunction( CostFunction ): """ @@ -206,6 +203,84 @@ def g(self, x, u, t): return dJ + +############################################################################# + +class QuadraticCostFunctionVectorized( CostFunction ): + """ + Vectorized: (x, u , t) can be trajectory of time matrices + + Quadratic cost functions of continuous dynamical systems + ---------------------------------------------- + n : number of states + m : number of control inputs + --------------------------------------- + J = int( g(x,u,t) * dt ) + h( x(T) , T ) + + g = xQx + uRu + h = xSx + + """ + + ############################ + def __init__(self, n, m): + + CostFunction.__init__(self) + + # dimensions + self.n = n + self.m = m + + # Quadratic cost weights + self.Q = np.diag( np.ones(n) ) + self.R = np.diag( np.ones(m) ) + self.S = np.diag( np.zeros(n) ) + + self.is_vectorized = True + + + ############################ + @classmethod + def from_sys(cls, sys): + """ From ContinuousDynamicSystem instance """ + + instance = cls( sys.n , sys.m ) + + return instance + + + ############################# + def h(self, x , t = 0): + """ Final cost function with zero value """ + + if x.ndim == 1 : + + J_f = x.T @ self.S @ x + + + else: + + # Quadratic terminal cost + J_f = np.diag( x.T @ self.S @ x ) + + return J_f + + + ############################# + def g(self, x, u, t): + """ Quadratic additive cost """ + + if x.ndim == 1 : + + dJ = x.T @ self.Q @ x + u.T @ self.R @ u + + else: + + dJ = np.diag( x.T @ self.Q @ x ) + np.diag( u.T @ self.R @ u ) + + return dJ + + ############################################################################## diff --git a/pyro/analysis/costfunction_legacy.py b/pyro/analysis/costfunction_legacy.py deleted file mode 100644 index ef418dcf..00000000 --- a/pyro/analysis/costfunction_legacy.py +++ /dev/null @@ -1,261 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Fri Aug 07 11:51:55 2015 - -@author: agirard -""" - -############################################################################### -import numpy as np -from copy import copy -from scipy.integrate import cumtrapz -############################################################################### - - -############################################################################### -# Mother cost function class -############################################################################### - -class CostFunction(): - """ - Mother class for cost functions of continuous dynamical systems - ---------------------------------------------- - n : number of states - m : number of control inputs - p : number of outputs - --------------------------------------- - J = int( g(x,u,y,t) * dt ) + h( x(T) , y(T) , T ) - - """ - - ############################ - def __init__(self): - self.INF = 1E3 - self.EPS = 1E-3 - - ########################################################################### - # The two following functions needs to be implemented by child classes - ########################################################################### - - ############################# - def h(self, x, t = 0): - """ Final cost function """ - - raise NotImplementedError - - ############################# - def g(self, x, u, y, t): - """ step cost function """ - - raise NotImplementedError - - ########################################################################### - # Method using h and g - ########################################################################### - - ############################# - def trajectory_evaluation(self, traj): - """ - - Compute cost and add it to simulation data - - Parameters - ---------- - traj : instance of `pyro.analysis.Trajectory` - - Returns - ------- - new_traj : A new instance of the input trajectory, with updated `J` and - `dJ` fields - - J : array of size ``traj.time_steps`` (number of timesteps in - trajectory) - Cumulative value of cost integral at each time step. The total - cost is - therefore ``J[-1]``. - - dJ : array of size ``traj.time_steps`` (number of timesteps in - trajectory) - Value of cost function evaluated at each point of the tracjectory. - """ - - dJ = np.empty(traj.time_steps) - for i in range(traj.time_steps): - x = traj.x[i, :] - u = traj.u[i, :] - y = traj.y[i, :] - t = traj.t[i] - dJ[i] = self.g( x, u, y, t) - - J = cumtrapz(y=dJ, x=traj.t, initial=0) - - new_traj = copy(traj) - new_traj.J = J - new_traj.dJ = dJ - - return new_traj - - -############################################################################### -# Basic cost functions -############################################################################### - - -############################################################################# - -class QuadraticCostFunction( CostFunction ): - """ - Quadratic cost functions of continuous dynamical systems - ---------------------------------------------- - n : number of states - m : number of control inputs - p : number of outputs - --------------------------------------- - J = int( g(x,u,y,t) * dt ) + h( x(T) , y(T) , T ) - - g = xQx + uRu + yVy - h = 0 - - """ - - ############################ - def __init__(self, n, m, p): - - CostFunction.__init__(self) - - self.n = n - self.m = m - self.p = p - - self.xbar = np.zeros(self.n) - self.ubar = np.zeros(self.m) - self.ybar = np.zeros(self.p) - - # Quadratic cost weights - self.Q = np.diag( np.ones(n) ) - self.R = np.diag( np.ones(m) ) - self.V = np.diag( np.zeros(p) ) - - # Optionnal zone of zero cost if ||dx|| < EPS - self.ontarget_check = True - - ############################ - @classmethod - def from_sys(cls, sys): - """ From ContinuousDynamicSystem instance """ - - instance = cls( sys.n , sys.m , sys.p ) - - instance.xbar = sys.xbar - instance.ubar = sys.ubar - instance.ybar = np.zeros( sys.p ) - - return instance - - - ############################# - def h(self, x , t = 0): - """ Final cost function with zero value """ - - return 0 - - - ############################# - def g(self, x, u, y, t): - """ Quadratic additive cost """ - - # Check dimensions - if not x.shape[0] == self.Q.shape[0]: - raise ValueError( - "Array x of shape %s does not match weights Q with %d components" \ - % (x.shape, self.Q.shape[0]) - ) - if not u.shape[0] == self.R.shape[0]: - raise ValueError( - "Array u of shape %s does not match weights R with %d components" \ - % (u.shape, self.R.shape[0]) - ) - if not y.shape[0] == self.V.shape[0]: - raise ValueError( - "Array y of shape %s does not match weights V with %d components" \ - % (y.shape, self.V.shape[0]) - ) - - # Delta values with respect to bar values - dx = x - self.xbar - du = u - self.ubar - dy = y - self.ybar - - dJ = ( np.dot( dx.T , np.dot( self.Q , dx ) ) + - np.dot( du.T , np.dot( self.R , du ) ) + - np.dot( dy.T , np.dot( self.V , dy ) ) ) - - # Set cost to zero if on target - if self.ontarget_check: - if ( np.linalg.norm( dx ) < self.EPS ): - dJ = 0 - - return dJ - - -############################################################################## - -class TimeCostFunction( CostFunction ): - """ - Mother class for cost functions of continuous dynamical systems - ---------------------------------------------- - n : number of states - m : number of control inputs - p : number of outputs - --------------------------------------- - J = int( g(x,u,y,t) * dt ) + h( x(T) , y(T) , T ) = T - - g = 1 - h = 0 - - """ - - ############################ - def __init__(self, xbar ): - - CostFunction.__init__(self) - - self.xbar = xbar - - self.ontarget_check = True - - ############################# - def h(self, x , t = 0): - """ Final cost function with zero value """ - - return 0 - - - ############################# - def g(self, x , u , y, t = 0 ): - """ Unity """ - - if (x.shape[0] != self.xbar.shape[0]): - raise ValueError("Got x with %d values, but xbar has %d values" % - (x.shape[1], self.xbar.shape[0])) - - dJ = 1 - - if self.ontarget_check: - dx = x - self.xbar - if ( np.linalg.norm( dx ) < self.EPS ): - dJ = 0 - - return dJ - -''' -################################################################# -################## Main ######## -################################################################# -''' - - -if __name__ == "__main__": - """ MAIN TEST """ - - pass \ No newline at end of file diff --git a/pyro/analysis/graphical.py b/pyro/analysis/graphical.py index 475c9ab3..e42ef4d7 100644 --- a/pyro/analysis/graphical.py +++ b/pyro/analysis/graphical.py @@ -30,7 +30,7 @@ # Default figure settings default_figsize = (4, 3) -default_dpi = 300 +default_dpi = 250 default_linestyle = '-' default_fontsize = 5 @@ -57,9 +57,9 @@ def __init__(self, sys): self.sys = sys # Ploting - self.fontsize = 5 - self.figsize = (4, 3) - self.dpi = 300 + self.fontsize = default_fontsize + self.figsize = default_figsize + self.dpi = default_dpi ########################################################################## @@ -114,6 +114,8 @@ def plot(self, traj, plot = 'x' , show = True ): simfig , plots = plt.subplots(l, sharex=True, figsize=self.figsize, dpi=self.dpi, frameon=True) + + lines = [None] * l ####################################################################### #Fix bug for single variable plotting @@ -128,7 +130,7 @@ def plot(self, traj, plot = 'x' , show = True ): if plot=='All' or plot=='x' or plot=='xu' or plot=='xy' or plot=='xuj': # For all states for i in range( sys.n ): - plots[j].plot( traj.t , traj.x[:,i] , 'b') + lines[j] = plots[j].plot( traj.t , traj.x[:,i] , 'b')[0] plots[j].set_ylabel(sys.state_label[i] +'\n'+ sys.state_units[i] , fontsize=self.fontsize ) plots[j].grid(True) @@ -138,7 +140,7 @@ def plot(self, traj, plot = 'x' , show = True ): if plot == 'All' or plot == 'u' or plot == 'xu' or plot == 'xuj': # For all inputs for i in range( sys.m ): - plots[j].plot( traj.t , traj.u[:,i] , 'r') + lines[j] = plots[j].plot( traj.t , traj.u[:,i] , 'r')[0] plots[j].set_ylabel(sys.input_label[i] + '\n' + sys.input_units[i] , fontsize=self.fontsize ) plots[j].grid(True) @@ -148,7 +150,7 @@ def plot(self, traj, plot = 'x' , show = True ): if plot == 'All' or plot == 'y' or plot == 'xy': # For all outputs for i in range( sys.p ): - plots[j].plot( traj.t , traj.y[:,i] , 'k') + lines[j] = plots[j].plot( traj.t , traj.y[:,i] , 'k')[0] plots[j].set_ylabel(sys.output_label[i] + '\n' + sys.output_units[i] , fontsize=self.fontsize ) plots[j].grid(True) @@ -157,12 +159,12 @@ def plot(self, traj, plot = 'x' , show = True ): if plot == 'All' or plot == 'j' or plot == 'xuj': # Cost function - plots[j].plot( traj.t , traj.dJ[:] , 'b') + lines[j] = plots[j].plot( traj.t , traj.dJ[:] , 'b')[0] plots[j].set_ylabel('dJ', fontsize=self.fontsize ) plots[j].grid(True) plots[j].tick_params( labelsize = self.fontsize ) j = j + 1 - plots[j].plot( traj.t , traj.J[:] , 'r') + lines[j] = plots[j].plot( traj.t , traj.J[:] , 'r')[0] plots[j].set_ylabel('J', fontsize=self.fontsize ) plots[j].grid(True) plots[j].tick_params( labelsize = self.fontsize ) @@ -172,7 +174,7 @@ def plot(self, traj, plot = 'x' , show = True ): # Internal states n = sys.n - sys.controller.l for i in range( l ): - plots[j].plot( traj.t , traj.x[:,i+n] , 'b') + lines[j] = plots[j].plot( traj.t , traj.x[:,i+n] , 'b')[0] plots[j].set_ylabel(sys.state_label[i+n] +'\n'+ sys.state_units[i+n] , fontsize=self.fontsize ) plots[j].grid(True) @@ -189,8 +191,78 @@ def plot(self, traj, plot = 'x' , show = True ): self.fig = simfig self.plots = plots + self.lines = lines + self.l = l + + return (simfig, plots, lines) + + ########################################################################## + def update_plot(self, traj, plot = 'x'): + """ + Create a figure with trajectories for states, inputs, outputs and cost + ---------------------------------------------------------------------- + plot = 'All' + plot = 'xu' + plot = 'xy' + plot = 'x' + plot = 'u' + plot = 'y' + plot = 'j' + plot = 'z' + """ + + lines = self.lines + sys = self.sys + plots = self.plots + + j = 0 + + if plot=='All' or plot=='x' or plot=='xu' or plot=='xy' or plot=='xuj': + + # For all states + for i in range( sys.n ): + lines[j].set_data( traj.t , traj.x[:,i] ) + plots[j].relim() + plots[j].autoscale_view(True,True,True) + j = j + 1 + + if plot == 'All' or plot == 'u' or plot == 'xu' or plot == 'xuj': + # For all inputs + for i in range( sys.m ): + lines[j].set_data( traj.t , traj.u[:,i] ) + plots[j].relim() + plots[j].autoscale_view(True,True,True) + j = j + 1 + + if plot == 'All' or plot == 'y' or plot == 'xy': + # For all outputs + for i in range( sys.p ): + lines[j].set_data( traj.t , traj.y[:,i] ) + plots[j].relim() + plots[j].autoscale_view(True,True,True) + j = j + 1 + + if plot == 'All' or plot == 'j' or plot == 'xuj': + # Cost function + lines[j].set_data( traj.t , traj.dJ[:,i] ) + plots[j].relim() + plots[j].autoscale_view(True,True,True) + j = j + 1 + lines[j].set_data( traj.t , traj.J[:,i] ) + plots[j].relim() + plots[j].autoscale_view(True,True,True) + j = j + 1 + + if plot == 'z': + # Internal states + n = sys.n - sys.controller.l + for i in range( self.l ): + lines[j].set_data( traj.t , traj.x[:,i+n] ) + plots[j].relim() + plots[j].autoscale_view(True,True,True) + j = j + 1 - return (simfig, plots) + ########################################################################## def phase_plane_trajectory(self, traj, x_axis=0, y_axis=1): @@ -316,11 +388,11 @@ def __init__(self, sys ): self.x_axis = 0 self.y_axis = 1 - # Params - self.figsize = (4, 3) - self.dpi = 300 + # Ploting Param + self.fontsize = default_fontsize + self.figsize = default_figsize + self.dpi = default_dpi self.linestyle = sys.linestyle - self.fontsize = 5 # Label self.top_right_label = None @@ -839,10 +911,25 @@ def __animate__(self,i): """ MAIN TEST """ from pyro.dynamic import pendulum - from pyro.dynamic import vehicle + from pyro.dynamic import vehicle_steering sys = pendulum.DoublePendulum() sys.x0 = np.array([0.1,0.1,0,0]) + + sys.plot_phase_plane(0,2) + + traj = sys.compute_trajectory( 2.0 ) + + + plotter = TrajectoryPlotter( sys ) + plotter.plot( traj, 'xu' ) + + sys.x0 = np.array([0.2,0.1,0,0]) + traj2 = sys.compute_trajectory( 2.0 ) + + plotter.update_plot( traj2 ) + + plotter.phase_plane_trajectory( traj2 , 0 , 2 ) is_3d = False @@ -851,7 +938,7 @@ def __animate__(self,i): a = Animator(sys) a.animate_simulation( sys.traj, 1, is_3d) - sys = vehicle.KinematicBicyleModel() + sys = vehicle_steering.KinematicBicyleModel() sys.ubar = np.array([1,0.01]) sys.x0 = np.array([0,0,0]) diff --git a/pyro/analysis/phaseanalysis.py b/pyro/analysis/phaseanalysis.py index 558515f5..9ff2d4a0 100644 --- a/pyro/analysis/phaseanalysis.py +++ b/pyro/analysis/phaseanalysis.py @@ -9,6 +9,8 @@ import matplotlib import matplotlib.pyplot as plt +from pyro.analysis import graphical + # Embed font type in PDF matplotlib.rcParams['pdf.fonttype'] = 42 matplotlib.rcParams['ps.fonttype'] = 42 @@ -50,13 +52,13 @@ def __init__(self, ContinuousDynamicSystem , x_axis = 0 , y_axis = 1): # Plotting params self.color = 'b' - self.figsize = (3, 2) - self.dpi = 300 + self.figsize = graphical.default_figsize + self.dpi = graphical.default_dpi self.linewidth = 0.005 self.streamplot = False self.arrowstyle = '->' self.headlength = 4.5 - self.fontsize = 5 + self.fontsize = graphical.default_fontsize ########################################################################### diff --git a/pyro/analysis/simulation.py b/pyro/analysis/simulation.py index 21f3497f..ca29cb2f 100644 --- a/pyro/analysis/simulation.py +++ b/pyro/analysis/simulation.py @@ -72,6 +72,7 @@ def save(self, name = 'trajectory.npy' ): def load(cls, name): try: # try to load as new format (np.savez) + # with np.load(name) as data: with np.load(name) as data: return cls(**data) @@ -85,7 +86,7 @@ def load(cls, name): ############################ def _compute_size(self): - #print(self.t) + # print(self.t) self.time_final = self.t.max() self.time_steps = self.t.size @@ -95,10 +96,13 @@ def _compute_size(self): self.ubar = np.zeros( self.m ) - # Check consistency between signals - for arr in [self.x, self.y, self.u, self.dx, self.r, self.J, self.dJ]: - if (arr is not None) and (arr.shape[0] != self.time_steps): - raise ValueError("Result arrays must have same length along axis 0") + # # Check consistency between signals + # for arr in [self.x, self.y, self.u, self.dx, self.r, self.J, self.dJ]: + + # if arr is not None: + + # if arr.shape[0] != self.time_steps: + # raise ValueError("Result arrays must have same length along axis 0") ############################ diff --git a/pyro/control/controller.py b/pyro/control/controller.py index d4c97e63..79b8c4c8 100644 --- a/pyro/control/controller.py +++ b/pyro/control/controller.py @@ -110,6 +110,23 @@ def t2r( self , t ): return r + ######################################################################### + def forward_kinematic_lines_plus( self, x , u , t ): + """ + Graphical output for the controller + ----------------------------------- + default is nothing + + x,u,t are the state, input and time of the global closed-loop system + + """ + + pts = None + style = None + color = None + + return pts, style, color + ######################################################################### # No need to overwrite the following functions for child classes @@ -382,6 +399,83 @@ def t2u( self , t ): return u + + ########################################################################### + # Place holder graphical output, overload with specific graph output + ########################################################################### + + ############################# + def xut2q( self, x , u , t ): + """ Compute configuration variables ( q vector ) """ + + # Use the plant function + q = self.plant.xut2q( x, u, t) + + return q + + + ########################################################################### + def forward_kinematic_domain(self, q ): + """ Set the domain range for ploting, can be static or dynamic """ + + # Use the plant function + domain = self.plant.forward_kinematic_domain( q ) + + return domain + + + ########################################################################### + def forward_kinematic_lines(self, q ): + """ + Compute points p = [x;y;z] positions given config q + ---------------------------------------------------- + - points of interest for ploting + + Outpus: + lines_pts = [] : a list of array (n_pts x 3) for each lines + + """ + + lines_pts = self.plant.forward_kinematic_lines( q ) + + return lines_pts + + + ########################################################################### + def forward_kinematic_lines_plus(self, x , u , t ): + """ + Return combined graphical output for the controller and the system + """ + + # TODO: this is a quick fix, need to be improved + + sys = self.plant.forward_kinematic_lines_plus( x , u , t ) + ctl = self.controller.forward_kinematic_lines_plus( x , u , t ) + + if type(sys) is tuple: + lines_pts = sys[0] + lines_style = sys[1] + lines_color = sys[2] + else: + # Legacy graph function to remove eventually + lines_pts = sys + lines_style = [] + lines_color = [] + for j, line in enumerate(lines_pts): + lines_style.append( self.plant.linestyle ) # default value + lines_color.append( self.plant.linescolor ) # default value + + if ctl[0] is not None: + lines_pts = ctl[0] + lines_pts + lines_style = ctl[1] + lines_style + lines_color = ctl[2] + lines_color + + return lines_pts, lines_style, lines_color + + ############################################################################# + #### Updated shortcuts + ############################################################################# + ########################################################################### def plot_phase_plane_closed_loop(self , x_axis = 0 , y_axis = 1 ): @@ -447,7 +541,8 @@ def get_plotter(self): ########################################################################### def get_animator(self): - return self.plant.get_animator() + + return graphical.Animator(self) ########################################################################### @@ -680,6 +775,24 @@ def cbar( self , y , t = 0 ): u = self.c( self.zbar, y , self.rbar , t ) return u + + + ######################################################################### + def forward_kinematic_lines_plus( self, x, u , t ): + """ + Graphical output for the controller + ----------------------------------- + default is nothing + + x,u,t are the state, input and time of the global closed-loop system + + """ + + pts = None + style = None + color = None + + return pts, style, color ############################# diff --git a/pyro/control/lqr.py b/pyro/control/lqr.py index c9b3733b..91a7cb57 100755 --- a/pyro/control/lqr.py +++ b/pyro/control/lqr.py @@ -12,6 +12,7 @@ ############################################################################## from pyro.control import linear +from pyro.control import controller from pyro.dynamic import statespace from pyro.analysis import costfunction ############################################################################## @@ -102,6 +103,214 @@ def linearize_and_synthesize_lqr_controller( sys , cf ): ctl = synthesize_lqr_controller( ss , cf , sys.xbar , sys.ubar ) return ctl + + + +############################################################################## +### Time varying LQR controller +############################################################################## + +class TrajectoryLQRController( controller.StaticController ): + """ + General (SISO or MIMO) proportional controller + ------------------------------------------------- + + u = u_d(t) + K(t) ( xd(t) - x ) + + + K = R^(-1) B(t) S(t) + + S(t) = integral of differential riccati equation + + A(t) = df/dx(t) + B(t) = df/du(t) + + x = y : state feedback is assumed + + ----------------------------------------- + r : reference signal vector k x 1 + y : sensor signal vector p x 1 + u : control inputs vector m x 1 + ----------------------------------------- + + """ + + ############################### + def __init__(self, sys , traj , cf = None ): + + n = sys.n # states dimensions + m = sys.m # input dimenstions + + controller.StaticController.__init__( self, n, m, n) + + self.name = "Trajectory LQR Controller" + + self.sys = sys + self.traj = traj + + if cf is None: + + self.cf = sys.cost_function + + else: + + self.cf = cf + + self.compute_linear_dynamic() + self.compute_cost_matrices() + self.compute_cost2go() + self.compute_gains() + + self.traj.generate_interpol_functions() + + print('\n\n-------------------------------------------------------------------------') + print('Warning: this method is an approximation of the riccati solution for now') + print('ToDo @Alex : fix differential riccati integration') + print('-------------------------------------------------------------------------\n') + + + ############################### + def compute_linear_dynamic(self): + + steps = self.traj.time_steps + + self.As = np.zeros(( self.sys.n , self.sys.n , steps )) + self.Bs = np.zeros(( self.sys.n , self.sys.m , steps )) + + for i in range(steps): + + self.sys.tbar = self.traj.t[i] + self.sys.xbar = self.traj.x[i,:] + self.sys.ubar = self.traj.u[i,:] + + # TODO more intelligent gradient computation + ss = statespace.linearize( self.sys , 0.01 ) + + self.As[:,:,i] = ss.A + self.Bs[:,:,i] = ss.B + + + + ############################### + def compute_cost_matrices(self): + + steps = self.traj.time_steps + + self.Qs = np.zeros(( self.sys.n , self.sys.n , steps )) + self.Rs = np.zeros(( self.sys.m , self.sys.m , steps )) + + for i in range(steps): + + # ToDo Add Time Varying Quadratic cost object + self.Qs[:,:,i] = self.cf.Q + self.Rs[:,:,i] = self.cf.R + + + + ############################### + def compute_cost2go(self): + + steps = self.traj.time_steps + + self.Ss = np.zeros(( self.sys.n , self.sys.n , steps )) + self.Ps = np.zeros(( self.sys.n , self.sys.n , steps )) + + # Boundary conditions + Af = self.As[:,:,-1] + Bf = self.Bs[:,:,-1] + Qf = self.Qs[:,:,-1] + Rf = self.Rs[:,:,-1] + + self.Ss[:,:,-1] = solve_continuous_are( Af , Bf , Qf , Rf ) + + # self.Ss[:,:,-1] = np.eye( self.sys.n ) + # self.Ps[:,:,-1] = np.eye( self.sys.n ) + + for i in range( steps -1 , 0, -1 ): + + A = self.As[:,:,i] + B = self.Bs[:,:,i] + Q = self.Qs[:,:,i] + R = self.Rs[:,:,i] + S = self.Ss[:,:,i] + # P = self.Ps[:,:,i] + + # Quick approx for testing + dS = Q + + ############## + ## TODO: Make a real integraiton of differential riccati equation + ############# + + # dS = S @ A + A.T @ S - S @ B @ np.linalg.inv(R) @ B.T @ S + Q + # dP = A.T @ P - 0.5 * S @ B @ np.linalg.inv(R) @ B.T @ P + 0.5 * Q @ np.linalg.inv(P).T + + dt = self.traj.t[i] - self.traj.t[i-1] + + # Euler integration (maybe is not engouh precision) + self.Ss[:,:,i-1] = self.Ss[:,:,i] + dS * dt + + # self.Ps[:,:,i-1] = self.Ps[:,:,i] + dP * dt + # self.Ss[:,:,i-1] = self.Ps[:,:,i-1] @ self.Ps[:,:,i-1].T + + # print('t = ', self.traj.t[i]) + # print('\ndS = \n' , dS) + # print('\nS = \n' , S) + + + ############################### + def compute_gains(self): + + steps = self.traj.time_steps + + self.Ks = np.zeros(( self.sys.m , self.sys.n , steps )) + + for i in range( steps ): + + A = self.As[:,:,i] + B = self.Bs[:,:,i] + Q = self.Qs[:,:,i] + R = self.Rs[:,:,i] + S = self.Ss[:,:,i] + + K = np.linalg.inv(R) @ B.T @ S + + print('\n-------------------\nt = ', self.traj.t[i]) + print('K = \n' , K) + + self.Ks[:,:,i] = K + + + ############################## + def c(self, x, r, t): + """ Feedback law """ + + if t < self.traj.time_final: + + # Nominal control input + u_bar = self.traj.inter_t2u( t ) + x_bar = self.traj.inter_t2x( t ) + + # Find closest traj point + i = (np.abs(self.traj.t - t)).argmin() + + K = self.Ks[:,:,i] # Time varying linear gain + + u_e = K @ ( x_bar - x ) + + u = u_bar + u_e + + else: + + x_bar = self.traj.x[-1,:] + + K = self.Ks[:,:,-1] + + u = K @ ( x_bar - x ) # this assume final position is a fixed point + + return u + + diff --git a/pyro/control/reinforcementlearning.py b/pyro/control/reinforcementlearning.py new file mode 100644 index 00000000..276fa6f2 --- /dev/null +++ b/pyro/control/reinforcementlearning.py @@ -0,0 +1,47 @@ +# -*- coding: utf-8 -*- + +############################################################################### +import numpy as np +############################################################################### +from pyro.control.controller import StaticController +############################################################################### + +class stable_baseline3_controller( StaticController ) : + """ + Wrap a stable baseline 3 model to use it as a pyro controller + """ + + def __init__(self, model): + + self.model = model + + # Dimensions + self.k = model.observation_space.shape[0] + self.m = model.action_space.shape[0] + self.p = model.observation_space.shape[0] + + StaticController.__init__( self, self.k, self.m, self.p) + + self.name = "Stable Baseline 3 Controller" + + + ############################# + def c( self , y , r , t = 0 ): + """ + Feedback static computation u = c( y, r, t) + + INPUTS + y : sensor signal vector p x 1 + r : reference signal vector k x 1 + t : time 1 x 1 + + OUTPUTS + u : control inputs vector m x 1 + + """ + + u, _x = self.model.predict( y , deterministic = True ) + + return u + + \ No newline at end of file diff --git a/pyro/dynamic/boat.py b/pyro/dynamic/boat.py new file mode 100644 index 00000000..6110999a --- /dev/null +++ b/pyro/dynamic/boat.py @@ -0,0 +1,364 @@ +# -*- coding: utf-8 -*- + +############################################################################### +import numpy as np +import matplotlib.pyplot as plt +############################################################################### +from pyro.dynamic import rigidbody +from pyro.kinematic import geometry +from pyro.kinematic import drawing +from pyro.analysis import graphical +############################################################################### + +############################################################################### + +class Boat2D( rigidbody.RigidBody2D ): + """ + Simple planar (3 DoF) boat model + + Partialy based on + 'Low-Speed Maneuvering Models for Dynamic Positioning (3 DOFs)' + see Section 6.7 in + Fossen, Thor I. Handbook of marine craft hydrodynamics and motion control + 2nd Editions. John Wiley & Sons, 2021. + + with Equation of motion (in body frame) described by: + ------------------------------------------------------- + M(q) dv + C(q,v) v + d(q,v) = B(q) u + dq = N(q) v + ------------------------------------------------------- + v : dim = (3, 1) : velocity variables ( foward speed, sway speed, yaw rate) in body frame + q : dim = (3, 1) : position variables ( x , y , theta ) in global frame + u : dim = (2, 1) : thruster force vector in body frame + d(q,v) : dim = (3, 1) : state-dependent dissipative forces in body frame + M(q) : dim = (3, 3) : inertia matrix in body frame + C(q,v) : dim = (3, 3) : corriolis matrix in body frame + B(q) : dim = (3, 2) : actuator matrix in body frame + N(q) : dim = (3, 3) : transformation matrix from body frame to global frame + + with the following hypothesis: + - The boat is a planar rigid body with 3 DoF ( surge, sway and yaw) + - The input is a 2D force vector [ F_x , F_y ] applied at a distance l_t behind the CG + - The boat is subject to linear and quadratic damping + - The default quadratic hydronamic forces coef. are taken from + Fossen, Thor I. Handbook of marine craft hydrodynamics and motion control + 2nd Editions. John Wiley & Sons, 2021. + See 6.7.1 and Fig. 6.11 + A rough fit on experimental data from a tanker + - TODO: not fully validated + - The c.g., c.p and body-frame origin are coincident + + """ + + ############################ + def __init__(self): + """ """ + + rigidbody.RigidBody2D.__init__( self , force_inputs = 2, other_inputs = 0) + + self.input_label = ['Tx','Ty'] + self.input_units = ['[N]','[N]'] + + # Dynamic properties + self.mass = 10000.0 + self.inertia = 10000.0 * 1.0 ** 2 + self.l_t = 3.0 # Distance between CG and Thrust vector + + # Hydrodynamic coefficients + + # linear damping + self.damping_coef = np.array([ 2000.0, 20000.0, 10000.0 ]) + + # quadratic damping + self.Cx_max = 0.5 + self.Cy_max = 0.6 + self.Cm_max = 0.1 + + self.rho = 1000.0 # water density + self.Alc = self.l_t * 2 # lateral area + self.Afc = 0.25 * self.Alc # frontal area + self.loa = self.l_t * 2 # length over all + + # current velocity in world frame + # TODO: not implemented + # self.v_current = np.array([0,0,0]) + + # Graphic output parameters + self.width = self.Afc + self.height = self.l_t * 2 + self.dynamic_domain = True + self.dynamic_range = 10 + + l = self.height * 0.5 + w = self.width * 0.5 + pts = np.zeros(( 6 , 3 )) + pts[0,:] = np.array([-l, +w,0]) + pts[1,:] = np.array([-l, -w,0]) + pts[2,:] = np.array([+l, -w,0]) + pts[3,:] = np.array([l+w,0,0]) + pts[4,:] = np.array([+l, +w,0]) + pts[5,:] = np.array([-l, +w,0]) + + self.drawing_body_pts = pts + + self.show_hydrodynamic_forces = False + + # State working range + xy_range = l * 3 + self.x_ub = np.array([+xy_range,+xy_range,+np.pi,10,10,10]) + self.x_lb = np.array([-xy_range,-xy_range,-np.pi,-10,-10,-10]) + + # Input working range + self.u_ub = np.array([+10000,+1000]) + self.u_lb = np.array([-10000,-1000]) + + ########################################################################### + def B(self, q , u ): + """ + Actuator Matrix + ------------------ + Here u is a 2D point force [ F_x , F_y ] + applied at a point located at a distance l_t behind the CG + hence also creating a yaw moment + """ + + B = np.zeros((3,2)) + + B[0,0] = 1 + B[1,1] = 1 + B[2,1] = - self.l_t + + return B + + ########################################################################### + def CurrentCoef(self, alpha ): + + # Cl = np.sin( 2 * alpha ) # flat plate approx + # Cd = 1 - np.cos( 2 * alpha ) # flat plate approx + # Cm = 0.0 + + # Fig. 7.6 from Fossen (1st edition) + # Fig. 6.11 from Fossen (2nd edition) + Cx = - self.Cx_max * np.cos( alpha ) * np.abs( np.cos( alpha ) ) + Cy = + self.Cy_max * np.sin( alpha ) * np.abs( np.sin( alpha ) ) + Cm = + self.Cm_max * np.sin( 2 * alpha ) + + return np.array([ Cx , Cy , Cm ]) + + ########################################################################### + def d(self, q , v , u ): + """ + Hydrodynamic dissipative forces + ----------------------------------- + + The model is a combination of linear and quadratic damping + + """ + + # linear damping + d_lin = v * self.damping_coef + + V2 = v[0]**2 + v[1]**2 + alpha = -np.arctan2( v[1] , v[0] ) + + Cx = self.CurrentCoef( alpha )[0] + Cy = self.CurrentCoef( alpha )[1] + Cm = self.CurrentCoef( alpha )[2] + + # quadratic forces + fx = -0.5 * self.rho * self.Afc * Cx * V2 + fy = -0.5 * self.rho * self.Alc * Cy * V2 + mz = -0.5 * self.rho * self.Alc * self.loa * Cm * V2 + + d_quad = np.array([ fx , fy , mz ]) + + return d_quad + d_lin + + + ########################################################################### + def forward_kinematic_domain(self, q ): + + l = self.height * 3 + + x = q[0] + y = q[1] + z = 0 + + if self.dynamic_domain: + + domain = [ ( -l + x , l + x ) , + ( -l + y , l + y ) , + ( -l + z , l + z ) ]# + else: + + domain = [ ( -l , l ) , + ( -l , l ) , + ( -l , l ) ]# + + return domain + + + ########################################################################### + def forward_kinematic_lines(self, q ): + + lines_pts = [] # list of array (n_pts x 3) for each lines + lines_style = [] + lines_color = [] + + ########################### + # body + ########################### + + x = q[0] + y = q[1] + theta = q[2] + + W_T_B = geometry.transformation_matrix_2D( theta , x , y ) + + pts_B = self.drawing_body_pts + pts_W = drawing.transform_points_2D( W_T_B , pts_B ) + + lines_pts.append( pts_W ) + lines_style.append( '-') + lines_color.append( 'b' ) + + ########################### + # C.G. + ########################### + + pts = np.zeros(( 1 , 3 )) + pts[0,:] = np.array([x,y,0]) + + lines_pts.append( pts ) + lines_style.append( 'o') + lines_color.append( 'b' ) + + return lines_pts , lines_style , lines_color + + + ########################################################################### + def forward_kinematic_lines_plus(self, x , u , t ): + """ + Graphical output showing trust vectors and hydrodynamic forces + """ + + lines_pts = [] # list of array (n_pts x 3) for each lines + lines_style = [] + lines_color = [] + + # M per Newton of force + f2r = 1.0 / self.u_ub[0] * self.height * 0.5 + + ########################### + # trust force vector + ########################### + + vx = u[0] * f2r + vy = u[1] * f2r + offset = -self.l_t + + pts_body = drawing.arrow_from_components( vx , vy , x = offset, origin = 'tip' ) + W_T_B = geometry.transformation_matrix_2D( x[2], x[0] , x[1] ) + pts_W = drawing.transform_points_2D( W_T_B , pts_body ) + + lines_pts.append( pts_W ) + lines_style.append( '-') + lines_color.append( 'r' ) + + + ########################### + # hydro forces + ########################### + + if self.show_hydrodynamic_forces: + + q , v = self.x2q( x ) + + f = -self.d( q , v , u) + + pts_body = drawing.arrow_from_components( f[0] * f2r , f[1] * f2r ) + + pts_W = drawing.transform_points_2D( W_T_B , pts_body ) + + lines_pts.append( pts_W ) + lines_style.append( '--') + lines_color.append( 'k' ) + + + + return lines_pts , lines_style , lines_color + + + ########################################################################### + def plot_alpha2Coefs(self, alpha_min = -3.15, alpha_max = 3.15 ): + + alphas = np.arange( alpha_min, alpha_max, 0.05 ) + + n = alphas.shape[0] + Cxs = np.zeros((n,1)) + Cys = np.zeros((n,1)) + Cms = np.zeros((n,1)) + + for i in range(n): + Cxs[i] = self.CurrentCoef( alphas[i] )[0] + Cys[i] = self.CurrentCoef( alphas[i] )[1] + Cms[i] = self.CurrentCoef( alphas[i] )[2] + + fig , ax = plt.subplots(3, figsize=graphical.default_figsize, + dpi= graphical.default_dpi, frameon=True) + + fig.canvas.manager.set_window_title('Aero curve') + + ax[0].plot( alphas , Cxs , 'b') + ax[0].set_ylabel('Cx', fontsize=graphical.default_fontsize) + ax[0].set_xlabel('alpha', fontsize=graphical.default_fontsize ) + ax[0].tick_params( labelsize = graphical.default_fontsize ) + ax[0].grid(True) + + ax[1].plot( alphas , Cys , 'b') + ax[1].set_ylabel('Cy', fontsize=graphical.default_fontsize) + ax[1].set_xlabel('alpha', fontsize=graphical.default_fontsize ) + ax[1].tick_params( labelsize = graphical.default_fontsize ) + ax[1].grid(True) + + ax[2].plot( alphas , Cms , 'b') + ax[2].set_ylabel('Cm', fontsize=graphical.default_fontsize) + ax[2].set_xlabel('alpha', fontsize=graphical.default_fontsize ) + ax[2].tick_params( labelsize = graphical.default_fontsize ) + ax[2].grid(True) + + fig.tight_layout() + fig.canvas.draw() + + plt.show() + + +''' +################################################################# +################## Main ######## +################################################################# +''' + + +if __name__ == "__main__": + """ MAIN TEST """ + + sys = Boat2D() + + sys.x0[0] = 0 + sys.x0[1] = 0 + sys.x0[2] = 0 + + sys.x0[3] = 2.0 + sys.x0[4] = 0.0 + sys.x0[5] = 0.0 + + sys.ubar[0] = 10000 + sys.ubar[1] = 1000 + + sys.plot_alpha2Coefs() + + #sys.show_hydrodynamic_forces = True + + sys.plot_trajectory('xu') + sys.animate_simulation() \ No newline at end of file diff --git a/pyro/dynamic/drone.py b/pyro/dynamic/drone.py index 059d6dff..80859462 100644 --- a/pyro/dynamic/drone.py +++ b/pyro/dynamic/drone.py @@ -327,19 +327,19 @@ def __init__(self): self.dynamic_range = 10 # Point Obstacles - self.obstacles = [ - np.array([ 0. , 20. ]) , - np.array([ 0. , 25. ]) , - np.array([ 0. , 30. ]) , - np.array([ 5. , 20. ]) , - np.array([ 5. , 25. ]) , - np.array([ 5. , 30. ]) , - np.array([ 10. , 20. ]) , - np.array([ 10. , 25. ]) , - np.array([ 10. , 30. ]) , - np.array([ 2.5 , 35 ]) , - np.array([ 7.5 , 35 ]) , - ] + self.obstacles = np.array([[ 0. , 20. ] , + [ 0. , 25. ] , + [ 0. , 30. ] , + [ 5. , 20. ] , + [ 5. , 25. ] , + [ 5. , 30. ] , + [ 10. , 20. ] , + [ 10. , 25. ] , + [ 10. , 30. ] , + [ 2.5 , 35 ] , + [ 7.5 , 35 ] ] ) + + self.is_vectorized = True ########################################################################### def f(self, x , u , t = 0 ): @@ -794,8 +794,9 @@ def forward_kinematic_lines_plus(self, x , u , t ): if __name__ == "__main__": """ MAIN TEST """ + #sys = Drone2D() - if False: + if True: sys = Drone2D() @@ -807,7 +808,7 @@ def forward_kinematic_lines_plus(self, x , u , t ): sys.plot_trajectory() sys.animate_simulation() - if True: + if False: sys = SpeedControlledDrone2D() diff --git a/pyro/dynamic/manipulator.py b/pyro/dynamic/manipulator.py index 6bf9edf8..7ab34738 100644 --- a/pyro/dynamic/manipulator.py +++ b/pyro/dynamic/manipulator.py @@ -1484,6 +1484,11 @@ def forward_kinematic_lines(self, q ): lines_color.append('b') return lines_pts , lines_style , lines_color + + ########################################################################### + def forward_kinematic_lines_plus(self, x , u , t ): + + return [],[],[] @@ -1513,6 +1518,8 @@ def __init__(self): # params self.setparams() + + self.lines_plus = False ############################# @@ -1669,7 +1676,11 @@ def forward_kinematic_lines(self, q ): lines_color.append('b') return lines_pts , lines_style , lines_color - + + ########################################################################### + def forward_kinematic_lines_plus(self, x , u , t ): + + return [],[],[] ############################################################################## @@ -1781,8 +1792,6 @@ def forward_kinematic_lines(self, q ): return lines_pts , lines_style , lines_color - ########################################################################### - ############################################################################### # Five Planar Link Manipulator with obstacles @@ -1877,8 +1886,6 @@ def forward_kinematic_lines(self, q ): lines_color.append('k') return lines_pts , lines_style , lines_color - - ########################################################################### @@ -1898,9 +1905,10 @@ def forward_kinematic_lines(self, q ): sys.x0[0] = 0.1 sys.ubar[0] = 4 sys.ubar[1] = 4 - #sys.animate_simulation() - #sys.plot_trajectory() - #sys.plot_end_effector_trajectory() + + sys.plot_trajectory() + sys.animate_simulation() + sys.plot_end_effector_trajectory() #Ellispoid validation diff --git a/pyro/dynamic/mechanical.py b/pyro/dynamic/mechanical.py index 10fd8896..687b3b7c 100644 --- a/pyro/dynamic/mechanical.py +++ b/pyro/dynamic/mechanical.py @@ -422,7 +422,7 @@ def ddq(self, q , dq , u , t = 0 ): H = self.H( q ) C = self.C( q , dq ) g = self.g( q ) - d = self.d( q , dq) + d = self.d( q , dq, u ) B = self.B( q , u ) e = self.u2e( u ) diff --git a/pyro/dynamic/pendulum.py b/pyro/dynamic/pendulum.py index e1fbaa62..a5fad64f 100644 --- a/pyro/dynamic/pendulum.py +++ b/pyro/dynamic/pendulum.py @@ -1163,9 +1163,6 @@ def forward_kinematic_lines_plus(self, x , u , t ): if __name__ == "__main__": """ MAIN TEST """ - sys = DoublePendulum() - - #sys = SinglePendulum() if False: diff --git a/pyro/dynamic/plane.py b/pyro/dynamic/plane.py index 43a31d07..83559756 100644 --- a/pyro/dynamic/plane.py +++ b/pyro/dynamic/plane.py @@ -10,18 +10,19 @@ import numpy as np import matplotlib.pyplot as plt ############################################################################### -from pyro.dynamic import system -from pyro.dynamic import mechanical +from pyro.analysis import graphical +from pyro.dynamic import mechanical +from pyro.kinematic import geometry +from pyro.kinematic import drawing ############################################################################### - ############################################################################## -# 2D planar drone +# 2D planar plane ############################################################################## -class Plane2D( mechanical.MechanicalSystem ): +class Plane2D( mechanical.MechanicalSystemWithPositionInputs ): """ Equations of Motion @@ -48,20 +49,171 @@ def __init__(self): self.output_units = self.state_units # State working range - self.x_ub = np.array([+50,+100,+2,10,10,10]) - self.x_lb = np.array([-50,-0,-2,-10,-10,-10]) + self.x_ub = np.array([+100,+200,+2,30,30,10]) + self.x_lb = np.array([-100,-0,-2,-30,-30,-10]) + + self.u_ub = np.array([+10,+0.3]) + self.u_lb = np.array([ 0, -0.3]) # Model param - self.mass = 1000 - self.inertia = 100 + self.mass = 2.0 # kg + self.inertia = 0.1 # kgm2 self.gravity = 9.8 - # Kinematic param + # Aero param + self.rho = 1.29 # air density + + self.S_w = 0.2 # wing ref. area + self.S_t = 0.05 # tail ref. area + self.l_w = 0.0 # wing a.c. position with respect to c.g., negative is behind c.g. + self.l_t = 1.0 # tail a.c. position with respect to c.g., negative is behind c.g. + + # we assume same wing profile and geometry for wing and tail + self.Cd0 = 0.02 # parasite drag + self.AR = 5.0 # aspect ratio + self.e_factor = 0.8 # oswald efficiency factor + self.Cm0 = 0.0 # Aero moment coef. ac + self.alpha_stall = np.pi / 12. # Graphic output parameters + self.length = 2.0 + self.l_cg = self.length * 0.6 # distance from back of airplane to cg + self.width = self.length / 10.0 self.dynamic_domain = True - self.dynamic_range = 10 + self.dynamic_range = self.length * 1.0 + self.static_range = self.length * 30 + + + ########################################################################### + def compute_velocity_vector(self, q , dq ): + + theta = q[2] + vx = dq[0] + vy = dq[1] + + V = np.sqrt( vx**2 + vy**2 ) # absolute velocity + gamma = np.arctan2( vy , vx ) # velocity vector angle + + alpha = theta - gamma # angle of attack + + return ( V , gamma , alpha ) + + + ########################################################################### + def Cl(self, alpha ): + + # Rough fit on + # https://www.aerospaceweb.org/question/airfoils/q0150b.shtml + + Cl = np.sin( 2 * alpha ) # falt plate approx + + #If not stalled + if (alpha < self.alpha_stall ) and (alpha > -self.alpha_stall ): + + Cl = Cl + 4 * alpha + + return Cl + + + ########################################################################### + def Cd(self, alpha ): + + Cl = self.Cl( alpha ) + + # Body parasite drag + Cd = self.Cd0 + + # Wing flat plate approx + Cd = Cd + ( 1 - np.cos( 2 * alpha )) + + #If not stalled: add induced drag + if (alpha < self.alpha_stall ) and (alpha > -self.alpha_stall ): + + Cd = Cd + Cl **2 / ( np.pi * self.e_factor * self.AR ) + + + return Cd + + + ########################################################################### + def Cm(self, alpha ): + + Cm = self.Cm0 + + return Cm + + + ############################# + def plot_alpha2Cl(self, alpha_min = -3.15, alpha_max = 3.15 ): + + alphas = np.arange( alpha_min, alpha_max, 0.05 ) + + n = alphas.shape[0] + Cls = np.zeros((n,1)) + Cds = np.zeros((n,1)) + Cms = np.zeros((n,1)) + + for i in range(n): + Cls[i] = self.Cl( alphas[i] ) + Cds[i] = self.Cd( alphas[i] ) + Cms[i] = self.Cm( alphas[i] ) + + fig , ax = plt.subplots(3, figsize=graphical.default_figsize, + dpi= graphical.default_dpi, frameon=True) + + fig.canvas.manager.set_window_title('Aero curve') + + ax[0].plot( alphas , Cls , 'b') + ax[0].set_ylabel('Cl', fontsize=graphical.default_fontsize) + ax[0].set_xlabel('alpha', fontsize=graphical.default_fontsize ) + ax[0].tick_params( labelsize = graphical.default_fontsize ) + ax[0].grid(True) + + ax[1].plot( alphas , Cds , 'b') + ax[1].set_ylabel('Cd', fontsize=graphical.default_fontsize) + ax[1].set_xlabel('alpha', fontsize=graphical.default_fontsize ) + ax[1].tick_params( labelsize = graphical.default_fontsize ) + ax[1].grid(True) + + ax[2].plot( alphas , Cms , 'b') + ax[2].set_ylabel('Cm', fontsize=graphical.default_fontsize) + ax[2].set_xlabel('alpha', fontsize=graphical.default_fontsize ) + ax[2].tick_params( labelsize = graphical.default_fontsize ) + ax[2].grid(True) + + fig.tight_layout() + fig.canvas.draw() + + plt.show() + + + + ########################################################################### + def compute_aerodynamic_forces( self, V , alpha , delta ): + + rv2 = 0.5 * self.rho * V**2 + + c_w = np.sqrt( self.S_w / self.AR ) + c_t = np.sqrt( self.S_t / self.AR ) + + Cl_w = self.Cl( alpha ) + Cd_w = self.Cd( alpha ) + Cm_w = self.Cm( alpha ) + + L_w = rv2 * self.S_w * Cl_w + D_w = rv2 * self.S_w * Cd_w + M_w = rv2 * self.S_w * c_w * Cm_w + + Cl_t = self.Cl( alpha + delta ) + Cd_t = self.Cd( alpha + delta ) + Cm_t = self.Cm( alpha + delta ) + + L_t = rv2 * self.S_t * Cl_t + D_t = rv2 * self.S_t * Cd_t + M_t = rv2 * self.S_t * c_t * Cm_t + + return ( L_w , D_w , M_w , L_t , D_t , M_t ) @@ -114,4 +266,346 @@ def g(self, q ): return g - \ No newline at end of file + + ########################################################################### + def d(self, q , dq , u ): + """ + State-dependent dissipative forces : dof x 1 + """ + + V , gamma , alpha = self.compute_velocity_vector( q , dq ) + + delta = u[1] + + L_w, D_w, M_w, L_t, D_t, M_t = self.compute_aerodynamic_forces( V , alpha, delta ) + + ########################################################## + # Total aero forces vector at c.g. in wind-aligned basis + ########################################################## + + s = np.sin( alpha ) + c = np.cos( alpha ) + + L = L_w + L_t + D = D_w + D_t + M = M_w + M_t - self.l_w * ( L_w * c + D_w * s ) - self.l_t * ( L_t * c + D_t * s ) + + ########################################################## + # Transformation of aero forces in global inertial basis + ########################################################## + + d_wind = np.array([ -D , L , M ]) + + s = np.sin( gamma ) + c = np.cos( gamma ) + + R = np.array([ [ c , -s , 0 ] , + [ s , c , 0 ] , + [ 0 , 0 , 1 ] ]) + + d = - R @ d_wind # aero forces in global inertial basis + + return d + + ########################################################################### + def B(self, q , u ): + """ + Actuator Matrix : dof x m + """ + + B = np.zeros((3,1)) + + theta = q[2] + + # TODO PLACE HOLDER + B[0,0] = np.cos( theta ) + B[1,0] = np.sin( theta ) + + return B + + + ########################################################################### + def forward_kinematic_domain(self, q ): + """ + """ + + x = q[0] + y = q[1] + z = 0 + + if self.dynamic_domain: + + l = self.dynamic_range + + domain = [ ( -l + x , l + x ) , + ( -l + y , l + y ) , + ( -l + z , l + z ) ] + else: + + l = self.static_range + + domain = [ ( -l * 0.01 , l ) , + ( -l * 0.01 , l ) , + ( -l * 0.01 , l ) ]# + + + return domain + + + ########################################################################### + def forward_kinematic_lines(self, q ): + """ + Compute points p = [x;y;z] positions given config q + ---------------------------------------------------- + - points of interest for ploting + + Outpus: + lines_pts = [] : a list of array (n_pts x 3) for each lines + + """ + + lines_pts = [] # list of array (n_pts x 3) for each lines + lines_style = [] + lines_color = [] + + + ########################### + # Dimensions + ########################### + + w = self.width # body width + l = self.length # body lenght + + ########################### + # Body + ########################### + + pts = np.zeros(( 5 , 3 )) + + x = q[0] + y = q[1] + theta = q[2] + + world_T_body = geometry.transformation_matrix_2D( theta , x , y ) + #body_T_wind = transformation_matrix_2D_from_base_angle( -alpha , 0 , 0 ) + body_T_drawing = geometry.transformation_matrix_2D( 0 , -self.l_cg , -w/2 ) + + body_pts_local = np.array([ [ 0 , 0 , 1 ] , + [ l , 0 , 1 ] , + [ l-w , w , 1 ] , + [ 2*w , w , 1 ] , + [ w , 3*w , 1 ] , + [ 0 , 3*w , 1 ] , + [ 0 , 0 , 1 ] ]) + + + body_pts_global = drawing.transform_points_2D( world_T_body @ body_T_drawing , body_pts_local ) + + lines_pts.append( body_pts_global ) + lines_style.append( '-') + lines_color.append( 'b') + + if self.dynamic_domain : + + cg = np.array([ [ x , y , 1 ] ]) + + lines_pts.append( cg ) + lines_style.append( 'o') + lines_color.append( 'k') + + ########################### + # Wings + ########################### + + pts = np.zeros(( 2 , 3 )) + + c_w = np.sqrt( self.S_w / self.AR ) + + + wings_pts_body = np.array([ [ -self.l_w + c_w , 0 , 1 ] , + [ -self.l_w - c_w , 0 , 1 ] ]) + + + wings_pts_world = drawing.transform_points_2D( world_T_body , wings_pts_body ) + + lines_pts.append( wings_pts_world ) + lines_style.append( '-') + lines_color.append( 'b') + + ########################### + # bottom line + ########################### + + pts = np.zeros((2,3)) + + pts[0,0] = -10000 + pts[1,0] = 10000 + pts[0,1] = 0 + pts[1,1] = 0 + + lines_pts.append( pts ) + lines_style.append('--') + lines_color.append('k') + + + return lines_pts , lines_style , lines_color + + + ########################################################################### + def forward_kinematic_lines_plus(self, x , u , t ): + """ + plots the force vector + + """ + + lines_pts = [] # list of array (n_pts x 3) for each lines + lines_style = [] + lines_color = [] + + #w = self.width + + q, dq = self.x2q(x) + + x = q[0] + y = q[1] + theta = q[2] + + V , gamma , alpha = self.compute_velocity_vector( q , dq ) + + + world_T_body = geometry.transformation_matrix_2D( theta , x , y ) + body_T_wind = geometry.transformation_matrix_2D( -alpha , 0 , 0 ) + + delta = u[1] + + ########################### + # Trust vector + ########################### + + # Max trust --> arrow is long as airplane + f_scale = self.length / (self.u_ub[0] - self.u_lb[0]) + + trust_vector_lenght = u[0] * f_scale + + #pts = arrow_from_tip_angle( trust_vector_lenght , theta , bx , by ) + + trust_arrow_body = drawing.arrow_from_length_angle( trust_vector_lenght, 0, -self.l_cg, 0 , origin = 'tip') + + trust_arrow_world = drawing.transform_points_2D( world_T_body , trust_arrow_body ) + + lines_pts.append( trust_arrow_world ) + lines_style.append( '-') + lines_color.append( 'r') + + ########################### + # Control surface + ########################### + + c_t = np.sqrt( self.S_t / self.AR ) + + # NOT TO scale to better see the elevator + tail_pts_tail = np.array([ [ 1.0 * c_t , 0 , 1 ] , + [ -1.0 * c_t , 0 , 1 ] ]) + + body_T_tail = geometry.transformation_matrix_2D( delta , - self.l_t , 0 ) + + tail_pts_global = drawing.transform_points_2D( world_T_body @ body_T_tail , tail_pts_tail ) + + lines_pts.append( tail_pts_global ) + lines_style.append( '-') + lines_color.append( 'b') + + # ########################### + # # Velocity vector + # ########################### + + v_length = V * self.length / self.x_ub[3] + if v_length > self.length: v_length = self.length + + v_pts = drawing.arrow_from_length_angle( v_length , 0 ) + + v_world = drawing.transform_points_2D( world_T_body @ body_T_wind , v_pts ) + + lines_pts.append( v_world ) + lines_style.append('-') + lines_color.append('k') + + # ########################### + # # Aero forces + # ########################### + + L_w, D_w, M_w, L_t, D_t, M_t = self.compute_aerodynamic_forces( V , alpha , delta ) + + L_w_pts = drawing.arrow_from_components( 0, L_w * f_scale ) + D_w_pts = drawing.arrow_from_components( -D_w * f_scale, 0 ) + L_t_pts = drawing.arrow_from_components( 0, L_t * f_scale ) + D_t_pts = drawing.arrow_from_components( -D_t * f_scale, 0 ) + + body_T_acw = geometry.transformation_matrix_2D( 0 , -self.l_w , 0 ) + body_T_act = geometry.transformation_matrix_2D( 0 , -self.l_t , 0 ) + + L_w_pts_global = drawing.transform_points_2D( world_T_body @ body_T_acw @ body_T_wind , L_w_pts ) + D_w_pts_global = drawing.transform_points_2D( world_T_body @ body_T_acw @ body_T_wind , D_w_pts ) + + lines_pts.append( L_w_pts_global ) + lines_style.append('-') + lines_color.append('b') + + lines_pts.append( D_w_pts_global ) + lines_style.append('-') + lines_color.append( 'r' ) + + L_t_pts_global = drawing.transform_points_2D( world_T_body @ body_T_act @ body_T_wind , L_t_pts ) + D_t_pts_global = drawing.transform_points_2D( world_T_body @ body_T_act @ body_T_wind , D_t_pts ) + + + lines_pts.append( L_t_pts_global ) + lines_style.append('-') + lines_color.append('b') + + lines_pts.append( D_t_pts_global ) + lines_style.append('-') + lines_color.append('r') + + + return lines_pts , lines_style , lines_color + + + + +''' +################################################################# +################## Main ######## +################################################################# +''' + + +if __name__ == "__main__": + """ MAIN TEST """ + + + if True: + + sys = Plane2D() + + #sys.plot_alpha2Cl() + + sys.x0 = np.array([0,0,0.2,15,0,0]) + + + + def t2u(t): + + u = np.array([ 2 * t , -0.12 * t ]) + + return u + + sys.t2u = t2u + + #sys.gravity = 0 + + sys.compute_trajectory( 2 , 1001 , 'euler' ) + sys.plot_trajectory('x') + + # sys.dynamic_domain = False + sys.animate_simulation( time_factor_video=0.5 ) diff --git a/pyro/dynamic/rigidbody.py b/pyro/dynamic/rigidbody.py new file mode 100644 index 00000000..ab628fce --- /dev/null +++ b/pyro/dynamic/rigidbody.py @@ -0,0 +1,748 @@ +# -*- coding: utf-8 -*- +""" +Created on Tue Oct 23 20:45:37 2018 + +@author: Alexandre +""" + + +############################################################################### +import numpy as np +############################################################################### +from pyro.dynamic import system +from pyro.kinematic import geometry +from pyro.kinematic import drawing +############################################################################### + +############################################################################### +class GeneralizedMechanicalSystem( system.ContinuousDynamicSystem ): + """ + Mechanical system where the generalized velocities are not the time + derivative of the generalized coordinates. + ------------------------------------------------------- + M(q) dv + C(q,v) v + d(q,v) + g(q) = B(q) u + dq = N(q) v + ------------------------------------------------------- + v : dim = (dof, 1) : velocity variables + q : dim = (pos, 1) : position variables + dq : dim = (pos, 1) : derivatives of position variables + dv : dim = (dof, 1) : acceleration variables + u : dim = (m, 1) : force input variables + d(q,v) : dim = (dof, 1) : state-dependent dissipative forces + g(q) : dim = (dof, 1) : state-dependent conservatives forces + M(q) : dim = (dof, dof) : inertia matrix + C(q,v) : dim = (dof, dof) : corriolis matrix + B(q) : dim = (dof, m) : actuator matrix + N(q) : dim = (pos, dof) : transformation matrix + + """ + + ############################ + def __init__(self, dof = 1 , pos = None, actuators = None): + """ """ + + # Degree of Freedom + self.dof = dof + + # Nb of configurations + if pos == None: # If not specifyied + pos = dof + self.pos = pos + + # Nb of actuators + if actuators == None: # If not specifyied the sys is fully actuated + actuators = dof + + # Dimensions + n = dof + pos + m = actuators + p = n + + # initialize standard params + system.ContinuousDynamicSystem.__init__(self, n, m, p) + + # Name + self.name = str(dof) + 'DoF Generalized Mechanical System' + + # Labels, bounds and units + for i in range(pos): + # positions states + self.x_ub[i] = + 10 + self.x_lb[i] = - 10 + self.state_label[i] = 'Position '+ str(i) + self.state_units[i] = '[m]' + for i in range(dof): + # generalized velocity states + j = i + pos + self.x_ub[j] = + 10 + self.x_lb[j] = - 10 + self.state_label[j] = 'Velocity ' + str(i) + self.state_units[j] = '[m/sec]' + for i in range(actuators): + self.u_ub[i] = + 5 + self.u_lb[i] = - 5 + self.input_label[i] = 'Force input ' + str(i) + self.input_units[i] ='[N]' + self.output_label = self.state_label + self.output_units = self.state_units + + ########################################################################### + # The following functions needs to be overloaded by child classes + # to represent the dynamic of the system + ########################################################################### + + ########################################################################### + def M(self, q ): + """ + Inertia matrix + ---------------------------------- + dim( H ) = ( dof , dof ) + + such that --> Kinetic Energy = 0.5 * v^T * H(q) * v + + """ + + M = np.diag( np.ones( self.dof ) ) # Default is identity matrix + + return M + + + ########################################################################### + def C(self, q , v ): + """ + Corriolis and Centrifugal Matrix + ------------------------------------ + dim( C ) = ( dof , dof ) + + such that --> d H / dt = C + C^T + + + """ + + C = np.zeros( ( self.dof , self.dof ) ) # Default is zeros matrix + + return C + + ########################################################################### + def N(self, q ): + """ + Transformation matrix from generalized velocities to derivatives of + configuration variables + ------------------------------------ + dim( N ) = ( pos , dof ) + + dq = N(q) v + ------------------------------------ + """ + + N = np.zeros( ( self.pos , self.dof ) ) + + for i in range(min( self.pos, self.dof) ): + N[i,i] = 1 # Diag matrix for the first m rows + + return N + + + ########################################################################### + def B(self, q ): + """ + Actuator Matrix : dof x m + """ + + B = np.zeros( ( self.dof , self.m ) ) + + for i in range(min(self.m,self.dof)): + B[i,i] = 1 # Diag matrix for the first m rows + + return B + + + ########################################################################### + def g(self, q ): + """ + Gravitationnal forces vector : dof x 1 + """ + + g = np.zeros( self.dof ) # Default is zero vector + + return g + + + ########################################################################### + def d(self, q , v ): + """ + State-dependent dissipative forces : dof x 1 + """ + + d = np.zeros(self.dof ) # Default is zero vector + + return d + + + ########################################################################### + # No need to overwrite the following functions for custom system + ########################################################################### + + ############################# + def x2q( self, x ): + """ from state vector (x) to angle and speeds (q,dq) """ + + q = x[ 0 : self.pos ] + v = x[ self.pos : self.n ] + + return [ q , v ] + + + ############################# + def q2x( self, q , v ): + """ from angle and speeds (q,dq) to state vector (x) """ + + x = np.zeros( self.n ) + + x[ 0 : self.pos ] = q + x[ self.pos : self.n ] = v + + return x + + + ############################# + def xut2q( self, x , u , t ): + """ compute configuration variables """ + + return self.x2q(x)[0] + + + ############################## + def generalized_forces(self, q , v , dv , t = 0 ): + """ Computed generalized forces given a trajectory """ + + M = self.M( q ) + C = self.C( q , v ) + g = self.g( q ) + d = self.d( q , v ) + + # Generalized forces + forces = M @ dv + C @ v + g + d + + return forces + + + ############################## + def actuator_forces(self, q , v , dv , t = 0 ): + """ Computed actuator forces given a trajectory (inverse dynamic) """ + + if self.dof == self.m: + + B = self.B( q ) + + # Generalized forces + forces = self.generalized_forces( q , v , dv , t ) + + # Actuator forces + u = np.linalg.inv( B ) @ forces + + return u + + else: + + raise NotImplementedError + + + ############################## + def accelerations(self, q , v , u , t = 0 ): + """ + Compute accelerations vector (foward dynamic) + given : + - actuator forces + - actual position and velocities + """ + + M = self.M( q ) + C = self.C( q , v ) + g = self.g( q ) + d = self.d( q , v) + B = self.B( q ) + + dv = np.linalg.inv( M ) @ ( B @ u - C @ v - g - d ) + + return dv + + + ########################################################################### + def f(self, x , u , t = 0 ): + """ + Continuous time foward dynamics evaluation + + dx = f(x,u,t) + + INPUTS + x : state vector n x 1 + u : control inputs vector m x 1 + t : time 1 x 1 + + OUPUTS + dx : state derivative vectror n x 1 + + """ + + # from state vector (x) to position and velocities + [ q , v ] = self.x2q( x ) + + # compute accelerations + dv = self.accelerations( q , v , u , t ) + + # compute derivative of position varibles + dq = self.N( q ) @ v + + # convert to state vector diff (dx) + dx = self.q2x( dq , dv ) + + return dx + + + ########################################################################### + def kinetic_energy(self, q , v ): + """ Compute kinetic energy """ + + e = 0.5 * v.T @ self.M( q ) @ v + + return e + + + + + +############################################################################### + +class GeneralizedMechanicalSystemWithPositionInputs( GeneralizedMechanicalSystem ): + """ + Generalized Mechanical system with position inputs + + ------------------------------------------------------- + M(q) dv + C(q,v) v + d(q,v,u) + g(q) = B(q,u) e(u) + dq = N(q) v + ------------------------------------------------------- + + numper of inputs are: + m = m_f + m_o + --------------------------------------------------- + m : integer : number of inputs + m_f : integer : number of force inputs + m_o : integer : number of other inputs + u : dim = ( m , 1) : vector of all input variables + + v : dim = (dof, 1) : velocity variables + q : dim = (pos, 1) : position variables + dq : dim = (pos, 1) : derivatives of position variables + dv : dim = (dof, 1) : acceleration variables + e : dim = (m_f, 1) : force input variables + d(q,v,u) : dim = (dof, 1) : state-dependent dissipative forces + g(q) : dim = (dof, 1) : state-dependent conservatives forces + M(q) : dim = (dof, dof) : inertia matrix + C(q,v) : dim = (dof, dof) : corriolis matrix + B(q,u) : dim = (dof, m_f) : actuator matrix + N(q) : dim = (pos, dof) : transformation matrix + + """ + + ############################ + def __init__(self, dof = 1 , force_inputs = 1, other_inputs = 1, pos = None): + """ """ + + # Degree of Freedom + self.dof = dof + + self.m_f = force_inputs + self.m_o = other_inputs + + # Nb of configurations + if pos == None: # If not specifyied + pos = dof + self.pos = pos + + # Dimensions + n = dof + pos + m = self.m_f + self.m_o + p = n + + # initialize standard params + system.ContinuousDynamicSystem.__init__(self, n, m, p) + + # Name + self.name = str(dof) + 'DoF Generalized Mechanical System' + + # Labels, bounds and units + for i in range(pos): + # positions states + self.x_ub[i] = + 10 + self.x_lb[i] = - 10 + self.state_label[i] = 'Position '+ str(i) + self.state_units[i] = '[m]' + for i in range(dof): + # generalized velocity states + j = i + pos + self.x_ub[j] = + 10 + self.x_lb[j] = - 10 + self.state_label[j] = 'Velocity ' + str(i) + self.state_units[j] = '[m/sec]' + for i in range(self.m_f): + self.u_ub[i] = + 5 + self.u_lb[i] = - 5 + self.input_label[i] = 'Force input ' + str(i) + self.input_units[i] ='[N]' + self.output_label = self.state_label + self.output_units = self.state_units + + ########################################################################### + # The following functions needs to be overloaded by child classes + # to represent the dynamic of the system + ########################################################################### + + ############################# + def u2e( self, u ): + """ + extract force inputs from all inputs + """ + + e = u[ 0 : self.m_f ] + + return e + + ########################################################################### + def B(self, q , u ): + """ + Actuator Matrix : dof x m + """ + + B = np.zeros( ( self.dof , self.m_f ) ) + + for i in range(min( self.m_f , self.dof )): + B[i,i] = 1 # Diag matrix for the first m rows + + return B + + + ########################################################################### + def d(self, q , v , u ): + """ + State-dependent dissipative forces : dof x 1 + """ + + d = np.zeros(self.dof ) # Default is zero vector + + return d + + + ########################################################################### + # No need to overwrite the following functions for custom system + ########################################################################### + + ############################## + def generalized_forces(self, q , v , dv , t = 0 ): + + raise NotImplementedError + + + ############################## + def actuator_forces(self, q , v , dv , t = 0 ): + """ Computed actuator forces given a trajectory (inverse dynamic) """ + + raise NotImplementedError + + + ############################## + def accelerations(self, q , v , u , t = 0 ): + """ + Compute accelerations vector (foward dynamic) + given : + - actuator forces + - actual position and velocities + """ + + M = self.M( q ) + C = self.C( q , v ) + g = self.g( q ) + d = self.d( q , v, u ) + B = self.B( q , u ) + + e = self.u2e( u ) + + dv = np.linalg.inv( M ) @ ( B @ e - C @ v - g - d ) + + return dv + + + +############################################################################### + +class RigidBody2D( GeneralizedMechanicalSystemWithPositionInputs ): + """ + + Mechanical system with Equations of Motion written in + a body-fixed frame of reference + + """ + + ############################ + def __init__(self, force_inputs = 2, other_inputs = 0): + """ """ + + # Degree of Freedom + self.dof = 3 + self.pos = 3 + + self.m_f = force_inputs + self.m_o = other_inputs + + # Dimensions + n = 6 + m = self.m_f + self.m_o + p = n + + # initialize standard params + system.ContinuousDynamicSystem.__init__(self, n, m, p) + + # Name + self.name = 'Planar Rigid Body' + self.state_label = ['x','y','theta','v1','v2','w'] + self.state_units = ['[m]','[m]','[rad]','[m/sec]','[m/sec]','[rad/sec]'] + self.input_label = ['u1','u2'] + self.input_units = ['[]','[]'] + self.output_label = self.state_label + self.output_units = self.state_units + + # Dynamic properties + self.mass = 1.0 + self.inertia = 1.0 + self.l_t = 1.0 # Distance between CG and Thrust + + # Default graphic output parameters + self.dynamic_domain = True + + + ########################################################################### + def M(self, q ): + + M = np.diag( np.array([ self.mass , self.mass, self.inertia ]) ) + + return M + + + ########################################################################### + def C(self, q , v ): + + C = np.zeros( ( self.dof , self.dof ) ) + + w = v[2] + + C[1,0] = + self.mass * w + C[0,1] = - self.mass * w + + return C + + ########################################################################### + def N(self, q ): + """ + Transformation matrix from generalized velocities to derivatives of + configuration variables + ------------------------------------ + dim( N ) = ( pos , dof ) + + dq = N(q) v + ------------------------------------ + """ + + theta = q[2] + + N = np.array( [ [ np.cos(theta) , -np.sin(theta) , 0 ] , + [ np.sin(theta) , +np.cos(theta) , 0 ] , + [ 0 , 0 , 1 ] ] ) + + return N + + ########################################################################### + def g(self, q ): + """ + Gravitationnal forces vector : dof x 1 + """ + + g = np.zeros( self.dof ) # Default is zero vector + + return g + + + ########################################################################### + def d(self, q , v , u): + """ + State-dependent dissipative forces : dof x 1 + """ + + d = np.zeros(self.dof ) # Default is zero vector + + return d + + ########################################################################### + def B(self, q , u ): + """ + Actuator Matrix + ------------------ + This placeholder is for a 2D point force [ F_x , F_y ] + applied at a point located at a distance l_t behind the CG + """ + + B = np.zeros(( self.dof, self.m_f )) + + B[0,0] = 1 + B[1,1] = 1 + B[2,1] = - self.l_t + + return B + + ########################################################################### + def forward_kinematic_domain(self, q ): + """ + Place holder graphical output ( box with a force ) + """ + + l = self.l_t * 10 + + x = q[0] + y = q[1] + z = 0 + + if self.dynamic_domain: + + domain = [ ( -l + x , l + x ) , + ( -l + y , l + y ) , + ( -l + z , l + z ) ]# + else: + + domain = [ ( -l , l ) , + ( -l , l ) , + ( -l , l ) ]# + + return domain + + + ########################################################################### + def forward_kinematic_lines(self, q ): + """ + Place holder graphical output ( box with a force ) + """ + + lines_pts = [] # list of array (n_pts x 3) for each lines + lines_style = [] + lines_color = [] + + ########################### + # body + ########################### + + x = q[0] + y = q[1] + theta = q[2] + + W_T_B = geometry.transformation_matrix_2D( theta , x , y ) + + w = self.l_t + + # Points in body frame + pts = np.zeros(( 5 , 3 )) + pts[0,:] = np.array([-w,+w,0]) + pts[1,:] = np.array([-w,-w,0]) + pts[2,:] = np.array([+w,-w,0]) + pts[3,:] = np.array([+w,+w,0]) + pts[4,:] = np.array([-w,+w,0]) + + pts_W = drawing.transform_points_2D( W_T_B , pts ) + + lines_pts.append( pts_W ) + lines_style.append( '-') + lines_color.append( 'b' ) + + ########################### + # C.G. + ########################### + + pts = np.zeros(( 1 , 3 )) + pts[0,:] = np.array([x,y,0]) + + lines_pts.append( pts ) + lines_style.append( 'o') + lines_color.append( 'b' ) + + ########################### + # Diff flat output + ########################### + + J = self.inertia + m = self.mass + r = self.l_t + + xp = x + J / (m * r) * np.cos(theta) + yp = y + J / (m * r) * np.sin(theta) + + pts = np.zeros(( 1 , 3 )) + pts[0,:] = np.array([xp,yp,0]) + + lines_pts.append( pts ) + lines_style.append( 'o') + lines_color.append( 'r' ) + + return lines_pts , lines_style , lines_color + + ########################################################################### + def forward_kinematic_lines_plus(self, x , u , t ): + """ + Place holder graphical output ( box with a force ) + """ + + lines_pts = [] # list of array (n_pts x 3) for each lines + lines_style = [] + lines_color = [] + + # M per Newton of force + f2r = 1.0 / self.u_ub[0] * self.l_t + + ########################### + # force vector + ########################### + + vx = u[0] * f2r + vy = u[1] * f2r + offset = -self.l_t + + pts_body = drawing.arrow_from_components( vx , vy , x = offset, origin = 'tip' ) + W_T_B = geometry.transformation_matrix_2D( x[2], x[0] , x[1] ) + pts_W = drawing.transform_points_2D( W_T_B , pts_body ) + + lines_pts.append( pts_W ) + lines_style.append( '-') + lines_color.append( 'r' ) + + return lines_pts , lines_style , lines_color + + +''' +################################################################# +################## Main ######## +################################################################# +''' + + +if __name__ == "__main__": + """ MAIN TEST """ + + #sys = GeneralizedMechanicalSystem( dof = 2 , pos = 2 , actuators = 2 ) + #sys = GeneralizedMechanicalSystemWithPositionInputs( dof = 3 , pos = 1 , force_inputs= 1 , other_inputs=1 ) + sys = RigidBody2D() + + #sys.show( q = np.array([ 1.0, 2.0, 0.5 ]) ) + + sys.ubar = np.array([1,0.2]) + sys.x0 = np.array([0,0,0,0,0,0]) + + sys.compute_trajectory( tf = 20 ) + sys.plot_trajectory() + sys.animate_simulation() + \ No newline at end of file diff --git a/pyro/dynamic/rocket.py b/pyro/dynamic/rocket.py index 2d864d47..c5c5461d 100644 --- a/pyro/dynamic/rocket.py +++ b/pyro/dynamic/rocket.py @@ -10,8 +10,9 @@ import numpy as np import matplotlib.pyplot as plt ############################################################################### -from pyro.dynamic import system -from pyro.dynamic import mechanical +from pyro.dynamic import mechanical +from pyro.kinematic import geometry +from pyro.kinematic import drawing ############################################################################### @@ -63,6 +64,24 @@ def __init__(self): self.dynamic_domain = True self.dynamic_range = 10 + # rocket drawing + pts = np.zeros(( 10 , 3 )) + l = self.height + w = self.width + + pts[0,:] = np.array([ 0, -l,0]) + pts[1,:] = np.array([-w, -l,0]) + pts[2,:] = np.array([-w, +l,0]) + pts[3,:] = np.array([ 0,l+w,0]) + pts[4,:] = np.array([+w, +l,0]) + pts[5,:] = np.array([+w, -l,0]) + pts[6,:] = pts[0,:] + pts[7,:] = pts[0,:] + np.array([-w,-w,0]) + pts[8,:] = pts[0,:] + np.array([+w,-w,0]) + pts[9,:] = pts[0,:] + + self.drawing_body_pts = pts + ########################################################################### def H(self, q ): @@ -132,7 +151,7 @@ def g(self, q ): ########################################################################### - def d(self, q , dq ): + def d(self, q , dq , u ): """ State-dependent dissipative forces : dof x 1 """ @@ -206,32 +225,21 @@ def forward_kinematic_lines(self, q ): # body ########################### - x = q[0] - y = q[1] - s = np.sin(q[2]) - c = np.cos(q[2]) - l = self.height - w = self.width - - pts = np.zeros(( 10 , 3 )) - pts[0,:] = np.array([x+l*s,y-l*c,0]) - pts[1,:] = pts[0,:] + np.array([-w*c,-w*s,0]) - pts[2,:] = pts[1,:] + np.array([-2*l*s,2*l*c,0]) - pts[3,:] = np.array([x-(l+w)*s,y+(l+w)*c,0]) - pts[4,:] = np.array([x-l*s+w*c,y+l*c+w*s,0]) - pts[5,:] = pts[4,:] + np.array([2*l*s,-2*l*c,0]) - pts[6,:] = pts[0,:] - pts[7,:] = pts[0,:] + np.array([w*s-w*c,-w*c-w*s,0]) - pts[8,:] = pts[0,:] + np.array([w*s+w*c,-w*c+w*s,0]) - pts[9,:] = pts[0,:] + x = q[0] + y = q[1] + theta = q[2] + W_T_B = geometry.transformation_matrix_2D( theta , x , y ) - lines_pts.append( pts ) + pts_B = self.drawing_body_pts + pts_W = drawing.transform_points_2D( W_T_B , pts_B ) + + lines_pts.append( pts_W ) lines_style.append( '-') lines_color.append( 'b' ) ########################### - # cg + # C.G. ########################### pts = np.zeros(( 1 , 3 )) @@ -257,32 +265,18 @@ def forward_kinematic_lines_plus(self, x , u , t ): lines_color = [] ########################### - # trust force vectors + # trust force vector ########################### - l = self.height - - s = np.sin(x[2]) - c = np.cos(x[2]) - + length = u[0] * 0.0002 # arrow length + theta = u[1] - 0.5 * np.pi # arrow angle (body frame) + y_offset = -self.height - self.width - xb = x[0]+l*s - yb = x[1]-l*c + pts_body = drawing.arrow_from_length_angle( length, theta, y = y_offset ) + W_T_B = geometry.transformation_matrix_2D( x[2], x[0] , x[1] ) + pts_W = drawing.transform_points_2D( W_T_B , pts_body ) - s = np.sin(x[2]+u[1]) - c = np.cos(x[2]+u[1]) - - T = u[0] * 0.0002 - h = self.width - - pts = np.zeros(( 5 , 3 )) - pts[0,:] = np.array([xb,yb,0]) - pts[1,:] = pts[0,:] + np.array([T*s,-T*c,0]) - pts[2,:] = pts[1,:] + np.array([h*c-h*s,h*s+h*c,0]) - pts[3,:] = pts[1,:] - pts[4,:] = pts[1,:] + np.array([-h*c-h*s,-h*s+h*c,0]) - - lines_pts.append( pts ) + lines_pts.append( pts_W ) lines_style.append( '-') lines_color.append( 'r' ) @@ -305,7 +299,7 @@ def forward_kinematic_lines_plus(self, x , u , t ): sys.x0[0] = 0 sys.ubar[0] = sys.mass * sys.gravity * 1.1 - sys.ubar[1] = -0.005 + sys.ubar[1] = -0.001 sys.plot_trajectory() sys.animate_simulation() \ No newline at end of file diff --git a/pyro/dynamic/statespace.py b/pyro/dynamic/statespace.py index 47725db4..b3dcd701 100644 --- a/pyro/dynamic/statespace.py +++ b/pyro/dynamic/statespace.py @@ -35,6 +35,8 @@ def __init__(self, A, B, C, D): ContinuousDynamicSystem.__init__( self, n, m, p) + self.is_vectorized = True + ############################################ def _check_dimensions(self): @@ -205,6 +207,7 @@ def linearize(sys, epsilon_x=0.001, epsilon_u=None): xbar = sys.xbar.astype(float) ubar = sys.ubar.astype(float) + tbar = sys.tbar epsilon_x = np.asarray(epsilon_x) @@ -223,16 +226,16 @@ def linearize(sys, epsilon_x=0.001, epsilon_u=None): def f_x(x): - return sys.f(x, ubar, 0) + return sys.f(x, ubar, tbar) def f_u(u): - return sys.f(xbar, u, 0) + return sys.f(xbar, u, tbar) def h_x(x): - return sys.h(x, ubar, 0) + return sys.h(x, ubar, tbar) def h_u(u): - return sys.h(xbar, u, 0) + return sys.h(xbar, u, tbar) A = _approx_jacobian(f_x, xbar, epsilon_x) B = _approx_jacobian(f_u, ubar, epsilon_u) diff --git a/pyro/dynamic/system.py b/pyro/dynamic/system.py index dd746fba..7536f9eb 100644 --- a/pyro/dynamic/system.py +++ b/pyro/dynamic/system.py @@ -11,29 +11,29 @@ from pyro.analysis import phaseanalysis from pyro.analysis import graphical from pyro.analysis import costfunction - -''' + +""" ############################################################################### -''' +""" ############################################################################### class ContinuousDynamicSystem: - """ + """ Mother class for continuous dynamical systems - + Main functions --------------------------------------------- dx = f( x , u , t ) : dynamic equation - - optionnal: + + optionnal: y = h( x , u , t ) : output equation (by default y = x ) u = t2u( t ) : time-dependent input signal (by default u = ubar) q = xut2q( x , u , t ) : get configuration vector (by default q = x ) - + graphic output: lines = forward_kinematic_lines( q ) : draw the system based on q - + Signal and Dimentions ---------------------------------------------- x : state vector n x 1 @@ -41,305 +41,297 @@ class ContinuousDynamicSystem: t : time 1 x 1 y : output vector p x 1 q : configuration vector ? x 1 (dimension is not imposed) - + """ + ########################################################################### # The two following functions needs to be implemented by child classes ########################################################################### - + ############################ - def __init__(self, n = 1, m = 1, p = 1): - """ + def __init__(self, n=1, m=1, p=1): + """ The __init__ method of the Mother class can be used to fill-in default labels, units, bounds, and base values. """ - + ############################# # Parameters ############################# # Dimensions - self.n = n - self.m = m + self.n = n + self.m = m self.p = p - + # Labels - self.name = 'ContinuousDynamicSystem' - self.state_label = [] - self.input_label = [] + self.name = "ContinuousDynamicSystem" + self.state_label = [] + self.input_label = [] self.output_label = [] - + # Units - self.state_units = [] - self.input_units = [] + self.state_units = [] + self.input_units = [] self.output_units = [] - + # Default Label and units for i in range(n): - self.state_label.append('State '+str(i)) - self.state_units.append('') + self.state_label.append("State " + str(i)) + self.state_units.append("") for i in range(m): - self.input_label.append('Input '+str(i)) - self.input_units.append('') + self.input_label.append("Input " + str(i)) + self.input_units.append("") for i in range(p): - self.output_label.append('Output '+str(i)) - self.output_units.append('') - + self.output_label.append("Output " + str(i)) + self.output_units.append("") + # Default state and input domain - self.x_ub = np.zeros(self.n) +10 # States Upper Bounds - self.x_lb = np.zeros(self.n) -10 # States Lower Bounds - self.u_ub = np.zeros(self.m) +1 # Control Upper Bounds - self.u_lb = np.zeros(self.m) -1 # Control Lower Bounds - - # Default state and inputs values + self.x_ub = np.zeros(self.n) + 10 # States Upper Bounds + self.x_lb = np.zeros(self.n) - 10 # States Lower Bounds + self.u_ub = np.zeros(self.m) + 1 # Control Upper Bounds + self.u_lb = np.zeros(self.m) - 1 # Control Lower Bounds + + # Default state and inputs values self.xbar = np.zeros(self.n) self.ubar = np.zeros(self.m) - + self.tbar = 0 + # Plot params - self.domain = [ (-10,10) , (-10,10) , (-10,10) ] - self.linestyle = 'o-' - self.linestyle_plus = '--' - self.linescolor = 'b' - self.linescolor_plus = 'r' - self.lines_plus = True # Bool to active second graphic outpout - self.is_3d = False # Use 2d plot by default - + self.domain = [(-10, 10), (-10, 10), (-10, 10)] + self.linestyle = "o-" + self.linestyle_plus = "--" + self.linescolor = "b" + self.linescolor_plus = "r" + self.lines_plus = True # Bool to active second graphic outpout + self.is_3d = False # Use 2d plot by default + ################################ # Variables ################################ - + # Initial value for simulations - self.x0 = np.zeros(self.n) - + self.x0 = np.zeros(self.n) + # Result of last simulation self.traj = None - + # Cost function for evaluation # default is a quadratic cost function with diag Q and R matrices self.cost_function = costfunction.QuadraticCostFunction.from_sys(self) - ############################# - def f( self , x , u , t ): - """ + def f(self, x, u, t): + """ Continuous time foward dynamics evaluation dx = f(x,u,t) - + INPUTS x : state vector n x 1 u : control inputs vector m x 1 t : time 1 x 1 - + OUPUTS dx : state derivative vector n x 1 - + """ - - dx = np.zeros(self.n) # State derivative vector - + + dx = np.zeros(self.n) # State derivative vector + ################################################ # Place holder: put the equations of motion here raise NotImplementedError ################################################ - + return dx - - + ########################################################################### # The following functions can be overloaded when necessary by child classes ########################################################################### - + ############################# - def h( self , x , u , t ): - """ + def h(self, x, u, t): + """ Output fonction y = h(x,u,t) - + INPUTS x : state vector n x 1 u : control inputs vector m x 1 t : time 1 x 1 - + OUTPUTS y : output vector p x 1 - + """ - - #y = np.zeros(self.p) # Output vector - - y = x # default output is all states - + + # y = np.zeros(self.p) # Output vector + + y = x # default output is all states + return y - + ############################# - def t2u( self , t ): - """ + def t2u(self, t): + """ Reference signal fonction u = t2u(t) - + INPUTS t : time 1 x 1 - + OUTPUTS u : control inputs vector m x 1 - + Defaul is a constant signal equal to self.ubar, can be overloaded - with a more complexe reference signal time-function - + with a more complexe reference signal time-function + """ - + # Default is a constant signal u = self.ubar - + return u - - + ########################################################################### # Basic domain checks, overload if something more complex is needed ########################################################################### - + ############################# - def isavalidstate(self , x ): - """ check if x is in the state domain """ + def isavalidstate(self, x): + """check if x is in the state domain""" ans = False for i in range(self.n): - ans = ans or ( x[i] < self.x_lb[i] ) - ans = ans or ( x[i] > self.x_ub[i] ) - - return not(ans) - + ans = ans or (x[i] < self.x_lb[i]) + ans = ans or (x[i] > self.x_ub[i]) + + return not (ans) + ############################# - def isavalidinput(self , x , u): - """ check if u is in the control inputs domain given x """ + def isavalidinput(self, x, u): + """check if u is in the control inputs domain given x""" ans = False for i in range(self.m): - ans = ans or ( u[i] < self.u_lb[i] ) - ans = ans or ( u[i] > self.u_ub[i] ) - - return not(ans) - - + ans = ans or (u[i] < self.u_lb[i]) + ans = ans or (u[i] > self.u_ub[i]) + + return not (ans) + ########################################################################### # Place holder graphical output, overload with specific graph output ########################################################################### - + ############################# - def xut2q( self, x , u , t ): - """ Compute configuration variables ( q vector ) """ - + def xut2q(self, x, u, t): + """Compute configuration variables ( q vector )""" + # default is q = x - + return x - - + ########################################################################### - def forward_kinematic_domain(self, q ): - """ Set the domain range for ploting, can be static or dynamic """ - + def forward_kinematic_domain(self, q): + """Set the domain range for ploting, can be static or dynamic""" + return self.domain - - + ########################################################################### - def forward_kinematic_lines(self, q ): - """ - Compute points p = [x;y;z] positions given config q + def forward_kinematic_lines(self, q): + """ + Compute points p = [x;y;z] positions given config q ---------------------------------------------------- - points of interest for ploting - + Outpus: lines_pts = [] : a list of array (n_pts x 3) for each lines - + """ - - lines_pts = [] # list of array (n_pts x 3) for each lines - + + lines_pts = [] # list of array (n_pts x 3) for each lines + ########################### # Your graphical code here ########################### - + # simple place holder for q_i in q: - pts = np.zeros(( 1 , 3 )) # array of 1 pts for the line - pts[0,0] = q_i # x cord of point 0 = q - lines_pts.append( pts ) # list of all arrays of pts - + pts = np.zeros((1, 3)) # array of 1 pts for the line + pts[0, 0] = q_i # x cord of point 0 = q + lines_pts.append(pts) # list of all arrays of pts + return lines_pts - - + ########################################################################### - def forward_kinematic_lines_plus(self, x , u , t ): - """ + def forward_kinematic_lines_plus(self, x, u, t): + """ Additionnal optionnal graphic output used in animations - + Compute points p = [x;y;z] positions given state x , u , t ------------------------------------------------------------- - additionnal points of interest for ploting - for instances forces arrows illustrating control inputs - + Outpus: lines_pts = [] : a list of array (n_pts x 3) for each lines - + """ - - lines_pts = [] # list of array (n_pts x 3) for each lines - + + lines_pts = [] # list of array (n_pts x 3) for each lines + ########################### # Your graphical code here ########################### - + # simple place holder for u_i in u: - pts = np.zeros(( 1 , 3 )) # array of 1 pts for the line - pts[0,0] = u_i # x cord of point 0 = u - lines_pts.append( pts ) # list of all arrays of pts - + pts = np.zeros((1, 3)) # array of 1 pts for the line + pts[0, 0] = u_i # x cord of point 0 = u + lines_pts.append(pts) # list of all arrays of pts + return lines_pts - - + ########################################################################### # No need to overwrite the following functions for custom dynamic systems ########################################################################### - + ############################# - def fsim( self, x , t = 0 ): - """ + def fsim(self, x, t=0): + """ Continuous time foward dynamics evaluation dx = f(x,t), inlcuding the internal reference input signal computation - + INPUTS x : state vector n x 1 t : time 1 x 1 - + OUPUTS dx : state derivative vector n x 1 - + """ - - u = self.t2u( t ) - dx = self.f( x, u, t) - + + u = self.t2u(t) + dx = self.f(x, u, t) + return dx - ############################# - def x_next( self , x , u , t = 0 , dt = 0.1 , steps = 1 ): - """ - Discrete time foward dynamics evaluation + def x_next(self, x, u, t=0, dt=0.1, steps=1): + """ + Discrete time foward dynamics evaluation ------------------------------------- - using Euler integration - + """ - - x_next = np.zeros(self.n) # k+1 State vector - + + x_next = np.zeros(self.n) # k+1 State vector + # Multiple integration steps for i in range(steps): - - x_next = self.f(x,u,t) * dt + x - + + x_next = self.f(x, u, t) * dt + x + # Multiple steps - x = x_next - + x = x_next + return x_next - - + ########################################################################### # Quick Analysis Shorcuts ########################################################################### @@ -369,41 +361,35 @@ def new_h(x, u, t): pass """ - - - + ############################# def get_plotter(self): - """ Return a Plotter object with param based on sys instance """ - + """Return a Plotter object with param based on sys instance""" + return graphical.TrajectoryPlotter(self) - - + ############################# def get_animator(self): - """ Return an Animator object with param based on sys instance """ - + """Return an Animator object with param based on sys instance""" + return graphical.Animator(self) - ############################# - def plot_phase_plane(self , x_axis = 0 , y_axis = 1 ): - """ + def plot_phase_plane(self, x_axis=0, y_axis=1): + """ Plot Phase Plane vector field of the system ------------------------------------------------ x_axis : index of state on x axis y_axis : index of state on y axis - + """ - pp = phaseanalysis.PhasePlot( self , x_axis , y_axis ) - + pp = phaseanalysis.PhasePlot(self, x_axis, y_axis) + pp.plot() - - + ############################# - def compute_trajectory( - self, tf=10, n=10001, solver='solve_ivt', **solver_args): + def compute_trajectory(self, tf=10, n=10001, solver="solve_ivt", **solver_args): """ Simulation of time evolution of the system ------------------------------------------------ @@ -418,24 +404,22 @@ def compute_trajectory( return self.traj - ############################# - def plot_trajectory(self, plot='x', **kwargs): + def plot_trajectory(self, plot="x", **kwargs): """ Plot time evolution of a simulation of this system ------------------------------------------------ note: will call compute_trajectory if no simulation data is present """ - + # Check if trajectory is already computed if self.traj == None: self.compute_trajectory() - + plotter = self.get_plotter() - - return plotter.plot( self.traj, plot, **kwargs) + return plotter.plot(self.traj, plot, **kwargs) ############################# def plot_phase_plane_trajectory(self, x_axis=0, y_axis=1): @@ -443,56 +427,51 @@ def plot_phase_plane_trajectory(self, x_axis=0, y_axis=1): Plot a trajectory in the Phase Plane --------------------------------------------------------------- note: will call compute_trajectory if no simulation data is present - + """ - + # Check is trajectory is already computed if self.traj == None: self.compute_trajectory() - + plotter = self.get_plotter() - - return plotter.phase_plane_trajectory( self.traj, x_axis , y_axis) + return plotter.phase_plane_trajectory(self.traj, x_axis, y_axis) ############################# - def plot_phase_plane_trajectory_3d(self , x_axis=0, y_axis=1, z_axis=2): + def plot_phase_plane_trajectory_3d(self, x_axis=0, y_axis=1, z_axis=2): """ Plot the trajectory in the Phase Plane --------------------------------------------------------------- note: will call compute_trajectory if no simulation data is present """ - + # Check is trajectory is already computed if self.traj == None: self.compute_trajectory() - + plotter = self.get_plotter() - - return plotter.phase_plane_trajectory_3d( - self.traj, x_axis , y_axis, z_axis) + return plotter.phase_plane_trajectory_3d(self.traj, x_axis, y_axis, z_axis) ############################################# - def show(self, q , x_axis = 0 , y_axis = 1 ): - """ Plot figure of configuration q """ - + def show(self, q, x_axis=0, y_axis=1): + """Plot figure of configuration q""" + ani = self.get_animator() - ani.x_axis = x_axis - ani.y_axis = y_axis - - ani.show( q ) - - + ani.x_axis = x_axis + ani.y_axis = y_axis + + ani.show(q) + ############################################# - def show3(self, q ): - """ Plot figure of configuration q """ - + def show3(self, q): + """Plot figure of configuration q""" + ani = self.get_animator() - - ani.show3( q ) - + + ani.show3(q) ############################## def animate_simulation(self, **kwargs): @@ -504,16 +483,15 @@ def animate_simulation(self, **kwargs): note: will call compute_trajectory if no simulation data is present """ - + # Check is trajectory is already computed if self.traj == None: self.compute_trajectory() - + ani = self.get_animator() - - return ani.animate_simulation( self.traj, **kwargs) - - + + return ani.animate_simulation(self.traj, **kwargs) + ############################## def generate_simulation_html_video(self, **kwargs): """ @@ -524,124 +502,137 @@ def generate_simulation_html_video(self, **kwargs): note: will call compute_trajectory if no simulation data is present """ - + # Check is trajectory is already computed if self.traj == None: self.compute_trajectory() - + animator = self.get_animator() - animator.animate_simulation( self.traj, show = False , **kwargs ) + animator.animate_simulation(self.traj, show=False, **kwargs) html_video = animator.ani.to_html5_video() - + return html_video - - + ############################# - def generate_mode_animation_html(self, i = 0 , is_3d = False ): + def generate_mode_animation_html(self, i=0, is_3d=False): """ Linearize and show eigen mode i with html output """ - + from pyro.dynamic.statespace import linearize - - # - ss = linearize( self ) - + + # + ss = linearize(self) + # Compute eigen decomposition ss.compute_eigen_modes() - + # Simulate one mode - traj = ss.compute_eigen_mode_traj( i ) - + traj = ss.compute_eigen_mode_traj(i) + # Animate mode - animator = ss.get_animator() - - #label - template = 'Mode %i \n%0.1f+%0.1fj' - label = template % (i, ss.poles[i].real, ss.poles[i].imag) + animator = ss.get_animator() + + # label + template = "Mode %i \n%0.1f+%0.1fj" + label = template % (i, ss.poles[i].real, ss.poles[i].imag) animator.top_right_label = label - - animator.animate_simulation( traj, 3.0, is_3d , show = False) - + + animator.animate_simulation(traj, 3.0, is_3d, show=False) + html_video = animator.ani.to_html5_video() - + return html_video - - + ############################# def plot_linearized_bode(self, u_index=0, y_index=0): """ Bode plot of linearized siso """ - + from pyro.dynamic.statespace import linearize from pyro.dynamic.tranferfunction import ss2tf - - linearized_sys = linearize( self ) - siso_sys = ss2tf( linearized_sys, u_index, y_index) + + linearized_sys = linearize(self) + siso_sys = ss2tf(linearized_sys, u_index, y_index) siso_sys.bode_plot() - - + ############################# - def plot_linearized_pz_map(self, u_index = 0 , y_index = 0 ): - """ + def plot_linearized_pz_map(self, u_index=0, y_index=0): + """ """ - """ - from pyro.dynamic.statespace import linearize from pyro.dynamic.tranferfunction import ss2tf - - linearized_sys = linearize( self ) - siso_sys = ss2tf( linearized_sys, u_index, y_index) + + linearized_sys = linearize(self) + siso_sys = ss2tf(linearized_sys, u_index, y_index) siso_sys.pz_map() - - + ############################# - def animate_linearized_mode(self, i = 0 ): + def animate_linearized_mode(self, i=0): """ Linearize and show eigen mode i """ - + from pyro.dynamic.statespace import linearize - - linearized_sys = linearize( self ) - - linearized_sys.animate_eigen_mode( i , self.is_3d ) - + + linearized_sys = linearize(self) + + linearized_sys.animate_eigen_mode(i, self.is_3d) + return linearized_sys - - + ############################# - def animate_linearized_modes(self ): + def animate_linearized_modes(self): """ Linearize and show eigen modes """ - + from pyro.dynamic.statespace import linearize - - linearized_sys = linearize( self ) - + + linearized_sys = linearize(self) + animations = [] - + for i in range(self.n): - ani = linearized_sys.animate_eigen_mode( i , self.is_3d ) - - animations.append( ani ) - - return linearized_sys , animations + ani = linearized_sys.animate_eigen_mode(i, self.is_3d) + + animations.append(ani) + + return linearized_sys, animations + + ############################# + def convert_to_gymnasium(self, dt=0.05, tf=10.0, t0=0.0, render_mode=None): + """ + Create a gym environment from the system + + """ + try: + import gymnasium as gym + from gymnasium import spaces + from pyro.tools.sys2gym import Sys2Gym -''' + gym_sys = Sys2Gym(self, dt, tf, t0, render_mode) + + return gym_sys + + except: + + raise ImportError("gym library is not installed") + + +""" ################################################################# ################## Main ######## ################################################################# -''' +""" -if __name__ == "__main__": - """ MAIN TEST """ - - pass \ No newline at end of file +if __name__ == "__main__": + """MAIN TEST""" + + pass diff --git a/pyro/dynamic/longitudinal_vehicule.py b/pyro/dynamic/vehicle_propulsion.py similarity index 91% rename from pyro/dynamic/longitudinal_vehicule.py rename to pyro/dynamic/vehicle_propulsion.py index cc8a360b..82443e5e 100644 --- a/pyro/dynamic/longitudinal_vehicule.py +++ b/pyro/dynamic/vehicle_propulsion.py @@ -12,7 +12,6 @@ from pyro.dynamic import system ############################################################################## -import matplotlib import matplotlib.pyplot as plt @@ -69,7 +68,7 @@ def __init__(self): self.cdA = 0.3 * 2 # drag coef time area [m2] # Ground traction curve parameters - self.mu_max = 0.5 + self.mu_max = 1.0 self.mu_slope = 70. @@ -171,17 +170,58 @@ def f(self, x , u , t = 0 ): dx[0] = v # velocity dx[1] = a # acc - ################### + # ################### + # # Normal force check + # fn_front = m * g * rr - m * a * ry + # fn_rear = m * g * rf + m * a * ry + # if (fn_front<0) : + # print('Normal force on front wheel is negative: fn = ', fn_front) + # if (fn_rear<0) : + # print('Normal force on rear wheel is negative: fn = ', fn_rear) + # ################### + + return dx + + ############################# + def isavalidinput(self , x , u): + """ check if u is in the control inputs domain given x """ + + ans = False + + # Min-Max Slip + + for i in range(self.m): + ans = ans or ( u[i] < self.u_lb[i] ) + ans = ans or ( u[i] > self.u_ub[i] ) + + # Normal Forces Negatives? + + slip = u + v = x[1] + + # compute ratio of horizontal/vertical force + mu = self.slip2force( slip ) + + # constant params local vairables + ry, rr, rf = self.compute_ratios() + m = self.mass + g = self.gravity + rcda = self.rho * self.cdA + + # Drag froce + fd = 0.5 * rcda * v * np.abs( v ) # drag froce with the right sign + + # Acceleration (equation considering weight transfer) + a = (mu * m * g * rr - fd )/( m * (1 + mu * ry )) + # Normal force check fn_front = m * g * rr - m * a * ry fn_rear = m * g * rf + m * a * ry - if (fn_front<0) : - print('Normal force on front wheel is negative: fn = ', fn_front) - if (fn_rear<0) : - print('Normal force on rear wheel is negative: fn = ', fn_rear) - ################### - return dx + ans = ans or ( fn_front < 0. ) + ans = ans or ( fn_rear < 0. ) + + return not(ans) ########################################################################### diff --git a/pyro/dynamic/vehicle.py b/pyro/dynamic/vehicle_steering.py similarity index 98% rename from pyro/dynamic/vehicle.py rename to pyro/dynamic/vehicle_steering.py index 388f592e..3f6b58fc 100644 --- a/pyro/dynamic/vehicle.py +++ b/pyro/dynamic/vehicle_steering.py @@ -1206,29 +1206,29 @@ def xut2q( self, x , u , t ): if __name__ == "__main__": """ MAIN TEST """ - sys = KinematicBicyleModel() + # sys = KinematicBicyleModel() - sys.ubar = np.array([2,-0.5]) - sys.plot_trajectory() - #sys.animate_simulation() + # sys.ubar = np.array([2,-0.5]) + # sys.plot_trajectory() + # sys.animate_simulation() sys = KinematicCarModelwithObstacles() sys.ubar = np.array([2,-0.5]) sys.plot_trajectory() - #sys.animate_simulation() + sys.animate_simulation() - sys = UdeSRacecar() + # sys = UdeSRacecar() - sys.ubar = np.array([2,-0.5]) - sys.plot_trajectory() - sys.animate_simulation() + # sys.ubar = np.array([2,-0.5]) + # sys.plot_trajectory() + # sys.animate_simulation() - sys = HolonomicMobileRobotwithObstacles() + # sys = HolonomicMobileRobotwithObstacles() - sys.ubar = np.array([1,1]) - sys.plot_trajectory() - #sys.animate_simulation() + # sys.ubar = np.array([1,1]) + # sys.plot_trajectory() + # sys.animate_simulation() \ No newline at end of file diff --git a/pyro/kinematic/drawing.py b/pyro/kinematic/drawing.py new file mode 100644 index 00000000..b77e5c18 --- /dev/null +++ b/pyro/kinematic/drawing.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Thu Sep 14 09:12:12 2023 + +@author: alex +""" + +############################################################################### +import numpy as np +import matplotlib.pyplot as plt +############################################################################### +from pyro.kinematic import geometry +############################################################################### + + +############################################################################### +### Transformation tools +############################################################################### + + +############################################################################### +def transform_points_2D( A_T_B , pts_B ): + """ + + Take a list of pts in a given frame B and express them in frame A base on + transformation matrix A_T_B that describe a 2D transform in the x-y plane + + Parameters + ---------- + pts : TYPE + DESCRIPTION. + T : 3 x 3 numpy array + A 2D transformation matrix + T = np.array([ [ c , -s , x ] , + [ s , c , y ] , + [ 0 , 0 , 1 ] ]) + + Returns + ------- + pts_transformed : TYPE + DESCRIPTION. + + """ + + # Init output array of same dimension + pts_A = np.zeros( pts_B.shape ) + + # save z values + z = pts_B[:,2] + + # set last component to 1 for homegeneous transform operations + pts_B[:,2] = 1. + + # Transform all (x,y) pts in the list + for i in range(pts_B.shape[0]): + + pts_A[ i ] = A_T_B @ pts_B[ i ] + + # reset original z values + pts_A[:,2] = z + + return pts_A + + +############################################################################### +### drawing shorcuts tools +############################################################################### + + +############################################################################### +def arrow_from_length_angle( l , theta , x = 0 , y = 0 , w = None , origin = 'base'): + + # width of arrow secondary lines + if w is None: + d = l * 0.15 + else: + d = w + + # Local points + + if origin == 'tip': + + pts_local = np.array([ [ -l , 0 , 0. ] , + [ 0 , 0 , 0. ] , + [ -d , d , 0. ] , + [ 0 , 0 , 0. ] , + [ -d , -d , 0. ] ]) + + # elif origin == 'base': + else: + + pts_local = np.array([ [ 0 , 0 , 0. ] , + [ l , 0 , 0. ] , + [ l-d , d , 0. ] , + [ l , 0 , 0. ] , + [ l-d , -d , 0. ] ]) + + T = geometry.transformation_matrix_2D( theta , x , y ) + + pts_global = transform_points_2D( T , pts_local ) + + + return pts_global + + +############################################################################### +def arrow_from_components( vx , vy , x = 0 , y = 0 , w = None , origin = 'base' ): + + l = np.sqrt( vx**2 + vy**2 ) + theta = np.arctan2( vy , vx ) + + return arrow_from_length_angle( l , theta , x , y , w , origin ) + + diff --git a/pyro/kinematic/geometry.py b/pyro/kinematic/geometry.py new file mode 100644 index 00000000..5425b7f3 --- /dev/null +++ b/pyro/kinematic/geometry.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Thu Sep 14 09:12:12 2023 + +@author: alex +""" + +############################################################################### +import numpy as np +import matplotlib.pyplot as plt +############################################################################### + + +############################################################################### + +# Notation + +# aRb : rotation matrix of basis b in basis a +# ATB : transformation matrix of frame b in frame a +# v_a : vector components in basis a + + +############################################################################### +def transformation_matrix_2D( theta , x , y ): + """ + + Transformation Matrix between 2 frame in 2D + + Parameters + ---------- + theta : float + roation angle (arround z) + x : float + translation along x-axis + y : float + translation along y-axis + + Returns + ------- + T : 3 x 3 np.array + Transformation matrix + + """ + + s = np.sin( theta ) + c = np.cos( theta ) + + T = np.array([ [ c , -s , x ] , + [ s , c , y ] , + [ 0 , 0 , 1 ] ]) + + return T \ No newline at end of file diff --git a/pyro/planning/discretizer_legacy.py b/pyro/planning/discretizer_legacy.py deleted file mode 100644 index 65f73cc7..00000000 --- a/pyro/planning/discretizer_legacy.py +++ /dev/null @@ -1,304 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Wed Jul 12 10:02:12 2017 - -@author: alxgr -""" - -import numpy as np - -''' -################################################################################ -''' - - -class GridDynamicSystem: - """ Create a discrete gird state-action space for a continous dynamic system """ - - ############################ - def __init__(self, sys , xgriddim = ( 101 , 101 ), ugriddim = ( 11 , 1 ) , dt = 0.05 , lookup = True ): - - self.sys = sys # Dynamic system class - - # Discretization Parameters - - self.dt = dt # time discretization - - # Grid size - self.xgriddim = xgriddim - self.ugriddim = ugriddim - - # Options - self.uselookuptable = lookup - - self.compute() - - ############################## - def compute(self): - """ """ - - self.discretizespace() - self.discretizeactions() - - print('\nDiscretization:\n---------------------------------') - print('State space dimensions:', self.sys.n , ' Input space dimension:', self.sys.m ) - print('Number of nodes:', self.nodes_n , ' Number of actions:', self.actions_n ) - print('Number of node-action pairs:', self.nodes_n * self.actions_n ) - - self.generate_nodes() - self.generate_actions() - - if self.uselookuptable: - self.compute_lookuptable() - - - ############################# - def discretizespace(self): - """ Grid the state space """ - - self.xd = [] - self.nodes_n = 1 - - # n-D grid - self.x_grid2node = np.zeros( self.xgriddim , dtype = int ) # grid of corresponding index - - # linespace for each x-axis and total number of nodes - for i in range(self.sys.n): - self.xd.append( np.linspace( self.sys.x_lb[i] , self.sys.x_ub[i] , self.xgriddim[i] ) ) - self.nodes_n = self.nodes_n * self.xgriddim[i] - - # 1-D List of nodes - self.nodes_state = np.zeros(( self.nodes_n , self.sys.n ), dtype = float ) # Number of nodes x state dimensions - self.nodes_index = np.zeros(( self.nodes_n , self.sys.n ), dtype = int ) # Number of nodes x state dimensions - - - ############################# - def discretizeactions(self): - """ Grid the action space """ - - self.ud = [] - self.actions_n = 1 - - # linespace for each u-axis and total number of actions - for i in range(self.sys.m): - self.ud.append( np.linspace( self.sys.u_lb[i] , self.sys.u_ub[i] , self.ugriddim[i] ) ) - self.actions_n = self.actions_n * self.ugriddim[i] - - # 1-D List of actions - self.actions_input = np.zeros(( self.actions_n , self.sys.m ), dtype = float ) # Number of actions x inputs dimensions - self.actions_index = np.zeros(( self.actions_n , self.sys.m ), dtype = int ) # Number of actions x inputs dimensions - - - ############################## - def generate_nodes(self): - """ Compute 1-D list of nodes """ - - # For all state nodes - node = 0 - - if self.sys.n == 2 : - - for i in range(self.xgriddim[0]): - for j in range(self.xgriddim[1]): - - # State - x = np.array([ self.xd[0][i] , self.xd[1][j] ]) - - # State and grid index based on node # - self.nodes_state[node,:] = x - self.nodes_index[node,:] = np.array([i,j]) - - # Node # based on index ij - self.x_grid2node[i,j] = node - - # Increment node number - node = node + 1 - - - elif self.sys.n == 3: - - for i in range(self.xgriddim[0]): - for j in range(self.xgriddim[1]): - for k in range(self.xgriddim[2]): - - # State - x = np.array([ self.xd[0][i] , self.xd[1][j] , self.xd[2][k] ]) - - # State and grid index based on node # - self.nodes_state[node,:] = x - self.nodes_index[node,:] = np.array([i,j,k]) - - # Node # based on index ijk - self.x_grid2node[i,j,k] = node - - # Increment node number - node = node + 1 - - - - elif self.sys.n == 4: - - # NOT yet validated!!! - - for i in range(self.xgriddim[0]): - for j in range(self.xgriddim[1]): - for k in range(self.xgriddim[2]): - for l in range(self.xgriddim[3]): - - # State - x = np.array([ self.xd[0][i] , self.xd[1][j] , self.xd[2][k] , self.xd[3][l]]) - - # State and grid index based on node # - self.nodes_state[node,:] = x - self.nodes_index[node,:] = np.array([i,j,k,l]) - - # Node # based on index ijkl - self.x_grid2node[i,j,k,l] = node - - # Increment node number - node = node + 1 - - else: - - raise NotImplementedError - - - ############################## - def generate_actions(self): - """ Compute 1-D list of actions """ - - # For all state nodes - action = 0 - - # Single input - - if self.sys.m == 1 : - - for k in range(self.ugriddim[0]): - - u = np.array([ self.ud[0][k] ]) - - # State and grid index based on node # - self.actions_input[action,:] = u - self.actions_index[action,:] = k - - # Increment node number - action = action + 1 - - elif self.sys.m == 2 : - - for k in range(self.ugriddim[0]): - for l in range(self.ugriddim[1]): - - u = np.array([ self.ud[0][k] , self.ud[1][l] ]) - - # State and grid index based on node # - self.actions_input[action,:] = u - self.actions_index[action,:] = np.array([k,l]) - - # Increment node number - action = action + 1 - - else: - - raise NotImplementedError - - - ############################## - def compute_lookuptable(self): - """ Compute lookup table for faster evaluation """ - - if self.uselookuptable: - # Evaluation lookup tables - self.action_isok = np.zeros( ( self.nodes_n , self.actions_n ) , dtype = bool ) - self.x_next = np.zeros( ( self.nodes_n , self.actions_n , self.sys.n ) , dtype = float ) # lookup table for dynamic - - # For all state nodes - for node in range( self.nodes_n ): - - x = self.nodes_state[ node , : ] - - # For all control actions - for action in range( self.actions_n ): - - u = self.actions_input[ action , : ] - - # Compute next state for all inputs - x_next = self.sys.f( x , u ) * self.dt + x - - # validity of the options - x_ok = self.sys.isavalidstate(x_next) - u_ok = self.sys.isavalidinput(x,u) - - self.x_next[ node, action, : ] = x_next - self.action_isok[ node, action] = ( u_ok & x_ok ) - - - - ############################## - ### Quick shorcut - ############################## - - ############################## - def x2node(self, x ): - """ """ - raise NotImplementedError - - s = 0 - - return s - - ############################## - def x2index(self, x ): - """ """ - raise NotImplementedError - - i = 0 - j = 0 - - return (i,j) - - ############################## - def node2x(self, x ): - """ """ - raise NotImplementedError - - s = 0 - - return s - - - ############################## - def index2x(self, u ): - """ """ - raise NotImplementedError - - a = 0 - - return a - - ############################## - def u2index(self, u ): - """ """ - raise NotImplementedError - - k = 0 - - return k - - - - - - -''' -################################################################# -################## Main ######## -################################################################# -''' - - -if __name__ == "__main__": - """ MAIN TEST """ - - pass \ No newline at end of file diff --git a/pyro/planning/trajectorygeneration.py b/pyro/planning/trajectorygeneration.py new file mode 100644 index 00000000..d8477f02 --- /dev/null +++ b/pyro/planning/trajectorygeneration.py @@ -0,0 +1,852 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on 22 Feb 2024 + +@author: alex +""" + +import numpy as np +import matplotlib.pyplot as plt +from scipy.optimize import minimize +import warnings + +# Import standard graphical parameters if part of pyro +try: + from pyro.analysis import graphical + + default_figsize = graphical.default_figsize + default_dpi = graphical.default_dpi + default_fontsize = graphical.default_fontsize +except: + default_figsize = (10, 6) + default_dpi = 100 + default_fontsize = 12 + + +############################################################################### +class SingleAxisPolynomialTrajectoryGenerator: + """ + This class is a tool to generate a point-to-point trajectory for a + single axis based on boundary conditions (position and higher order derivative) + + Polynomial of order N + + x(t) = p0 + p1*t + p2*t^2 + ... + pN*t^N + + if boundary conditions do not fully specify the parameters of the polynomial, + then an optimization is conducted to minimize the cost function which is defined + as a weighted sum of the integral of the square of the ith derivative of the profile. + + Parameters: + ----------- + tf : float + duration of the trajectory + poly_N : int + order of the polynomial + diff_N : int + order of the highest derivative to compute + x0 : array + initial conditions (position, velocity, acceleration, jerk, snap, crackle, pop, ...) + xf : array + final conditions (position, velocity, acceleration, jerk, snap, crackle, pop, ...) + x0_N : int + number of initial conditions to impose (higher order derivative of the initial conditions) + xf_N : int + number of final conditions to impose (higher order derivative of the final conditions) + Rs : array + weights for the cost function penalizing the ith polynomial parameters directly + Ws : array + weights for the cost function penalizing the ith derivative of the profile + dt : float + time step for the numerical solution of the trajectory + + Output: + ------- + p : array + polynomial parameters + X : array + profile of the trajectory X[i, j] is the ith derivative of the profile at time t[j] + t : array + time vector + + """ + + ################################################ + def __init__( + self, + tf=10, + poly_N=5, + diff_N=7, + x0=np.array([0.0, 0.0, 0.0]), + xf=np.array([0.0, 0.0, 10.0]), + dt=0.01, + ): + + self.tf = tf + self.poly_N = poly_N + self.diff_N = diff_N + self.x0 = x0 + self.xf = xf + self.x0_N = x0.shape[0] + self.xf_N = xf.shape[0] + self.Rs = np.zeros((self.poly_N + 1)) + self.Ws = np.zeros((self.diff_N)) + self.dt = dt + + # Outputs + self.t = None + self.X = None + self.p = None + + self.labels = [ + "pos", + "vel", + "acc", + "jerk", + "snap", + "crac", + "pop", + "7th", + "8th", + "9th", + "10th", + ] + + ################################################ + def compute_b(self, x0, xf, N0, Nf): + """Compute the boundary condition vector b = [x0;xf] which represents the initial and final conditions on the trajectory and its derivatives""" + + b = np.hstack((x0[:N0], xf[:Nf])) + + print("Boundary condition vector b shape: ", b.shape) + + return b + + ################################################ + def compute_A(self, tf, N0, Nf, poly_N): + """Compute the boundary condition matrix A which represents on the polynomial parameters are related to the boundary conditions""" + + A = np.zeros((N0 + Nf, poly_N + 1)) + + # For all jth derivative of the initial conditions + t0 = 0 + for j in range(N0): + # For all terms of the polynomical + for n in range(j, poly_N + 1): + exp = n - j + mul = 1 + for k in range(j): + mul = mul * (n - k) + A[j, n] = mul * t0**exp + + # For all jth derivative of the final conditions + for j in range(Nf): + # For all terms of the polynomical + for n in range(j, poly_N + 1): + exp = n - j + mul = 1 + for k in range(j): + mul = mul * (n - k) + A[N0 + j, n] = mul * tf**exp + + print("Boundary condition matrix A shape: ", A.shape) + + return A + + ################################################ + def compute_Q(self, poly_N, diff_N, tf, Ws, Rs): + """Compute the cost function matrix Q, only used if the boundary conditions do not fully specify the parameters of the polynomial""" + + # Quadratic cost matrix + Q = np.zeros((poly_N + 1, poly_N + 1)) + + # Quadratic cost matrix for each derivative + Qs = np.zeros((poly_N + 1, poly_N + 1, diff_N)) + + # Qs are weight corresponding to computing the integral of the square of the ith derivative of the profile + # J = p.T @ Qs[i] @ p = integral( [ d_dt(ith)x(t) ]^2 dt) + # see https://groups.csail.mit.edu/rrg/papers/BryIJRR15.pdf + for r in range(diff_N): + for i in range(poly_N + 1): + for l in range(poly_N + 1): + if (i >= r) and (l >= r): + mul = 1 + for m in range(r): + mul = mul * (i - m) * (l - m) + exp = i + l - 2 * r + 1 + Qs[i, l, r] = 2 * mul * tf**exp / (i + l - 2 * r + 1) + else: + Qs[i, l, r] = 0 + + # Total cost for all derivatives + for r in range(diff_N): + Q = Q + Ws[r] * Qs[:, :, r] + + # Regulation term penalizing the polynomial parameters directly + Q = Q + np.diag(Rs[: (poly_N + 1)]) + + return Q + + ################################################ + def solve_for_polynomial_parameters(self, A, b, Q): + """Solve for the polynomial parameters pç + + Parameters: + ----------- + A : array + boundary condition matrix + b : array + boundary condition vector + Q : array + cost function matrix + + Output: + ------- + p : array + polynomial parameters + + """ + + if A.shape[0] == A.shape[1]: + + print("Fully constrained trajectory parameters") + p = np.linalg.solve(A, b) + + elif A.shape[0] > A.shape[1]: + + warnings.warn( + "Warning! : impossible to respect all boundary condition, raise the order of the polynomial" + ) + print( + "Overconstrained boundary consitions: solving for best solution in the least-square sense" + ) + p = np.linalg.lstsq(A, b)[0] + + else: + + print("Optimization over free decision variables") + + try: + + Q_inv = np.linalg.inv(Q) + AQA = A @ Q_inv @ A.T + lam = np.linalg.solve(AQA, b) + p = Q_inv @ A.T @ lam + + except: + + print(" Q is not invertible, using optimization solver") + + p0 = np.zeros(A.shape[1]) + + constraints = {"type": "eq", "fun": lambda p: A @ p - b} + cost = lambda p: p.T @ Q @ p + grad = lambda p: 2 * p.T @ Q + hess = lambda p: 2 * Q + + # TODO: Change to a solver specifc to quadratic optimization + res = minimize( + cost, + p0, + method="SLSQP", + jac=grad, + hess=hess, + constraints=constraints, + options={"disp": True, "maxiter": 5000}, + ) + + p = res.x + + else: + + print(" Q is invertible, using direct solver") + + print("Computed polynomial parameters shape: ", p.shape) + + return p + + ################################################ + def generate_trajectory(self, tf, p, diff_N, dt=0.01): + """Generate a numerical trajectory based on the polynomial parameters""" + + Np1 = p.shape[0] # order of polynomial + steps = int(tf / dt) # number of time steps + ts = np.linspace(0, tf, steps) + X = np.zeros((diff_N, steps)) + + # For all jth derivative of the signal + for j in range(diff_N): + # For all time steps + for i in range(steps): + t = ts[i] + x = 0 + # For all terms of the polynomical + # TODO could replace this with A(t) generic code + for n in range(j, Np1): + p_n = p[n] + exp = n - j + mul = 1 + for k in range(j): + mul = mul * (n - k) + x = x + mul * p_n * t**exp + + X[j, i] = x + + return X, ts + + ################################################ + def get_trajectory(self, j, t, p): + """Get the jth derivative of the trajectory at time t based on the polynomial parameters p""" + + Np1 = p.shape[0] # order of polynomial + x = 0 + + # For all terms of the polynomical + for n in range(j, Np1): + p_n = p[n] + exp = n - j + mul = 1 + for k in range(j): + mul = mul * (n - k) + x = x + mul * p_n * t**exp + + return x + + ################################################ + def plot_trajectory(self, X, t, n_fig=None): + + # Number of derivatives to plot + n_max = X.shape[0] + if n_fig is None: + n = n_max + elif n_fig < n_max: + n = n_fig + else: + n = n_max + + fig, ax = plt.subplots( + n, + figsize=default_figsize, + dpi=default_dpi, + frameon=True, + ) + + if n == 1: + ax = [ax] + + for i in range(n): + + ax[i].plot(t, X[i, :], "b") + ax[i].set_ylabel(self.labels[i], fontsize=default_fontsize) + ax[i].tick_params(labelsize=default_fontsize) + ax[i].grid(True) + + ax[-1].set_xlabel("Time[sec]", fontsize=default_fontsize) + # fig.tight_layout() + fig.canvas.draw() + + plt.show() + + ################################################ + def solve(self, show=True): + + tf = self.tf + x0 = self.x0 + xf = self.xf + N0 = self.x0_N + Nf = self.xf_N + Np = self.poly_N + Nd = self.diff_N + Ws = self.Ws + Rs = self.Rs + dt = self.dt + + b = self.compute_b(x0, xf, N0, Nf) + A = self.compute_A(tf, N0, Nf, Np) + Q = self.compute_Q(Np, Nd, tf, Ws, Rs) + + p = self.solve_for_polynomial_parameters(A, b, Q) + + X, t = self.generate_trajectory(tf, p, Nd, dt) + + if show: + self.plot_trajectory(X, t) + + return p, X, t + + +############################################################################### +class MultiPointSingleAxisPolynomialTrajectoryGenerator( + SingleAxisPolynomialTrajectoryGenerator +): + """ + This class is a tool to generate a point-to-point trajectory for a + single axis based on boundary conditions (position and higher order derivative) + + k Polynomial segments of order N + + pi(t) = p0i + p1i*t + p2i*t^2 + ... + pNi*t^N + + x(T) = pi(t) with i s.t. ti < T < ti+1 and t = T - ti + + if boundary conditions do not fully specify the parameters of the polynomial, + then an optimization is conducted to minimize the cost function which is defined + as a weighted sum of the integral of the square of the ith derivative of the profile. + + Parameters: + ----------- + poly_N : int + order of the polynomials + diff_N : int + order of the highest derivative to compute + tc : array + array of time for initial, intermediate and final points + x0 : array + array of initial conditions + xf : array + array of final conditions + xc : array ( # of constrained derivatives , pts_N) + matrice of intermediate conditions at waypoints + Rs : array + weights for the cost function penalizing the ith polynomial parameters directly + Ws : array + weights for the cost function penalizing the ith derivative of the profile + dt : float + time step for the numerical solution of the trajectory + + Output: + ------- + p : array + polynomial parameters + X : array + profile of the trajectory X[i, j] is the ith derivative of the profile at time t[j] + t : array + time vector + + """ + + ################################################ + def __init__( + self, + poly_N=5, + diff_N=7, + con_N=3, + x0=np.array([0.0, 0.0, 0.0]), + xf=np.array([10.0, 0.0, 0.0]), + tc=np.array([0.0, 2.0, 8.0, 10.0]), + xc=np.array([[3.0, 7.0], [0.0, 0.0]]), + dt=0.01, + ): + self.poly_N = poly_N + self.diff_N = diff_N + self.x0 = x0 + self.xf = xf + self.xc = xc + self.tc = tc + self.x0_N = x0.shape[0] + self.xf_N = xf.shape[0] + self.K = xc.shape[1] # number of waypoints + self.way_N = xc.shape[0] # number of derivative to impose at each waypoint + self.con_N = con_N # number continuity constraints + self.Rs = np.zeros((self.poly_N + 1)) + self.Ws = np.zeros((self.diff_N)) + self.dt = dt + + # Outputs + self.t = None + self.X = None + self.p = None + + self.labels = [ + "pos", + "vel", + "acc", + "jerk", + "snap", + "crac", + "pop", + "7th", + "8th", + "9th", + "10th", + ] + + ################################################ + def compute_b(self, x0, xf, xc, N0, Nf, Nw, Nc): + """ """ + + K = xc.shape[1] # number of waypoint + + b = x0[:N0] # initial conditions + + for k in range(K): + b = np.hstack((b, xc[:Nw, k])) # waypoint conditions + + b = np.hstack((b, xf[:Nf])) + + for k in range(K): + b = np.hstack((b, np.zeros(Nc))) # continuity conditions + + #print("Constraints vector b = \n", b) + + print("Constraints vector b shape = ",b.shape) + + return b + + ################################################ + def A_t(self, t, poly_N, diff_N): + """ + Compute the matrix X(t) = A(t) @ p + where + p = [p0, p1, p2, ..., pN] are the polynomial parameters + X(t) = [x(t), dx(t), d2x(t), ..., dNx(t)] are the derivatives of the trajectory at time t + + """ + + A = np.zeros((diff_N, poly_N + 1)) + + # For all jth derivative of the final conditions + for j in range(diff_N): + # For all terms of the polynomical + for n in range(j, poly_N + 1): + exp = n - j + mul = 1 + for k in range(j): + mul = mul * (n - k) + A[j, n] = mul * t**exp + + return A + + + ################################################ + def compute_A(self, tc, N0, Nf, Nw, Nc, Np): + """""" + Kp1 = tc.shape[0]-1 # number of segments + K = Kp1 - 1 # number of waypoints + N = Np + 1 # number of polynomial parameters per segments + N_params = Kp1 * N + + N_contraints = N0 + Nf + Nw * K + Nc * K + + A = np.zeros((N_contraints, N_params)) + + # Times on segments + dt = np.diff(tc) # time between waypoints + + # Initial conditions + A[0:N0, 0:N] = self.A_t(0, Np, N0) + + # Waypoint conditions + for k in range(K): + A[N0 + Nw * k : N0 + Nw * (k + 1), N * (k + 1) : N * (k + 2)] = self.A_t(0, Np, Nw) + + # Final conditions + A[N0 + Nw * K : N0 + Nw * K + Nf, N * K : N * (K + 1)] = self.A_t(dt[-1], Np, Nf) + + # Continuity conditions + for k in range(K): + A[N0 + Nw * K + Nf + Nc * k : N0 + Nw * K + Nf + Nc * (k + 1), N * k : N * (k + 1)] = self.A_t(dt[k], Np, Nc) + A[N0 + Nw * K + Nf + Nc * k : N0 + Nw * K + Nf + Nc * (k + 1), N * (k + 1) : N * (k + 2)] = -self.A_t(0, Np, Nc) + + #print("Constraints matrix: \n", A) + + print("Constraints matrix A shape = ",A.shape) + + return A + + ################################################ + def compute_Q(self, poly_N, diff_N, tc, Ws, Rs): + + # + Kp1 = tc.shape[0]-1 # number of segments + N = poly_N + 1 # number of polynomial parameters per segments + N_params = Kp1 * N + + # Times on segments + dt = np.diff(tc) # time between waypoints + + # Quadratic global cost matrix + N_params = (poly_N + 1) * Kp1 # number of polynomial parameters + Q = np.zeros((N_params, N_params)) + + # Waypoint conditions + for k in range(Kp1): + Qk = self.compute_segment_Q(poly_N, diff_N, dt[k], Ws, Rs) + Q[N * k : N * (k + 1), N * k : N * (k + 1)] = Qk + + #print("Quadratic cost matrix: \n", Q) + + print("Quadratic cost matrix shape = ",Q.shape) + + return Q + + + ################################################ + def compute_segment_Q(self, poly_N, diff_N, tf, Ws, Rs): + """Compute the cost function matrix Q, only used if the boundary conditions do not fully specify the parameters of the polynomial""" + + # Quadratic cost matrix + Q = np.zeros((poly_N + 1, poly_N + 1)) + + # Quadratic cost matrix for each derivative + Qs = np.zeros((poly_N + 1, poly_N + 1, diff_N)) + + # Qs are weight corresponding to computing the integral of the square of the ith derivative of the profile + # J = p.T @ Qs[i] @ p = integral( [ d_dt(ith)x(t) ]^2 dt) + # see https://groups.csail.mit.edu/rrg/papers/BryIJRR15.pdf + for r in range(diff_N): + for i in range(poly_N + 1): + for l in range(poly_N + 1): + if (i >= r) and (l >= r): + mul = 1 + for m in range(r): + mul = mul * (i - m) * (l - m) + exp = i + l - 2 * r + 1 + Qs[i, l, r] = 2 * mul * tf**exp / (i + l - 2 * r + 1) + else: + Qs[i, l, r] = 0 + + # Total cost for all derivatives + for r in range(diff_N): + Q = Q + Ws[r] * Qs[:, :, r] + + # Regulation term penalizing the polynomial parameters directly + Q = Q + np.diag(Rs[: (poly_N + 1)]) + + return Q + + ################################################ + def generate_trajectory(self, tc, p, poly_N, diff_N, dt=0.01): + """Generate a numerical trajectory based on the polynomial parameters""" + + Np1 = poly_N + 1 # order of polynomial + tf =tc[-1] + steps = int(tf / dt) # number of time steps + ts = np.linspace(0, tf, steps) + X = np.zeros((diff_N, steps)) + + for i in range(steps): + t = ts[i] + k = 0 + t0_segment = 0.0 + while (t > tc[k+1]): + t0_segment = tc[k+1] + k = k + 1 + + t_local = t - t0_segment + p_local = p[Np1 * k : Np1 * (k + 1)] + At = self.A_t(t_local, Np1 - 1, diff_N) + X[:, i] = At @ p_local + + return X, ts + + ################################################ + def get_local_trajectory(self, t_local, p_local, diff_N): + """Get the jth derivative of the trajectory at time t based on the polynomial parameters p""" + + p = p_local + Np = p.shape[0] - 1 # order of polynomial + t = t_local + At = self.A_t(t, Np, diff_N) + + X = At @ p + + return X + + ################################################ + def solve(self, show=True): + + x0 = self.x0 + xf = self.xf + xc = self.xc + tc = self.tc + N0 = self.x0_N + Nf = self.xf_N + Nw = self.way_N + Nc = self.con_N + Np = self.poly_N + Nd = self.diff_N + Ws = self.Ws + Rs = self.Rs + dt = self.dt + + b = self.compute_b(x0, xf, xc, N0, Nf, Nw, Nc) + A = self.compute_A(tc, N0, Nf, Nw, Nc, Np) + Q = self.compute_Q(Np, Nd, tc, Ws, Rs) + + p = self.solve_for_polynomial_parameters(A, b, Q) + + X, t = self.generate_trajectory(tc, p, Np, Nd, dt) + + if show: + self.plot_trajectory(X, t) + + return b, A, p, X, t + + +""" +################################################################# +################## Main ######## +################################################################# +""" + + +if __name__ == "__main__": + """MAIN TEST""" + + x0 = np.array([0, 0, 0, 0, 0, 0, 0, 0]) + xf = np.array([10, 0, 0, 0, 0, 0, 0, 0]) + + ge = SingleAxisPolynomialTrajectoryGenerator( + x0=x0, xf=xf, tf=10, poly_N=12, diff_N=7, dt=0.01 + ) + + ############################# + ### Fully constrained order 3 + ############################# + + # ge.x0_N = 2 + # ge.xf_N = 2 + # ge.poly_N = 3 + # ge.diff_N = 7 + + # ge.solve() + + ############################# + ### Fully constrained order 5 + ############################# + + # ge.x0_N = 3 + # ge.xf_N = 3 + # ge.poly_N = 5 + # ge.diff_N = 7 + + # ge.solve() # order 5 fully constrained + + ########################################### + ### Optimization on polynomial parameters + ########################################### + + ge.poly_N = 12 + ge.x0_N = 3 + ge.xf_N = 3 + ge.Rs = 0.0 * np.ones(ge.poly_N + 1) + # ge.Ws = np.array([0, 0.0, 10.0, 1.0, 1.0, 1.0, 1.0]) + ge.Ws = np.array([0, 1.0, 0.0, 0, 0.0, 0, 0]) + # ge.Ws = np.array([0.01, 10.0, 0.01, 0.01, 0.01, 0.01, 0.01]) + + p, X, t = ge.solve() # order 12 with optimization on polynomial parameters + + ############################# + ### Fully constrained order 7 + ############################# + + # ge = SingleAxisPolynomialTrajectoryGenerator( + # x0=x0, xf=xf, tf=10, poly_N=7, diff_N=7, dt=0.01 + # ) + + # ge.x0_N = 4 + # ge.xf_N = 4 + + # ge.solve() + + ############################# + ### Fully constrained order 9 + ############################# + + ge = SingleAxisPolynomialTrajectoryGenerator( + x0=x0, xf=xf, tf=10, poly_N=9, diff_N=7, dt=0.01 + ) + + ge.x0_N = 5 + ge.xf_N = 5 + + ge.solve() + + ############################# + ### Overconstrained order 3 + ############################# + + # ge = SingleAxisPolynomialTrajectoryGenerator( + # x0=x0, xf=xf, tf=10, poly_N=3, diff_N=7, dt=0.01 + # ) + + # ge.x0_N = 3 + # ge.xf_N = 3 + + # ge.solve() + + ############################# + ### Waypoint simple fully constraint test + ############################# + + # traj = MultiPointSingleAxisPolynomialTrajectoryGenerator( + # poly_N=3, + # diff_N=5, + # con_N=2, + # x0=np.array([0.0, 0.0]), + # xf=np.array([10.0, 0.0]), + # tc=np.array([0.0, 2.0, 8.0, 10.0]), + # xc=np.array([[3.0, 7.0], [1.0, 1.0]]), + # dt=0.01, + # ) + + # b, A, p, X, t = traj.solve() + + ############################# + ### Waypoint test + ############################# + + # traj = MultiPointSingleAxisPolynomialTrajectoryGenerator( + # poly_N=3, + # diff_N=5, + # con_N=3, + # x0=np.array([0.0, 0.0]), + # xf=np.array([10.0, 0.0]), + # tc=np.array([0.0, 2.0, 8.0, 10.0]), + # xc=np.array([[3.0, 7.0]]), + # dt=0.01, + # ) + + # b, A, p, X, t = traj.solve() + + ############################# + ### Waypoint test + ############################# + + # traj = MultiPointSingleAxisPolynomialTrajectoryGenerator( + # poly_N=4, + # diff_N=5, + # con_N=4, + # x0=np.array([0.0, 0.0, 0.0]), + # xf=np.array([10.0, 0.0]), + # tc=np.array([0.0, 2.0, 8.0, 10.0]), + # xc=np.array([[3.0, 7.0]]), + # dt=0.01, + # ) + + # b, A, p, X, t = traj.solve() + + ############################# + ### Waypoint simple fully constraint test + ############################# + + traj = MultiPointSingleAxisPolynomialTrajectoryGenerator( + poly_N=9, + diff_N=7, + con_N=4, + x0=np.array([0.0, 0.0, 0.0]), + xf=np.array([10.0, 0.0, 0.0]), + tc=np.array([0.0, 2.0, 8.0, 10.0]), + xc=np.array([[5.0, 12.0]]), + dt=0.01, + ) + + traj.Ws[0]= 0.01 + traj.Ws[1]= 1.0 + traj.Ws[2]= 1.0 + traj.Ws[3]= 1.0 + traj.Ws[4]= 1.0 + + b, A, p, X, t = traj.solve() diff --git a/pyro/planning/trajectoryoptimisation.py b/pyro/planning/trajectoryoptimisation.py index 06fe86fa..5ae5e88c 100644 --- a/pyro/planning/trajectoryoptimisation.py +++ b/pyro/planning/trajectoryoptimisation.py @@ -7,10 +7,15 @@ """ import numpy as np +import matplotlib.pyplot as plt + +import time + from scipy.optimize import minimize from pyro.analysis import simulation from pyro.planning import plan +from pyro.analysis import graphical ############################################################################### @@ -27,7 +32,7 @@ class DirectCollocationTrajectoryOptimisation( plan.Planner ): """ ############################ - def __init__(self, sys , dt = 0.2 , grid = 20 , cost_function = None ): + def __init__(self, sys , dt = 0.2 , grid = 20 , cost_function = None, dynamic_plot = False ): # Set sys, default cost function x_start and x_goal @@ -45,8 +50,37 @@ def __init__(self, sys , dt = 0.2 , grid = 20 , cost_function = None ): self.compute_bounds() self.dec_init = np.zeros( grid * ( sys.n + sys.m ) ) + + # Check if vectorized operation are available + try: + is_vectorized = self.sys.is_vectorized & self.cost_function.is_vectorized + except: + is_vectorized = False + self.is_vectorized = is_vectorized + + + # Memory variable self.iter_count = 0 + self.start_time = time.time() + + # Optional Convergence Graph + self.dynamic_plot = dynamic_plot + + if dynamic_plot: + + self.init_dynamic_plot() + + + ############################ + def init_dynamic_plot(self): + + # Graphic option + self.dynamic_plot = True + + traj = self.decisionvariables2traj( self.dec_init ) + self.plotter = graphical.TrajectoryPlotter( self.sys ) + self.plotter.plot( traj, 'xu' ) ############################ @@ -55,6 +89,46 @@ def set_initial_trajectory_guest(self, traj): new_traj = traj.re_sample( self.grid ) self.dec_init = self.traj2decisionvariables( new_traj ) + if self.dynamic_plot: + + traj = self.decisionvariables2traj( self.dec_init ) + self.plotter.update_plot( traj, 'xu' ) + plt.pause( 0.001 ) + + ############################ + def set_linear_initial_guest(self, derivatives = False ): + + xs = np.linspace( self.x_start, self.x_goal, self.grid ) + us = np.linspace( self.sys.ubar, self.sys.ubar, self.grid ) + + # For second order mechanical system + if derivatives: + dof = int(self.sys.n/2) + tf = self.grid * self.dt + dx = ( self.x_goal[:dof] - self.x_start[:dof] ) / tf + dxs = np.linspace( dx, dx, self.grid ) + xs[:,dof:] = dxs + + + dec = np.array([]).reshape(0,1) # initialize dec_vars array + + for i in range(self.sys.n): # append states x + arr_to_add = xs[:,i].reshape(self.grid,1) + dec = np.append(dec,arr_to_add,axis=0) + + for i in range(self.sys.m): # append inputs u + arr_to_add = us[:,i].reshape(self.grid,1) + dec = np.append(dec,arr_to_add,axis=0) + + self.dec_init = dec[:,0] + + + if self.dynamic_plot: + + traj = self.decisionvariables2traj( self.dec_init ) + self.plotter.update_plot( traj, 'xu' ) + plt.pause( 0.001 ) + ############################ def decisionvariables2xu(self, dec ): @@ -80,17 +154,20 @@ def decisionvariables2xu(self, dec ): m = self.sys.m # number of inputs grid = self.grid # number of time steps - x = np.zeros( ( n , grid ) ) - u = np.zeros( ( m , grid ) ) + # x = np.zeros( ( n , grid ) ) + # u = np.zeros( ( m , grid ) ) - # Extract states variables - for i in range(self.sys.n): - x[i,:] = dec[ i * grid : (i+1) * grid ] + # # Extract states variables + # for i in range(self.sys.n): + # x[i,:] = dec[ i * grid : (i+1) * grid ] - # Extract input variables - for j in range(self.sys.m): - k = n + j - u[j,:] = dec[ k * grid : (k+1) * grid ] + # # Extract input variables + # for j in range(self.sys.m): + # k = n + j + # u[j,:] = dec[ k * grid : (k+1) * grid ] + + x = dec[: n * grid ].reshape( n , grid ) + u = dec[ n * grid :].reshape( m , grid ) return x,u @@ -127,7 +204,7 @@ def traj2decisionvariables(self, traj): arr_to_add = traj.u[:,i].reshape(self.grid,1) dec = np.append(dec,arr_to_add,axis=0) - return dec + return dec[:,0] ############################ @@ -199,26 +276,39 @@ def cost(self, dec): x,u = self.decisionvariables2xu( dec ) - J = 0 - - for i in range(self.grid -1): - #i - x_i = x[:,i] - u_i = u[:,i] - t_i = i*self.dt - dJi = self.cost_function.g( x_i , u_i, t_i ) + if self.is_vectorized: + + # Vectorized operation version + t = np.linspace(0, ( self.grid - 1 )* self.dt, self.grid) - #i+1 - x_i1 = x[:,i+1] - u_i1 = u[:,i+1] - t_i1 = (i+1)*self.dt - dJi1 = self.cost_function.g( x_i1 , u_i1, t_i1 ) + dJ = self.cost_function.g( x , u , t ) - #trapez - dJ = 0.5 * ( dJi + dJi1 ) + J = np.trapz( dJ , t ) - #integral - J = J + dJ * self.dt + else: + + # Loop version + + J = 0 + + for i in range(self.grid -1): + #i + x_i = x[:,i] + u_i = u[:,i] + t_i = i*self.dt + dJi = self.cost_function.g( x_i , u_i, t_i ) + + #i+1 + x_i1 = x[:,i+1] + u_i1 = u[:,i+1] + t_i1 = (i+1)*self.dt + dJi1 = self.cost_function.g( x_i1 , u_i1, t_i1 ) + + #trapez + dJ = 0.5 * ( dJi + dJi1 ) + + #integral + J = J + dJ * self.dt return J @@ -229,35 +319,50 @@ def dynamic_constraints(self, dec): x , u = self.decisionvariables2xu( dec ) - residues_vec = np.zeros( (self.grid-1) * self.sys.n ) - - for i in range(self.grid-1): + if self.is_vectorized: + + # Vectorized operation version + + t = np.linspace(0, ( self.grid - 1 )* self.dt, self.grid) - #i - x_i = x[:,i] - u_i = u[:,i] - t_i = i*self.dt - dx_i = self.sys.f(x_i,u_i,t_i) # analytical state derivatives + dx = self.sys.f( x ,u , t ) - #i+1 - x_i1 = x[:,i+1] - u_i1 = u[:,i+1] - t_i1 = (i+1)*self.dt - dx_i1 = self.sys.f(x_i1,u_i1,t_i1) # analytical state derivatives + dx_eqs = 0.5 * ( dx[:,0:-1] + dx[:,1:] ) * self.dt - #trapez - delta_x_eqs = 0.5 * self.dt * (dx_i + dx_i1) + dx_num = np.diff( x ) - #num diff - delta_x_num = x[:,i+1] - x[:,i] # numerical delta in trajectory data + residues = dx_num - dx_eqs - diff = delta_x_num - delta_x_eqs + + else: + + # Loop version + + residues = np.zeros( ( self.grid - 1 , self.sys.n )) - for j in range(self.sys.n): - #TODO numpy manip for replacing slow for loop - residues_vec[i + (self.grid-1) * j ] = diff[j] + for i in range(self.grid-1): + + #i + x_i = x[:,i] + u_i = u[:,i] + t_i = i*self.dt + dx_i = self.sys.f(x_i,u_i,t_i) # analytical state derivatives + + #i+1 + x_i1 = x[:,i+1] + u_i1 = u[:,i+1] + t_i1 = (i+1)*self.dt + dx_i1 = self.sys.f(x_i1,u_i1,t_i1) # analytical state derivatives + + #trapez + delta_x_eqs = 0.5 * self.dt * (dx_i + dx_i1) + + #num diff + delta_x_num = x[:,i+1] - x[:,i] # numerical delta in trajectory data + + residues[i,:] = delta_x_num - delta_x_eqs - return residues_vec + return residues.flatten() ############################## @@ -291,16 +396,26 @@ def compute_bounds(self): ############################## - def display_callback(self, a ): + def display_callback(self, x ): self.iter_count = self.iter_count + 1 - print('Optimizing trajectory: Iteration', self.iter_count) + print('Optimizing trajectory: iteration no', self.iter_count , + ' elapsed time = %.2f' % (time.time() - self.start_time ) ) + + if self.dynamic_plot: + + traj = self.decisionvariables2traj( x ) + self.plotter.update_plot( traj, 'xu' ) + plt.pause( 0.001 ) + ############################## def compute_optimal_trajectory(self): + self.start_time = time.time() + self.compute_bounds() dynamic_constraints = {'type': 'eq', 'fun': self.dynamic_constraints } @@ -346,5 +461,10 @@ def compute_solution(self): planner.x_start = np.array([0.1,0]) planner.x_goal = np.array([-3.14,0]) + planner.init_dynamic_plot() + planner.compute_solution() - planner.show_solution() \ No newline at end of file + planner.animate_solution() + + + \ No newline at end of file diff --git a/pyro/planning/valueiteration_legacy.py b/pyro/planning/valueiteration_legacy.py deleted file mode 100644 index 4d94665d..00000000 --- a/pyro/planning/valueiteration_legacy.py +++ /dev/null @@ -1,871 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Wed Jul 12 12:09:37 2017 - -@author: alxgr -""" - -import sys - -import numpy as np -import matplotlib.pyplot as plt -from scipy.interpolate import RectBivariateSpline as interpol2D -from scipy.interpolate import RegularGridInterpolator as rgi - -from pyro.control import controller - - -''' -################################################################################ -''' - - -class ViController(controller.StaticController): - """ - Feedback controller - --------------------------------------- - r : reference signal vector k x 1 - y : sensor signal vector m x 1 - u : control inputs vector p x 1 - t : time 1 x 1 - --------------------------------------- - u = c( y , r , t ) - - """ - ############################ - def __init__(self, k, m, p): - """ """ - - # Dimensions - self.k = k - self.m = m - self.p = p - - super().__init__(self.k, self.m, self.p) - - # Label - self.name = 'Value Iteration Controller' - - - ############################# - def vi_law( self , x ): - """ """ - u = np.zeros(self.m) # State derivative vector - return u - - ############################# - def c( self , y , r , t = 0 ): - """ State feedback (y=x) - no reference - time independent """ - x = y - u = self.vi_law( x ) - - return u - - - -############################################################################## -# Value Iteration x dim = 2, u dim = 1 -############################################################################## - -class ValueIteration_2D: - """ Dynamic programming for 2D continous dynamic system, one continuous input u """ - - ############################ - def __init__(self, grid_sys , cost_function ): - - # Dynamic system - self.grid_sys = grid_sys # Discretized Dynamic system class - self.sys = grid_sys.sys # Base Dynamic system class - - # Controller - self.ctl = ViController( self.sys.n , self.sys.m , self.sys.n) - - # Cost function - self.cf = cost_function - - # Print params - self.fontsize = 10 - - - # Options - self.uselookuptable = True - - - ############################## - def initialize(self): - """ initialize cost-to-go and policy """ - - self.J = np.zeros( self.grid_sys.xgriddim , dtype = float ) - self.action_policy = np.zeros( self.grid_sys.xgriddim , dtype = int ) - - self.Jnew = self.J.copy() - self.Jplot = self.J.copy() - - # Initial evaluation - - # For all state nodes - for node in range( self.grid_sys.nodes_n ): - - x = self.grid_sys.nodes_state[ node , : ] - - i = self.grid_sys.nodes_index[ node , 0 ] - j = self.grid_sys.nodes_index[ node , 1 ] - - # Final Cost - self.J[i,j] = self.cf.h( x ) - - - ############################### - def compute_step(self): - """ One step of value iteration """ - - # Get interpolation of current cost space - J_interpol = interpol2D( self.grid_sys.xd[0] , self.grid_sys.xd[1] , self.J , bbox=[None, None, None, None], kx=1, ky=1,) - - # For all state nodes - for node in range( self.grid_sys.nodes_n ): - - x = self.grid_sys.nodes_state[ node , : ] - - i = self.grid_sys.nodes_index[ node , 0 ] - j = self.grid_sys.nodes_index[ node , 1 ] - - # One steps costs - Q values - Q = np.zeros( self.grid_sys.actions_n ) - - # For all control actions - for action in range( self.grid_sys.actions_n ): - - u = self.grid_sys.actions_input[ action , : ] - - # Compute next state and validity of the action - - if self.uselookuptable: - - x_next = self.grid_sys.x_next[node,action,:] - action_isok = self.grid_sys.action_isok[node,action] - - else: - - x_next = self.sys.f( x , u ) * self.grid_sys.dt + x - x_ok = self.sys.isavalidstate(x_next) - u_ok = self.sys.isavalidinput(x,u) - action_isok = ( u_ok & x_ok ) - - # If the current option is allowable - if action_isok: - - J_next = J_interpol( x_next[0] , x_next[1] ) - - # Cost-to-go of a given action - y = self.sys.h(x, u, 0) - Q[action] = self.cf.g(x, u, y, 0) * self.grid_sys.dt + J_next[0,0] - - else: - # Not allowable states or inputs/states combinations - Q[action] = self.cf.INF - - - self.Jnew[i,j] = Q.min() - self.action_policy[i,j] = Q.argmin() - - # Impossible situation ( unaceptable situation for any control actions ) - if self.Jnew[i,j] > (self.cf.INF-1) : - self.action_policy[i,j] = -1 - - - # Convergence check - delta = self.J - self.Jnew - j_max = self.Jnew.max() - delta_max = delta.max() - delta_min = delta.min() - print('Max:',j_max,'Delta max:',delta_max, 'Delta min:',delta_min) - - self.J = self.Jnew.copy() - - - ################################ - def assign_interpol_controller(self): - """ controller from optimal actions """ - - # Compute grid of u - self.u_policy_grid = [] - - # for all inputs - for k in range(self.sys.m): - self.u_policy_grid.append( np.zeros( self.grid_sys.xgriddim , dtype = float ) ) - - # For all state nodes - for node in range( self.grid_sys.nodes_n ): - - i = self.grid_sys.nodes_index[ node , 0 ] - j = self.grid_sys.nodes_index[ node , 1 ] - - # If no action is good - if ( self.action_policy[i,j] == -1 ): - - # for all inputs - for k in range(self.sys.m): - self.u_policy_grid[k][i,j] = 0 - - else: - # for all inputs - for k in range(self.sys.m): - self.u_policy_grid[k][i,j] = self.grid_sys.actions_input[ self.action_policy[i,j] , k ] - - - # Compute Interpol function - self.x2u_interpol_functions = [] - - # for all inputs - for k in range(self.sys.m): - self.x2u_interpol_functions.append( - interpol2D( self.grid_sys.xd[0] , - self.grid_sys.xd[1] , - self.u_policy_grid[k] , - bbox=[None, None, None, None], - kx=1, ky=1,) ) - - # Asign Controller - self.ctl.vi_law = self.vi_law - - - - ################################ - def vi_law(self, x , t = 0 ): - """ controller from optimal actions """ - - u = np.zeros( self.sys.m ) - - # for all inputs - for k in range(self.sys.m): - u[k] = self.x2u_interpol_functions[k]( x[0] , x[1] ) - - return u - - - - ################################ - def compute_steps(self, l = 50, plot = False): - """ compute number of step """ - - for i in range(l): - print('Step:',i) - self.compute_step() - - - - ################################ - def plot_cost2go(self, maxJ = 1000 ): - """ print graphic """ - - xname = self.sys.state_label[0] + ' ' + self.sys.state_units[0] - yname = self.sys.state_label[1] + ' ' + self.sys.state_units[1] - - self.Jplot = self.J.copy() - - ## Saturation function for cost - for i in range(self.grid_sys.xgriddim[0]): - for j in range(self.grid_sys.xgriddim[1]): - if self.J[i,j] >= maxJ : - self.Jplot[i,j] = maxJ - else: - self.Jplot[i,j] = self.J[i,j] - - self.fig1 = plt.figure(figsize=(4, 4),dpi=300, frameon=True) - self.fig1.canvas.manager.set_window_title('Cost-to-go') - self.ax1 = self.fig1.add_subplot(1,1,1) - - plt.ylabel(yname, fontsize = self.fontsize) - plt.xlabel(xname, fontsize = self.fontsize) - self.im1 = plt.pcolormesh( self.grid_sys.xd[0] , - self.grid_sys.xd[1] , - self.Jplot.T, - shading='gouraud') - - plt.axis([self.sys.x_lb[0], - self.sys.x_ub[0], - self.sys.x_lb[1], - self.sys.x_ub[1]]) - - plt.colorbar() - plt.grid(True) - plt.tight_layout() - - - ################################ - def plot_policy(self, i = 0 ): - """ print graphic """ - - xname = self.sys.state_label[0] + ' ' + self.sys.state_units[0] - yname = self.sys.state_label[1] + ' ' + self.sys.state_units[1] - - policy_plot = self.u_policy_grid[i].copy() - - self.fig1 = plt.figure(figsize=(4, 4),dpi=300, frameon=True) - self.fig1.canvas.manager.set_window_title('Policy for u[%i]'%i) - self.ax1 = self.fig1.add_subplot(1,1,1) - - plt.ylabel(yname, fontsize = self.fontsize ) - plt.xlabel(xname, fontsize = self.fontsize ) - self.im1 = plt.pcolormesh( self.grid_sys.xd[0] , - self.grid_sys.xd[1] , - policy_plot.T, - shading='gouraud') - - plt.axis([self.sys.x_lb[0], - self.sys.x_ub[0], - self.sys.x_lb[1], - self.sys.x_ub[1]]) - - plt.colorbar() - plt.grid(True) - plt.tight_layout() - - - ################################ - def load_data(self, name = 'DP_data'): - """ Save optimal controller policy and cost to go """ - - try: - - self.J = np.load( name + '_J' + '.npy' ) - self.action_policy = np.load( name + '_a' + '.npy' ).astype(int) - - except: - - print('Failed to load DP data ' ) - - - ################################ - def save_data(self, name='DP_data', prefix=''): - """ Save optimal controller policy and cost to go """ - - np.save(prefix + name + '_J', self.J) - np.save(prefix + name + '_a', self.action_policy.astype(int)) - - - - -############################################################################## -# Value Iteration generic algo -############################################################################## - -class ValueIteration_ND: - """ Dynamic programming for continuous dynamic system """ - - ############################ - def __init__(self, grid_sys, cost_function): - - # Dynamic system - self.grid_sys = grid_sys # Discretized Dynamic system class - self.sys = grid_sys.sys # Base Dynamic system class - - # initializes nb of dimensions and continuous inputs u - self.n_dim = self.sys.n - - # Controller - self.ctl = ViController(self.sys.n, self.sys.m, self.sys.n) - - # Cost function - self.cf = cost_function - - # Print params - self.fontsize = 5 - self.figsize = (4, 3) - self.dpi = 300 - self.plot_max_J = 10000 - - # Options - self.uselookuptable = True - - ############################## - def initialize(self): - """ initialize cost-to-go and policy """ - # Initial evaluation - - # J-arrays and action policy arrays - self.J = np.zeros(self.grid_sys.xgriddim, dtype=float) - self.action_policy = np.zeros(self.grid_sys.xgriddim, dtype=int) - - self.Jnew = self.J.copy() - self.Jplot = self.J.copy() - - # For all state nodes - for node in range(self.grid_sys.nodes_n): - x = self.grid_sys.nodes_state[node, :] - - # use tuple to get dynamic list of indices - indices = tuple(self.grid_sys.nodes_index[node, i] for i in range(self.n_dim)) - - # Final cost - self.J[indices] = self.cf.h(x) - - print('J shape:', self.J.shape) - - ############################### - def compute_step(self): - """ One step of value iteration """ - - # Get interpolation of current cost space - if self.n_dim == 2: - J_interpol = interpol2D(self.grid_sys.xd[0], self.grid_sys.xd[1], - self.J, bbox=[None, None, None, None], kx=1, ky=1) - elif self.n_dim == 3: - # call function for random shape - J_interpol = rgi([self.grid_sys.xd[0], self.grid_sys.xd[1], self.grid_sys.xd[2]], self.J) - else: - points = tuple(self.grid_sys.xd[i] for i in range(self.n_dim)) - J_interpol = rgi(points, self.J) - - # For all state nodes - for node in range(self.grid_sys.nodes_n): - - x = self.grid_sys.nodes_state[node, :] - - # use tuple to get dynamic list of indices - indices = tuple(self.grid_sys.nodes_index[node, i] for i in range(self.n_dim)) - - # One steps costs - Q values - Q = np.zeros(self.grid_sys.actions_n) - - # For all control actions - for action in range(self.grid_sys.actions_n): - - u = self.grid_sys.actions_input[action, :] - - # Compute next state and validity of the action - - if self.uselookuptable: - - x_next = self.grid_sys.x_next[node, action, :] - action_isok = self.grid_sys.action_isok[node, action] - - else: - - x_next = self.sys.f(x, u) * self.grid_sys.dt + x - x_ok = self.sys.isavalidstate(x_next) - u_ok = self.sys.isavalidinput(x, u) - action_isok = (u_ok & x_ok) - - # If the current option is allowable - if action_isok: - if self.n_dim == 2: - J_next = J_interpol(x_next[0], x_next[1]) - elif self.n_dim == 3: - J_next = J_interpol([x_next[0], x_next[1], x_next[2]]) - else: - J_next = J_interpol([x_next[0], x_next[1], x_next[2], x_next[3]]) - - # Cost-to-go of a given action - y = self.sys.h(x, u, 0) - if self.n_dim == 2: - Q[action] = ( self.cf.g(x, u, y, 0) * self.grid_sys.dt - + J_next[0, 0] ) - else: - Q[action] = ( self.cf.g(x, u, y, 0) * self.grid_sys.dt - + J_next) - - else: - # Not allowable states or inputs/states combinations - Q[action] = self.cf.INF - - self.Jnew[indices] = Q.min() - self.action_policy[indices] = Q.argmin() - - # Impossible situation ( unaceptable situation for any control actions ) - if self.Jnew[indices] > (self.cf.INF - 1): - self.action_policy[indices] = -1 - - # Convergence check - delta = self.J - self.Jnew - j_max = self.Jnew.max() - delta_max = delta.max() - delta_min = delta.min() - print('Max:', j_max, 'Delta max:', delta_max, 'Delta min:', delta_min) - - self.J = self.Jnew.copy() - - # TODO: Combine deltas? Check if delta_min or max changes - return delta_min - - ################################ - def assign_interpol_controller(self): - """ controller from optimal actions """ - - # Compute grid of u - self.u_policy_grid = [] - - # for all inputs - for k in range(self.sys.m): - self.u_policy_grid.append(np.zeros(self.grid_sys.xgriddim, dtype=float)) - - # For all state nodes - for node in range(self.grid_sys.nodes_n): - - # use tuple to get dynamic list of indices - indices = tuple(self.grid_sys.nodes_index[node, i] for i in range(self.n_dim)) - - # If no action is good - if (self.action_policy[indices] == -1): - - # for all inputs - for k in range(self.sys.m): - self.u_policy_grid[k][indices] = 0 - - else: - # for all inputs - for k in range(self.sys.m): - self.u_policy_grid[k][indices] = self.grid_sys.actions_input[self.action_policy[indices], k] - - # Compute Interpol function - self.interpol_functions = [] - - # for all inputs - for k in range(self.sys.m): - if self.n_dim == 2: - self.interpol_functions.append( - interpol2D(self.grid_sys.xd[0], - self.grid_sys.xd[1], - self.u_policy_grid[k], - bbox=[None, None, None, None], - kx=1, ky=1, )) - elif self.n_dim == 3: - self.interpol_functions.append( - rgi([self.grid_sys.xd[0], self.grid_sys.xd[1], self.grid_sys.xd[2]], self.u_policy_grid[k])) - else: - points = tuple(self.grid_sys.xd[i] for i in range(self.n_dim)) - self.interpol_functions.append( - rgi(points, - self.u_policy_grid[k], - method='linear')) - - # Asign Controller - self.ctl.vi_law = self.vi_law - - ################################ - def vi_law(self, x, t=0): - """ controller from optimal actions """ - - u = np.zeros(self.sys.m) - - x_clipped = x.copy() - - for i in range(self.sys.n): - if x[i] < self.sys.x_lb[i]: - x_clipped[i] = self.sys.x_lb[i] - if x[i] > self.sys.x_ub[i]: - x_clipped[i] = self.sys.x_ub[i] - - # for all inputs - for k in range(self.sys.m): - if self.n_dim == 2: - u[k] = self.interpol_functions[k]( x_clipped[0], x_clipped[1]) - else: - u[k] = self.interpol_functions[k]( x_clipped ) - - return u - - ################################ - def compute_steps(self, l=50, plot=False, threshold=1.0e-25, maxJ=1000): - """ compute number of step """ - step = 0 - print('Step:', step) - cur_threshold = self.compute_step() - print('Current threshold', cur_threshold) - if plot: - self.plot_dynamic_cost2go() - # while abs(cur_threshold) > threshold: - while step < l: - step = step + 1 - print('Step:', step) - cur_threshold = self.compute_step() - print('Current threshold', cur_threshold) - if plot: - self.draw_cost2go( step, maxJ ) - - ################################ - def plot_dynamic_cost2go(self): - """ print graphic """ - - maxJ = self.plot_max_J - - #plt.ion() - - xname = self.sys.state_label[0] + ' ' + self.sys.state_units[0] - yname = self.sys.state_label[1] + ' ' + self.sys.state_units[1] - - self.fig_dynamic = plt.figure(figsize= self.figsize, dpi=self.dpi, frameon=True) - self.fig_dynamic.canvas.manager.set_window_title('Dynamic Cost-to-go') - self.ax1_dynamic = self.fig_dynamic.add_subplot(1, 1, 1) - - plt.ylabel(yname, fontsize=self.fontsize) - plt.xlabel(xname, fontsize=self.fontsize) - - plt.axis([self.sys.x_lb[0], - self.sys.x_ub[0], - self.sys.x_lb[1], - self.sys.x_ub[1]]) - - self.Jplot = self.J.copy() - self.create_Jplot() - - - #TODO update next line for deciding axis to plot - #plot = self.Jplot.T if self.n_dim == 2 else self.Jplot[..., 0].T - - if self.n_dim == 2: - plot = self.Jplot.T - - elif self.n_dim == 3: - plot = self.Jplot[..., 0].T - - elif self.n_dim == 4: - plot = self.Jplot[...,0,0].T - - else: - raise NotImplementedError() - - - self.im1_dynamic = plt.pcolormesh(self.grid_sys.xd[0], - self.grid_sys.xd[1], - plot, - shading='gouraud') - - self.step_text_template = 'Number of steps = %i' - self.time_text = self.ax1_dynamic.text( - 0.05, 0.05, '', transform=self.ax1_dynamic.transAxes, - fontsize=self.fontsize ) - - self.ax1_dynamic.tick_params( labelsize = self.fontsize ) - - plt.colorbar() - plt.grid(True) - plt.tight_layout() - - #plt.draw() - plt.pause(0.001) - - plt.ion() - - - ############################# - def draw_cost2go(self, step, maxJ=1000): - self.Jplot = self.J.copy() - self.create_Jplot() - - if self.n_dim == 2: - plot = self.Jplot.T - - elif self.n_dim == 3: - plot = self.Jplot[..., 0].T - - elif self.n_dim == 4: - plot = self.Jplot[...,0,0].T - - else: - raise NotImplementedError() - - self.im1_dynamic.set_array(np.ravel(plot)) - self.time_text.set_text(self.step_text_template % ( step )) - #plt.draw() - plt.pause(0.001) - - - ################################ - def create_Jplot(self): - - maxJ = self.plot_max_J - - ## Saturation function for cost - if self.n_dim == 2: - for i in range(self.grid_sys.xgriddim[0]): - for j in range(self.grid_sys.xgriddim[1]): - self.Jplot[i, j] = maxJ if self.J[i, j] >= maxJ else self.J[i, j] - - elif self.n_dim == 3: - for i in range(self.grid_sys.xgriddim[0]): - for j in range(self.grid_sys.xgriddim[1]): - for k in range(len(self.J[i, j])): - self.Jplot[i, j, k] = maxJ if self.J[i, j, k] >= maxJ else self.J[i, j, k] - - elif self.n_dim == 4: - for i in range(self.grid_sys.xgriddim[0]): - for j in range(self.grid_sys.xgriddim[1]): - for k in range(self.grid_sys.xgriddim[2]): - for l in range(self.grid_sys.xgriddim[3]): - self.Jplot[i, j, k, l] = maxJ if self.J[i, j, k, l] >= maxJ else self.J[i, j, k, l] - - - ################################ - def plot_cost2go(self, maxJ=1000): - """ print graphic """ - - self.plot_max_J = maxJ - - self.plot_dynamic_cost2go() - #self.draw_cost2go() - - - ################################ - def plot_policy(self, i=0): - """ print graphic """ - - plt.ion() - - xname = self.sys.state_label[0] + ' ' + self.sys.state_units[0] - yname = self.sys.state_label[1] + ' ' + self.sys.state_units[1] - - policy_plot = self.u_policy_grid[i].copy() - - self.fig1 = plt.figure(figsize=(4, 4), dpi=300, frameon=True) - self.fig1.canvas.manager.set_window_title('Policy for u[%i]' % i) - self.ax1 = self.fig1.add_subplot(1, 1, 1) - - #plot = policy_plot.T if self.n_dim == 2 else policy_plot[..., 0].T - - if self.n_dim == 2: - plot = policy_plot.T - - elif self.n_dim == 3: - plot = policy_plot[..., 0].T - - elif self.n_dim == 4: - plot = policy_plot[...,0,0].T - - else: - raise NotImplementedError() - - plt.ylabel(yname, fontsize=self.fontsize) - plt.xlabel(xname, fontsize=self.fontsize) - self.im1 = plt.pcolormesh(self.grid_sys.xd[0], - self.grid_sys.xd[1], - plot, - shading='gouraud') - - plt.axis([self.sys.x_lb[0], - self.sys.x_ub[0], - self.sys.x_lb[1], - self.sys.x_ub[1]]) - - plt.colorbar() - plt.grid(True) - plt.tight_layout() - - plt.draw() - plt.pause(0.001) - - - ################################ - def plot_3D_policy(self, i=0): - """ print graphic """ - - plt.ion() - - xname = self.sys.state_label[0] + ' ' + self.sys.state_units[0] - yname = self.sys.state_label[1] + ' ' + self.sys.state_units[1] - - policy_plot = self.u_policy_grid[i].copy() - - self.fig1 = plt.figure() - self.fig1.canvas.manager.set_window_title('Policy for u[%i]' % i) - self.ax1 = self.fig1.gca(projection='3d') - - if self.n_dim == 2: - plot = policy_plot.T - - elif self.n_dim == 3: - plot = policy_plot[..., 0].T - - elif self.n_dim == 4: - plot = policy_plot[...,0,0].T - - else: - raise NotImplementedError() - - plt.ylabel(yname, fontsize=self.fontsize) - plt.xlabel(xname, fontsize=self.fontsize) - x = self.grid_sys.xd[0] - y = self.grid_sys.xd[1] - X, Y = np.meshgrid(x, y) - Z = plot - self.ax1.plot_surface(X, Y, Z) - - plt.axis([self.sys.x_lb[0], - self.sys.x_ub[0], - self.sys.x_lb[1], - self.sys.x_ub[1]]) - - # plt.colorbar() - - plt.draw() - plt.pause(1) - - ################################ - def plot_3D_cost(self): - """ print graphic """ - - xname = self.sys.state_label[0] + ' ' + self.sys.state_units[0] - yname = self.sys.state_label[1] + ' ' + self.sys.state_units[1] - - cost_plot = self.J.copy() - - self.fig1 = plt.figure() - self.fig1.canvas.manager.set_window_title('Cost-to-go') - self.ax1 = self.fig1.gca(projection='3d') - - if self.n_dim == 2: - plot = cost_plot.T - - elif self.n_dim == 3: - plot = cost_plot[..., 0].T - - elif self.n_dim == 4: - plot = cost_plot[...,0,0].T - - else: - raise NotImplementedError() - - plt.ylabel(yname, fontsize=self.fontsize) - plt.xlabel(xname, fontsize=self.fontsize) - x = self.grid_sys.xd[0] - y = self.grid_sys.xd[1] - X, Y = np.meshgrid(x, y) - Z = plot - self.ax1.plot_surface(X, Y, Z) - - plt.axis([self.sys.x_lb[0], - self.sys.x_ub[0], - self.sys.x_lb[1], - self.sys.x_ub[1]]) - - plt.show() - - - - ################################ - def load_data(self, name='DP_data', prefix=''): - """ Save optimal controller policy and cost to go """ - - try: - self.J = np.load(prefix + name + '_J' + '.npy') - self.action_policy = np.load(prefix + name + '_a' + '.npy').astype(int) - print('Data loaded from file:',name) - - except IOError: - type, value, traceback = sys.exc_info() - print('Error opening %s: %s' % (value.filename, value.strerror)) - - # returns filled array to signal that the trajectory has been loaded - # used in Slash library - return [1] - - ################################ - def save_data(self, name='DP_data', prefix=''): - """ Save optimal controller policy and cost to go """ - - print('Final J', self.J) - print('Final policy', self.action_policy) - - np.save(prefix + name + '_J', self.J) - np.save(prefix + name + '_a', self.action_policy.astype(int)) diff --git a/pyro/tools/sys2gym.py b/pyro/tools/sys2gym.py new file mode 100644 index 00000000..a785927a --- /dev/null +++ b/pyro/tools/sys2gym.py @@ -0,0 +1,281 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Fri Mar 1 14:08:03 2024 + +@author: alex +""" + +import numpy as np +import matplotlib.pyplot as plt + +import gymnasium as gym +from gymnasium import spaces + + +################################################################# +# Create a Gym Environment from a Pyro System +################################################################# +class Sys2Gym(gym.Env): + """Create a Gym Environment from a Pyro System + + Taken from the Pyro system: + - x0: nominal initial state + - f: state evolution function xdot = f(x,u,t) + - g: cost function g(x,u,t) (reward = -g) + - h: observation function y = h(x,u,t) + - x_ub, x_lb: state boundaries + - u_ub, u_lb: control boundaries + + Additionnal parameters of the gym wrapper are: + - dt: time step for the integration + - tf: maximum duration of an episode + - t0: initial time (only relevant if the system is time dependent) + - render_mode: None or "human" for rendering the system + - reset_mode: "uniform", "gaussian" or "determinist" + - clipping_inputs: True if the system clips the control inputs + - clipping_states: True if the system clips the states + - x0_lb, x0_ub: boundaries for the initial state (only relevant if reset_mode is "uniform") + - x0_std: standard deviation for the initial state (only relevant if reset_mode is "gaussian") + - termination_is_reached: method to define the termination condition of the task (default is always False representing a continous time task) + + """ + + metadata = {"render_modes": ["human"]} + + ################################################################# + def __init__( + self, sys, dt=0.05, tf=10.0, t0=0.0, render_mode=None, reset_mode="uniform" + ): + + # Boundaries + self.t0 = t0 + self.tf = tf # For truncation of episodes + self.observation_space = spaces.Box( + sys.x_lb, sys.x_ub + ) # default is state feedback + self.action_space = spaces.Box(sys.u_lb, sys.u_ub) + + # Dynamic evolution + self.sys = sys + self.dt = dt + self.clipping_inputs = True # The sys is assumed to clip out of bounds inputs + self.clipping_states = False # The sys is assumed to clip out of bounds states (some gym env do that but this is a very fake behavior not exibiited by real systems, to use with caution) + + # Rendering + self.render_mode = render_mode + + # Reset parameters (stocasticity of the initial state) + self.reset_mode = reset_mode + # self.reset_mode = "uniform" + self.x0_lb = sys.x0 + 0.1 * sys.x_lb + self.x0_ub = sys.x0 + 0.1 * sys.x_ub + # self.reset_mode = "gaussian" + self.x0_std = 0.1 * (sys.x_ub - sys.x_lb) + # self.reset_mode = "determinist" + + # Memory + self.x = sys.x0 + self.u = sys.ubar + self.t = t0 + + # Init + self.render_is_initiated = False + + if self.render_mode == "human": + self.init_render() + + ################################################################# + def reset(self, seed=None, options=None): + + if self.reset_mode == "uniform": + + super().reset(seed=seed) + + self.x = self.np_random.uniform(self.x0_lb, self.x0_ub) + self.u = self.sys.ubar + self.t = self.t0 + + elif self.reset_mode == "gaussian": + + super().reset(seed=seed) + + self.x = self.np_random.normal(self.sys.x0, self.x0_std) + self.u = self.sys.ubar + self.t = 0.0 + + else: + # Deterministic + self.x = self.sys.x0 + self.u = self.sys.ubar + self.t = 0.0 + + # Observation + y = self.sys.h(self.x, self.u, self.t) + + # Info + info = {"state": self.x, "action": self.u} + + return y, info + + ################################################################# + def step(self, u): + + # Clipping of inputs + if self.clipping_inputs: + u = np.clip(u, self.sys.u_lb, self.sys.u_ub) + + # State and time at the beginning of the step + x = self.x + t = self.t + dt = self.dt + + # Derivatives + dx = self.sys.f(x, u, t) + + # Euler integration #TODO use a better integrator + x_new = x + dx * dt + t_new = t + dt + + # Horrible optionnal hack to avoid the system to go out of bounds + if self.clipping_states: + x_new = np.clip(x_new, self.sys.x_lb, self.sys.x_ub) + + # Termination of episodes + terminated = self.termination_is_reached() + + # Reward = negative of cost function + if terminated: + r = -self.sys.cost_funtion.h(x, t) # Terminal cost + else: + r = ( + -self.sys.cost_function.g(x, u, t) * dt + ) # Instantaneous cost integrated over the time step + + # Truncation of episodes if out of space-time bounds + truncated = (t_new > self.tf) or (not self.sys.isavalidstate(x_new)) + + # Memory update + self.x = x_new + self.t = t_new + self.u = u # The memory of the control input is only used for redering + + # Observation + y = self.sys.h(self.x, self.u, self.t) + + # Info + info = {"state": self.x, "action": self.u} + + # Rendering + if self.render_mode == "human": + self.render() + + return y, r, terminated, truncated, info + + ################################################################# + def init_render(self): + + self.render_is_initiated = True + + self.animator = self.sys.get_animator() + self.animator.show_plus(self.x, self.u, self.t) + plt.pause(0.001) + + ################################################################# + def render(self): + + if self.render_mode == "human": + if not self.render_is_initiated: + self.init_render() + self.animator.show_plus_update(self.x, self.u, self.t) + plt.pause(0.001) + + ################################################################# + def termination_is_reached(self): + """This method should be overloaded in the child class to define the termination condition for a task that is not time defined in continous time.""" + + return False + + +""" +################################################################# +################## Main ######## +################################################################# +""" + + +if __name__ == "__main__": + """MAIN TEST""" + + from pyro.dynamic import pendulum + + from stable_baselines3 import PPO + + sys = pendulum.InvertedPendulum() + + # Physical parameters + sys.gravity = 10.0 + sys.m1 = 1.0 + sys.l1 = 1.0 + sys.lc1 = 0.5 * sys.l1 + sys.I1 = (1.0 / 12.0) * sys.m1 * sys.l1**2 + + sys.l_domain = 2 * sys.l1 # graphical domain + + # Min/max state and control inputs + sys.x_ub = np.array([+3.0 * np.pi, +8]) + sys.x_lb = np.array([-3.0 * np.pi, -8]) + sys.u_ub = np.array([+2.0]) + sys.u_lb = np.array([-2.0]) + + # Time constant + dt = 0.05 + + # Cost Function + # The reward function is defined as: r = -(theta2 + 0.1 * theta_dt2 + 0.001 * torque2) + sys.cost_function.xbar = np.array([0, 0]) # target + sys.cost_function.R[0, 0] = 0.001 / dt + sys.cost_function.Q[0, 0] = 1.0 / dt + sys.cost_function.Q[1, 1] = 0.1 / dt + + sys.x0 = np.array([-np.pi, 0.0]) + + gym_env = Sys2Gym(sys, dt=dt, render_mode=None) + + gym_env.clipping_states = True # To reproduce the behavior of gym pendulum + + gym_env.reset_mode = "uniform" + gym_env.x0_lb = np.array([-np.pi , -1.0]) + gym_env.x0_ub = np.array([+np.pi , +1.0]) + + model = PPO("MlpPolicy", gym_env, verbose=1) + model.learn(total_timesteps=250000) + + gym_env = Sys2Gym(sys, render_mode="human") + + #gym_env.reset_mode = "uniform" + + episodes = 3 + for episode in range(episodes): + y, info = gym_env.reset() + terminated = False + truncated = False + + print("\n Episode:", episode) + while not (terminated or truncated): + u, _states = model.predict(y, deterministic=True) + y, r, terminated, truncated, info = gym_env.step(u) + + + # from pyro.control.reinforcementlearning import stable_baseline3_controller + + # ppo_ctl = stable_baseline3_controller(model) + + + # ppo_ctl.plot_control_law(sys=sys, n=100) + # cl_sys = ppo_ctl + sys + + # cl_sys.x0 = np.array([-3.0, -0.0]) + # cl_sys.compute_trajectory(tf=10.0, n=10000, solver="euler") + # cl_sys.plot_trajectory("xu") + # cl_sys.animate_simulation()