tutorial steps

impact_tutorial/README.md

@@ -7,4 +7,49 @@
  * You can run a simulation of the MPC controller running over 1000 samples using `simulate.py`
 
 
-## Step 1: Getting acquainted with the furuta example
+## Step 1: Torque control versus velocity control
+
+ * In the `add_model` command, swap `furuta.yaml` by `furuta_velocity_mode.yaml`. Explain why the Torque plot is no longer piecewise-constant.
+
+## Step 2: Playing with the objective function
+
+ * Try changing the objective function, first to `dtheta1**2` and `dtheta**2`? Can you explain why the `theta2` plot looks differently? Hint: try plotting `dtheta2`, too.
+
+ Trick question: explain the plots if you use `theta1**2` in the objective.
+
+## Step 3: On the runtime statistics
+ * Run `simulate.py` a couple of times, looking at the bottom plot.
+   You'll notice the runtime having a noisy behaviour.
+   That's because teh controller is running on a non-realtime operating system.
+
+ * Switch `set_cmake_build_type('Debug')` to `set_cmake_build_type('Release')`.
+   Verify that you find a speedup.
+
+## Step 4: Extra constraints
+
+ * In the `dtheta2` plot you made, you'll notice that max velocity is reached in one sampling time. Let's try to calm down this motion.
+   Constrain `mpc.der(furuta.dtheta2)` to lie between -30 and 30 rad/s^2.
+ * What happens if you change this constraints's `include_first` keyword agument to `True`? How does this affect the plot?
+
+## Step 5: Switch to ACADOS
+
+ * Instead of fatrop + MultipleShooting, switch to ACADOS solver:
+```
+method = external_method('acados', N=25,qp_solver='PARTIAL_CONDENSING_HPIPM',nlp_solver_max_iter=200,hessian_approx='EXACT',regularize_method = 'CONVEXIFY',integrator_type='ERK',nlp_solver_type='SQP',qp_solver_cond_N=5)
+mpc.method(method)
+```
+ * You'll encounter two errors to fix
+  1. `lh <= h(x,u) <= uh only supported for qualifier include_first=False`
+      Solution: set `include_first` to `False` again.
+  2. `AttributeError: 'AcadosMethod' object has no attribute 'poly_coeff'`
+     It was nice being able to make high-resolution plots with (settings `grid='integrator', refine=10`)
+     Unforntunately, such introspection is not availble with acados. Replace all occurances with `grid='control'`
+ * Verify that you get simular results for `design5.py` and `simulate.py` for FATROP versus ACADOS.
+
+## Step 6: Simulate in Simulink
+
+ * In Simulink, load `library_tutorial.slx` that you'll find generated in `tutorial_build_dir`
+ * Follow instructions by the tutor
+
+
+

impact_tutorial/design0.m

@@ -51,7 +51,7 @@
 mpc.method(rockit.MultipleShooting('N',25,'intg','heun'));
 
 % Solve
-sol = mpc.solve_limited();
+sol = mpc.solve();
 
 
 

impact_tutorial/design0.py

@@ -10,7 +10,7 @@
 mpc = impact.MPC(T=0.5)
 
 # Add furuta model
-furuta = mpc.add_model('fu_pendulum','furuta.yaml')
+furuta = mpc.add_model('fu_pendulum','furuta_velocity_mode.yaml')
 
 # Parameters
 x_current = mpc.parameter('x_current',furuta.nx)
@@ -52,7 +52,7 @@
 mpc.method(MultipleShooting(N=25,intg='heun'))
 
 # Solve
-sol = mpc.solve_limited()
+sol = mpc.solve()
 
 
 from pylab import *

impact_tutorial/design1.py

@@ -0,0 +1,111 @@
+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_velocity_mode.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": False,
+        "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()
+
+
+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')
+
+[ts, Torque1sol] = sol.sample(furuta.Torque1, grid='control')
+[ts_fine, Torque1sol_fine] = sol.sample(furuta.Torque1, grid='integrator',refine=10)
+
+figure()
+plot(ts, Torque1sol,'b.')
+plot(ts_fine, Torque1sol_fine,'b')
+xlabel('Time [s]')
+ylabel('Torque [N]')
+
+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/design2.py

@@ -0,0 +1,116 @@
+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_velocity_mode.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.dtheta2**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": False,
+        "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()
+
+
+from pylab import *
+
+
+[ts, theta2sol] = sol.sample(furuta.theta2, grid='control')
+[ts_fine, theta2sol_fine] = sol.sample(furuta.theta2, grid='integrator',refine=10)
+
+[ts, dtheta2sol] = sol.sample(furuta.dtheta2, grid='control')
+[ts_fine, dtheta2sol_fine] = sol.sample(furuta.dtheta2, grid='integrator',refine=10)
+
+print("theta2sol",theta2sol)
+
+figure()
+plot(ts, theta2sol,'b.')
+plot(ts_fine, theta2sol_fine,'b')
+plot(ts, dtheta2sol,'g.')
+plot(ts_fine, dtheta2sol_fine,'g')
+xlabel('Time [s]')
+ylabel('theta2')
+
+[ts, Torque1sol] = sol.sample(furuta.Torque1, grid='control')
+[ts_fine, Torque1sol_fine] = sol.sample(furuta.Torque1, grid='integrator',refine=10)
+
+figure()
+plot(ts, Torque1sol,'b.')
+plot(ts_fine, Torque1sol_fine,'b')
+xlabel('Time [s]')
+ylabel('Torque [N]')
+
+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)