diff --git a/impact_tutorial/README.md b/impact_tutorial/README.md index d6aa5ab..7dfa27a 100644 --- a/impact_tutorial/README.md +++ b/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 + + + diff --git a/impact_tutorial/design0.m b/impact_tutorial/design0.m index fa87a31..4f36798 100644 --- a/impact_tutorial/design0.m +++ b/impact_tutorial/design0.m @@ -51,7 +51,7 @@ mpc.method(rockit.MultipleShooting('N',25,'intg','heun')); % Solve -sol = mpc.solve_limited(); +sol = mpc.solve(); diff --git a/impact_tutorial/design0.py b/impact_tutorial/design0.py index 62bbeb7..103ddcb 100644 --- a/impact_tutorial/design0.py +++ b/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 * diff --git a/impact_tutorial/design1.py b/impact_tutorial/design1.py new file mode 100644 index 0000000..103ddcb --- /dev/null +++ b/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) diff --git a/impact_tutorial/design2.py b/impact_tutorial/design2.py new file mode 100644 index 0000000..b4f591a --- /dev/null +++ b/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) diff --git a/impact_tutorial/design3.py b/impact_tutorial/design3.py new file mode 100644 index 0000000..e9769b8 --- /dev/null +++ b/impact_tutorial/design3.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('Release') + +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) diff --git a/impact_tutorial/design4.py b/impact_tutorial/design4.py new file mode 100644 index 0000000..c0950a7 --- /dev/null +++ b/impact_tutorial/design4.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('Release') + +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(-30 <= (mpc.der(furuta.dtheta2) <= 30 ), include_first=True, 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) diff --git a/impact_tutorial/design5.py b/impact_tutorial/design5.py new file mode 100644 index 0000000..b653ec3 --- /dev/null +++ b/impact_tutorial/design5.py @@ -0,0 +1,104 @@ +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('Release') + +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(-30 <= (mpc.der(furuta.dtheta2) <= 30 ), include_first=False, include_last=False) +mpc.subject_to(-ca.pi<= (furuta.theta1 <= ca.pi), include_first=False) + +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 +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) + +# Solve +sol = mpc.solve() + + +from pylab import * + + +[ts, theta2sol] = sol.sample(furuta.theta2, grid='control') +[ts_fine, theta2sol_fine] = sol.sample(furuta.theta2, grid='control') + +[ts, dtheta2sol] = sol.sample(furuta.dtheta2, grid='control') +[ts_fine, dtheta2sol_fine] = sol.sample(furuta.dtheta2, grid='control') + +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='control') + +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='control') +[_,ee_sol] = sol.sample(ee,grid='control') +[_,pivot_sol] = sol.sample(pivot,grid='control') + + +[_,theta1_sol] = sol.sample(furuta.theta1,grid='control') +[_,theta2_sol] = sol.sample(furuta.theta2,grid='control') + +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)