Stub of tutorial

impact_tutorial/design0.m

@@ -0,0 +1,63 @@
+% Open Matlab from the conda shell
+% Make sure to add casadi-matlab to the path
+import casadi.*
+
+addpath(char(py.rockit.matlab_path))
+addpath(char(py.impact.matlab_path))
+
+rockit.GlobalOptions.set_cmake_flags({'-G','Ninja','-DCMAKE_C_COMPILER=clang','-DCMAKE_CXX_COMPILER=clang'})
+rockit.GlobalOptions.set_cmake_build_type('Debug')
+
+mpc = impact.MPC('T',0.5);
+
+% Add furuta model
+furuta = mpc.add_model('fu_pendulum','furuta.yaml');
+
+% Parameters
+x_current = mpc.parameter('x_current',furuta.nx);
+x_final = mpc.parameter('x_final',furuta.nx);
+
+% Objectives
+mpc.add_objective(mpc.sum(furuta.Torque1^2 ));
+
+% Initial and final state constraints
+mpc.subject_to(mpc.at_t0(furuta.x)==x_current);
+mpc.subject_to(mpc.at_tf(furuta.x)==x_final);
+
+% Path constraints
+mpc.subject_to(-40 <= furuta.dtheta1 <= 40 , 'include_first',false, 'include_last', false)
+mpc.subject_to(-40 <= furuta.dtheta2 <= 40 , 'include_first',false, 'include_last', false)
+
+mpc.subject_to(-pi <= furuta.theta1 <= pi, 'include_first',false)
+
+ee = [furuta.ee_x; furuta.ee_y; furuta.ee_z];
+pivot = [furuta.pivot_x; furuta.pivot_y; furuta.pivot_z];
+
+options = struct();
+options.expand = true;
+options.structure_detection = 'auto';
+options.fatrop.tol = 1e-4;
+options.fatrop.print_level = 0;
+options.debug = true;
+options.print_time = false;
+options.common_options.final_options.cse = true;
+
+mpc.solver('fatrop', options);
+
+mpc.set_value(x_current, [-pi/3,0,0,0]);
+mpc.set_value(x_final, [pi/3,0,0,0]);
+
+% Transcription choice
+mpc.method(rockit.MultipleShooting('N',25,'intg','heun'));
+
+% Solve
+sol = mpc.solve_limited();
+
+
+
+[ts, theta2sol] = sol.sample(furuta.theta2, 'grid','control');
+[ts_fine, theta2sol_fine] = sol.sample(furuta.theta2, 'grid','integrator','refine',10);
+
+theta2sol
+
+

impact_tutorial/design0.py

@@ -0,0 +1,102 @@
+from impact import *
+import casadi as ca
+import numpy as np
+import rockit
+import impact
+
+rockit.GlobalOptions.set_cmake_flags(['-G','Ninja','-DCMAKE_C_COMPILER=clang','-DCMAKE_CXX_COMPILER=clang'])
+rockit.GlobalOptions.set_cmake_build_type('Debug')
+
+mpc = impact.MPC(T=0.5)
+
+# Add furuta model
+furuta = mpc.add_model('fu_pendulum','furuta.yaml')
+
+# Parameters
+x_current = mpc.parameter('x_current',furuta.nx)
+x_final = mpc.parameter('x_final',furuta.nx)
+
+# Objectives
+mpc.add_objective(mpc.sum(furuta.Torque1**2 ))
+
+# Initial and final state constraints
+mpc.subject_to(mpc.at_t0(furuta.x)==x_current)
+mpc.subject_to(mpc.at_tf(furuta.x)==x_final)
+
+# Path constraints
+mpc.subject_to(-40 <= (furuta.dtheta1 <= 40 ), include_first=False, include_last=False)
+mpc.subject_to(-40 <= (furuta.dtheta2 <= 40 ), include_first=False, include_last=False)
+
+mpc.subject_to(-ca.pi<= (furuta.theta1 <= ca.pi), include_first=False)
+
+# Solver choice
+options = {
+        "expand": True,
+        "structure_detection": "auto",
+        "fatrop.tol": 1e-4,
+        "print_time": False,
+        "fatrop.print_level": 0,
+        "debug": True,
+        "common_options":{"final_options":{"cse":True}},
+    }
+mpc.solver("fatrop", options)
+
+
+mpc.set_value(x_current, [-np.pi/3,0,0,0])
+mpc.set_value(x_final, [np.pi/3,0,0,0])
+
+ee = ca.vertcat(furuta.ee_x, furuta.ee_y, furuta.ee_z)
+pivot = ca.vertcat(furuta.pivot_x, furuta.pivot_y, furuta.pivot_z)
+
+# Transcription choice
+mpc.method(MultipleShooting(N=25,intg='heun'))
+
+# Solve
+sol = mpc.solve_limited()
+
+
+from pylab import *
+
+
+[ts, theta2sol] = sol.sample(furuta.theta2, grid='control')
+[ts_fine, theta2sol_fine] = sol.sample(furuta.theta2, grid='integrator',refine=10)
+
+print("theta2sol",theta2sol)
+
+figure()
+plot(ts, theta2sol,'b.')
+plot(ts_fine, theta2sol_fine,'b')
+xlabel('Time [s]')
+ylabel('theta2')
+
+figure()
+
+
+[_,ee_sol_fine] = sol.sample(ee,grid='integrator',refine=10)
+[_,ee_sol] = sol.sample(ee,grid='control')
+[_,pivot_sol] = sol.sample(pivot,grid='control')
+
+
+[_,theta1_sol] = sol.sample(furuta.theta1,grid='integrator',refine=10)
+[_,theta2_sol] = sol.sample(furuta.theta2,grid='integrator',refine=10)
+
+xlabel("theta1")
+ylabel("theta2")
+plot(theta1_sol,theta2_sol)
+
+axis('square')
+
+figure()
+
+plot(ee_sol_fine[:,1],ee_sol_fine[:,2])
+plot(ee_sol[:,1],ee_sol[:,2],'k.')
+
+for k in range(ee_sol.shape[0]):
+    plot([ee_sol[k,1],pivot_sol[k,1]],[ee_sol[k,2],pivot_sol[k,2]],'k-')
+
+axis('square')
+show()
+
+# Export MPC controller
+
+mpc.export('tutorial',short_output=True)

impact_tutorial/furuta.yaml

@@ -0,0 +1,62 @@
+# Simplified Futura Pendulum Model for tutorial
+# Cazzolato, B. S., & Prime, Z. (2011). On the dynamics of the furuta pendulum. Journal of Control Science and Engineering, 2011(1), 528341.
+# equation 33 and 34
+equations:
+  inline:
+    ode:
+      theta1: dtheta1
+      theta2: dtheta2
+      dtheta1: ((((((m2*l1)*l2)*cos(theta2))/(((J1+(m2*sq(l1)))*J2)-sq((((m2*l1)*l2)*cos(theta2)))))*((((sin(theta2)*((m2*g)*l2))+(sin(theta2)*(((m2*l1)*l2)*(dtheta1*dtheta2))))-(dtheta1*(((m2*l1)*l2)*(sin(theta2)*dtheta2))))+(b2*dtheta2)))-((J2/(((J1+(m2*sq(l1)))*J2)-sq((((m2*l1)*l2)*cos(theta2)))))*((b1*dtheta1)-((dtheta2*(((m2*l1)*l2)*(sin(theta2)*dtheta2)))+Torque1))))
+      dtheta2: ((((((m2*l1)*l2)*cos(theta2))/(((J1+(m2*sq(l1)))*J2)-sq((((m2*l1)*l2)*cos(theta2)))))*((b1*dtheta1)-((dtheta2*(((m2*l1)*l2)*(sin(theta2)*dtheta2)))+Torque1)))-(((J1+(m2*sq(l1)))/(((J1+(m2*sq(l1)))*J2)-sq((((m2*l1)*l2)*cos(theta2)))))*((((sin(theta2)*((m2*g)*l2))+(sin(theta2)*(((m2*l1)*l2)*(dtheta1*dtheta2))))-(dtheta1*(((m2*l1)*l2)*(sin(theta2)*dtheta2))))+(b2*dtheta2))))
+    outputs:
+      pivot_x: L1*cos(theta1)
+      pivot_y: L1*sin(theta1)
+      pivot_z: z1
+      ee_x: L1*cos(theta1) - L2*sin(theta1)*sin(theta2)
+      ee_y: L1*sin(theta1) + L2*cos(theta1)*sin(theta2)
+      ee_z: z1 - L2*cos(theta2)
+      T: 1/2*(J1+m2*l1**2)*dtheta1**2+1/2*J2*dtheta2**2+m2*l1*l2*cos(theta2)*dtheta1*dtheta2
+      V: m2*g*l2*(1-cos(theta2))
+differential_states: 
+  - name: theta1
+  - name: theta2
+  - name: dtheta1
+  - name: dtheta2
+controls: 
+  - name: Torque1
+outputs: 
+  - name: pivot_x
+  - name: pivot_y
+  - name: pivot_z
+  - name: ee_x
+  - name: ee_y
+  - name: ee_z
+  - name: T
+  - name: V
+parameters:
+  - name: b1
+    value: 0.0001 # N m s
+  - name: b2
+    # value: 0.00028 # N m s
+    value: 0.00013347   # N m s => 1.3347e-04
+
+constants:
+  inline:
+  # parameters from the section 8 of the paper
+    L1:    0.043 # m
+    L2:    0.147 # m
+    l1:    L1/2 # m
+    #l2:    0.088 # m
+    l2:    0.08900000996197938 # m
+    m1:    0.020 # kg
+    m2:    0.019 # kg
+    # J1:    0.0248 # kg m^2
+    # J2:    0.00386 # kg m^2
+    J1:    (1/12)*m1*L1**2 
+    J2:    0.00019756545194032853-m2*l2**2 
+    J0h:   J1+m1*l1**2+m2*L1**2
+    J1h:   J1+m1*l1**2
+    J2h:   J2+m2*l2**2
+    g:     9.81  # m/s^2
+    Torque2: 0.0 # N m
+    z1:    0.1825 # m # height of the pivot point

impact_tutorial/furuta_velocity_mode.yaml

@@ -0,0 +1,64 @@
+# Simplified Futura Pendulum Model for tutorial operated in velocity mode
+# Cazzolato, B. S., & Prime, Z. (2011). On the dynamics of the furuta pendulum. Journal of Control Science and Engineering, 2011(1), 528341.
+# From equation (19), the firs equation is used only to define torque1 as output. From the second equation, isolating \ddot{\theta}_2 we get the dynamic equation of the system.
+equations:
+  inline:
+    ode:
+      theta1: dtheta1
+      theta2: dtheta2
+      dtheta1: ddtheta1
+      dtheta2: (-((((((((m2*l1)*l2)*cos(theta2))*ddtheta1)-(dtheta1*(((m2*l1)*l2)*(sin(theta2)*dtheta2))))+((sin(theta2)*((m2*g)*l2))+(sin(theta2)*(((m2*l1)*l2)*(dtheta1*dtheta2)))))+(b2*dtheta2))/J2))
+    outputs:
+      pivot_x: L1*cos(theta1)
+      pivot_y: L1*sin(theta1)
+      pivot_z: z1
+      ee_x: L1*cos(theta1) - L2*sin(theta1)*sin(theta2)
+      ee_y: L1*sin(theta1) + L2*cos(theta1)*sin(theta2)
+      ee_z: z1 - L2*cos(theta2)
+      T: 1/2*(J1+m2*l1**2)*dtheta1**2+1/2*J2*dtheta2**2+m2*l1*l2*cos(theta2)*dtheta1*dtheta2
+      V: m2*g*l2*(1-cos(theta2))
+      Torque1: (-(((((m2*l1)*l2)*cos(theta2))*((((((((m2*l1)*l2)*cos(theta2))*ddtheta1)-(dtheta1*(((m2*l1)*l2)*(sin(theta2)*dtheta2))))+((sin(theta2)*((m2*g)*l2))+(sin(theta2)*(((m2*l1)*l2)*(dtheta1*dtheta2)))))+(b2*dtheta2))/J2))-((((0.5*(J1+(m2*sq(l1))))*(ddtheta1+ddtheta1))-(dtheta2*(((m2*l1)*l2)*(sin(theta2)*dtheta2))))+(b1*dtheta1))))
+differential_states: 
+  - name: theta1
+  - name: theta2
+  - name: dtheta1
+  - name: dtheta2
+controls: 
+  - name: ddtheta1
+outputs: 
+  - name: pivot_x        
+  - name: pivot_y        
+  - name: pivot_z        
+  - name: ee_x           
+  - name: ee_y          
+  - name: ee_z           
+  - name: T              
+  - name: V 
+  - name: Torque1             
+parameters:
+  - name: b1
+    value: 0.0001 # N m s
+  - name: b2
+    # value: 0.00028 # N m s
+    value: 0.00013347   # N m s => 1.3347e-04
+
+constants:
+  inline:
+  # parameters from the section 8 of the paper
+    L1:    0.043 # m
+    L2:    0.147 # m
+    l1:    L1/2 # m
+    #l2:    0.088 # m
+    l2:    0.08900000996197938 # m
+    m1:    0.020 # kg
+    m2:    0.019 # kg
+    # J1:    0.0248 # kg m^2
+    # J2:    0.00386 # kg m^2
+    J1:    (1/12)*m1*L1**2 
+    J2:    0.00019756545194032853-m2*l2**2 
+    J0h:   J1+m1*l1**2+m2*L1**2
+    J1h:   J1+m1*l1**2
+    J2h:   J2+m2*l2**2
+    g:     9.81  # m/s^2
+    Torque2: 0.0 # N m
+    z1:    0.1825 # m # height of the pivot point

impact_tutorial/simulate0.py

@@ -0,0 +1,64 @@
+import pylab as plt
+import numpy as np
+
+import sys
+sys.path.insert(0,"tutorial_build_dir")
+from impact import Impact
+
+impact = Impact("tutorial",src_dir=".")
+
+impact.solve()
+
+# Get solution trajectory
+x_opt = impact.get("x_opt", impact.ALL, impact.EVERYWHERE, impact.FULL)
+
+# Plotting
+_, ax = plt.subplots(3,1,sharex=True)
+ax[0].plot(x_opt.T)
+ax[0].set_title('Single OCP')
+ax[0].set_xlabel('Sample')
+
+print("Running MPC simulation loop")
+
+history = []
+runtime = []
+
+
+for i in range(1000):
+
+  mark = ((i//300) % 2 == 0)
+  sign = (mark-0.5)*2
+
+  impact.set("p", "x_final", impact.EVERYWHERE, impact.FULL, [sign*np.pi/3,0,0,0])
+
+  impact.solve()
+
+  runtime.append(impact.get_stats().runtime*1000)
+
+  # Optimal input at k=0
+  u = impact.get("u_opt", impact.ALL, 0, impact.FULL)
+
+  # Simulate 1 step forward in time (ask MPC prediction model)
+  x_sim = impact.get("x_opt", impact.ALL, 1, impact.FULL)
+
+  # Add some artificial noise
+  x_sim+= np.random.normal(0, 0.001, size=(x_sim.shape[0],1))
+
+  # Update current state
+  impact.set("x_current", impact.ALL, 0, impact.FULL, x_sim)
+  history.append(x_sim)
+
+# More plotting
+ax[1].plot(np.hstack(history).T)
+ax[1].set_title('Simulated MPC')
+ax[1].set_xlabel('Sample')
+
+ax[2].plot(np.hstack(runtime).T)
+ax[2].set_title('Runtime [ms]')
+ax[2].set_xlabel('Sample')
+plt.show()
+plt.show()
+
+
+
+