Final upload

Author: oselin

DRS_PodPul.msim

@@ -44,57 +44,57 @@ draw_mech := proc(sol::{list(`=`),set(`=`)}, data::{list(`=`),set(`=`)}, dof)
 
 end proc:
 # Plot position function
-plot_position := proc()
+plot_position := proc(dd::{list(`=`),set(`=`)})
 
           local A,B:
 
 
-          A := plot(subs(sol_kine, data, theta3(t) = q1_profile, theta1(t) * 180 /Pi), T = 0..3*actuation_time, color = c_set[1],
+          A := plot(subs(sol_kine, dd, theta3(t) = q1_profile, theta1(t) * 180 /Pi), T = 0..3*actuation_time, color = c_set[1],
             title = typeset("Position of ", theta__1(t)),labels=["time [s]","Angle [�]"],size = [800, 500]):
 
-          B := plot(subs(sol_kine, data, theta3(t) = q1_profile, s(t)), T = 0..3*actuation_time, color = c_set[2],
+          B := plot(subs(sol_kine, dd, theta3(t) = q1_profile, s(t)), T = 0..3*actuation_time, color = c_set[2],
             title = typeset("Position of", s(t)),labels=[t (s),typeset([m])],size = [800, 500]):
 
           print(plots[display](Array([A, B]))):
 
 end proc:
 # Plot velocity function
-plot_velocity := proc()
+plot_velocity := proc(dd::{list(`=`),set(`=`)})
 
           local A,B:
 
-          A := plot(subs(sol_kine, data, theta_base, vel_theta1), T = 0..3*actuation_time, color = c_set[1],
+          A := plot(subs(sol_kine, dd, theta_base, vel_theta1), T = 0..3*actuation_time, color = c_set[1],
             title = typeset("Velocity of ", theta__1(t)),labels=["time [s]", "velocity [rad/s]"],size = [800, 500]):
 
-          B := plot(subs(sol_kine, data, theta_base, vel_s), T = 0..3*actuation_time, color = c_set[2],
+          B := plot(subs(sol_kine, dd, theta_base, vel_s), T = 0..3*actuation_time, color = c_set[2],
             title = typeset("Velocity of ", s(t)),labels=["time [s]", "velocity [m/s]"],size = [800, 500]):
 
              print(plots[display](Array([A, B]))):
 end proc:
 # Plot acceleration function
-plot_acceleration := proc()
+plot_acceleration := proc(dd::{list(`=`),set(`=`)})
 
           local A,B:
 
 
-          A := plot(subs(sol_kine, data, theta_base, acc_theta1), T = 0..3*actuation_time, color = c_set[1], 
+          A := plot(subs(sol_kine, dd, theta_base, acc_theta1), T = 0..3*actuation_time, color = c_set[1], 
             title = typeset("Acceleration of ", theta__1(t)),labels = ["time [s]", " acceleration [rad/s^2]"],size = [800, 500]):
 
-          B := plot(subs(sol_kine, data, theta_base, acc_s), T = 0..3*actuation_time, color = c_set[2], 
+          B := plot(subs(sol_kine, dd, theta_base, acc_s), T = 0..3*actuation_time, color = c_set[2], 
             title = typeset("Acceleration of ", s(t)),labels = ["time [s]", "acceleration [m/s^2]"],size = [800, 500]):
 
           print(plots[display](Array([A, B]))):
 
 end proc:
 # Plot system configurations
-plot_system := proc()
+plot_system := proc(dd::{list(`=`),set(`=`)})
 
           local A,B:
 
 
-          A := draw_mech(sol_kine, data, theta3(t)=flap_closed, title = "Configuration Recursive", size = [500,500]):
+          A := draw_mech(sol_kine, dd, theta3(t)=flap_closed, title = "Configuration Recursive", size = [500,500]):
 
-          B := draw_mech(sol_kine, data, theta3(t)=flap_open  , title = "Configuration Recursive", size = [500,500]):
+          B := draw_mech(sol_kine, dd, theta3(t)=flap_open  , title = "Configuration Recursive", size = [500,500]):
 
           print(plots[display](Array([A, B])));
 end proc:
@@ -160,9 +160,14 @@ plot_hydr := proc()
 end proc:
 # Dynamic optimization - NE
 dynam_opt_ne := proc(aaa,bbb,ccc,ddd,eee)
-   local param,E,G,NE,NG,r_vars0,dof0,vel_dof0,qD0,vel_qD0,ics0,sol_act_dinam,dae_ics,dae_sys,sol_dae,cost_fun,i,k:
+   local param,E,G,NE,NG,r_vars0,dof0,vel_dof0,qD0,vel_qD0,ics0,sol_act_dinam,dae_ics,subs_inertia,dae_sys,sol_dae,cost_fun,i,k:
 
    param := [L1 = aaa,LP = bbb,anc = ccc,x1 = ddd,y1 = eee]:
+   subs_inertia := {m1  = subs(data_dae,sp=0.010,l=0.004,4510*L1*sp*l),
+                    Iz1 = subs(data_dae,l=0.004,sp=0.010,1/12*4510*L1*sp*l*(L1^2+sp^2)),
+                    m2  = subs(data_dae,sp=0.010,l=0.004,4510*LP*sp*l),
+                    Iz2 = subs(data_dae,l=0.004,sp=0.010,1/12*4510*LP*sp*l*(LP^2+sp^2))
+   }:
 
    #Estimate the initial conditions
    dof0 := theta3(t) = flap_closed:
@@ -172,23 +177,23 @@ dynam_opt_ne := proc(aaa,bbb,ccc,ddd,eee)
    ics0 := [dof0, vel_dof0, qD0, vel_qD0]:
 
    #Estimate the actuation force
-   sol_act_dinam := subs(forcesub,gamma(t)=q1_profile_t,sol_kine,theta3(t)=q1_profile_t,data_dae,param,sol_act):
+   sol_act_dinam := subs(forcesub,gamma(t)=q1_profile_t,sol_kine,theta3(t)=q1_profile_t,data_dae,subs_inertia,param,sol_act):
 
    #Estimate the reaction forces
    E, G := GenerateMatrix(subs(forcesub,gamma(t)=theta3(t),eqns), diff(q_vars,t,t) union r_vars):
-   NE := evalf(subs(subs(forcesub,gamma(t)=theta3(t),sol_act), ics0, data_dae,param, E)):
-   NG := evalf(subs(subs(forcesub,gamma(t)=theta3(t),sol_act), ics0, data_dae,param, G)):
+   NE := evalf(subs(subs(forcesub,gamma(t)=theta3(t),sol_act), ics0,subs_inertia, data_dae,param, E)):
+   NG := evalf(subs(subs(forcesub,gamma(t)=theta3(t),sol_act), ics0,subs_inertia, data_dae,param, G)):
    LinearSolve(NE, NG):
-   r_vars0 := [seq(r_vars[i] = %[-i],i=1..nops(r_vars))]:
+   r_vars0 := [seq(r_vars[-i] = %[-i],i=1..nops(r_vars))]:
 
    #Assembling the generic DAE
-   dae_ics := subs(t=0,data_dae,convert(convert(ics0,set)  minus {diff(theta1(t),t)=0} ,D) union r_vars0 ):
-   dae_sys := convert(subs(sol_act_dinam,forcesub,gamma(t)=q1_profile_t,data_dae, ne_eqns) union dae_ics,set):
+   dae_ics := subs(t=0,subs_inertia,data_dae,convert(convert(ics0,set)  minus {diff(theta1(t),t)=0} ,D) union r_vars0 ):
+   dae_sys := convert(subs(sol_act_dinam,forcesub,gamma(t)=q1_profile_t,subs_inertia,data_dae, ne_eqns) union dae_ics,set):
    sol_dae := dsolve(dae_sys,numeric,stiff=true,parameters = [L1,LP,anc,x1,y1]):
    sol_dae(parameters = subs(param,[L1,LP,anc,x1,y1])):
 
    #Cost function
-   cost_fun :=1/W*add(subs(data_dae,param,(delta__s[k]-rhs(sol_dae(k/np*actuation_time)[10]))^2),k=1..np);
+   cost_fun :=1/W*add(subs(subs_inertia,data_dae,param,(delta__s[k]-rhs(sol_dae(k/np*actuation_time)[10]))^2),k=1..np);
 end proc:
 # Dynamic optimization - Lagrange
 dynam_opt_lagr := proc(aaa,bbb,ccc,ddd,eee,fff)
@@ -218,7 +223,7 @@ dynam_opt_lagr := proc(aaa,bbb,ccc,ddd,eee,fff)
    NE := evalf(subs(subs(forcesub,gamma(t)=theta3(t),sol_act), ics0,subs_inertia, data_dae, E)):
    NG := evalf(subs(subs(forcesub,gamma(t)=theta3(t),sol_act), ics0,subs_inertia, data_dae, G)):
    LinearSolve(NE, NG):
-   l_vars0 := [seq(l_vars[i] = %[-i],i=1..nops(l_vars))]:
+   l_vars0 := [seq(l_vars[-i] = %[-i],i=1..nops(l_vars))]:
 
    #Assembling the generic DAE
    dae_ics := subs(t=0,subs_inertia,data_dae,convert(convert(ics0,set),D) union l_vars0 ):