diff --git a/requirements.txt b/requirements.txt index ff962dc..031b07d 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,3 +1,3 @@ casadi>=3.6.7 -rockit-meco==0.5.5 -impact-meco==0.3.4 +rockit-meco==0.6.0 +impact-meco==0.4.1 diff --git a/test.m b/test.m new file mode 100644 index 0000000..8932d8e --- /dev/null +++ b/test.m @@ -0,0 +1,140 @@ +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'}) + +mpc = impact.MPC('T',3.0); + + +furuta_pendulum = mpc.add_model('fu_pendulum','furuta.yaml'); + +furuta_pendulum.ee_x +% Parameters +x_current = mpc.parameter('x_current',furuta_pendulum.nx); +x_final = mpc.parameter('x_final',furuta_pendulum.nx); + + +% Objectives +mpc.add_objective(mpc.sum(furuta_pendulum.Torque1^2 )); + +% Initial and final state constraints +mpc.subject_to(mpc.at_t0(furuta_pendulum.x)==x_current); +mpc.subject_to(mpc.at_tf(furuta_pendulum.x)==x_final); + +% Torque limits +mpc.subject_to(-40 <= furuta_pendulum.Torque1 <= 40 ); +% Constraint to only one turn +mpc.subject_to(-pi<= furuta_pendulum.theta1 <= pi, 'include_first','false'); + + +ee = [furuta_pendulum.ee_x; furuta_pendulum.ee_y; furuta_pendulum.ee_z]; +pivot = [furuta_pendulum.pivot_x; furuta_pendulum.pivot_y; furuta_pendulum.pivot_z]; + +ee_nominal = evalf(substitute(ee,furuta_pendulum.x,[0,0,0,0])); +pivot_nominal = evalf(substitute(pivot,furuta_pendulum.x,[0,0,0,0])); + +mpc.set_value(x_current, [-pi/6,0,0,0]); % Start point +mpc.set_value(x_final, [pi/6,0,0,0]); % End point + +% Transcription +method = rockit.external_method('acados', 'N',50,'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',10); + +mpc.method(method); + +% Solve +sol = mpc.solve(); + +mpc.export('torq_obs_aca','short_output',true); + +% Sample a state/control trajectory +[tsa, theta1sol] = sol.sample(furuta_pendulum.theta1, 'grid','control'); +[tsa, theta2sol] = sol.sample(furuta_pendulum.theta2', 'grid','control'); +[tsa, dtheta1sol] = sol.sample(furuta_pendulum.dtheta1, 'grid','control'); +[tsa, dtheta2sol] = sol.sample(furuta_pendulum.dtheta2, 'grid','control'); + +[tsb, Torque1sol] = sol.sample(furuta_pendulum.Torque1, 'grid','control'); +% tsb, Torque2sol = sol.sample(furuta_pendulum.Torque2, grid='control') + +theta1sol + +assert(abs(theta1sol(1)-(-0.52359878))<1e-5) + + + +fatrop_options = struct(); +fatrop_options.expand = true; +fatrop_options.structure_detection = 'auto'; +fatrop_options.fatrop.tol = 1e-4; +fatrop_options.debug = true; +fatrop_options.common_options.final_options.cse = true; + +solver_config = {{'fatrop',fatrop_options},{'ipopt',struct},{'sqpmethod',struct('qpsol', 'osqp')},{'sleqp',struct}}; + +for i=1:length(solver_config) + solver = solver_config{i}{1}; + solver_options = solver_config{i}{2}; + + + mpc = impact.MPC('T',3.0); + + + furuta_pendulum = mpc.add_model('fu_pendulum','furuta.yaml'); + + furuta_pendulum.ee_x + % Parameters + x_current = mpc.parameter('x_current',furuta_pendulum.nx); + x_final = mpc.parameter('x_final',furuta_pendulum.nx); + + + % Objectives + mpc.add_objective(mpc.sum(furuta_pendulum.Torque1^2 )); + + % Initial and final state constraints + mpc.subject_to(mpc.at_t0(furuta_pendulum.x)==x_current); + mpc.subject_to(mpc.at_tf(furuta_pendulum.x)==x_final); + + % Torque limits + mpc.subject_to(-40 <= furuta_pendulum.Torque1 <= 40 ); + % Constraint to only one turn + mpc.subject_to(-pi<= furuta_pendulum.theta1 <= pi, 'include_first','false'); + + + ee = [furuta_pendulum.ee_x; furuta_pendulum.ee_y; furuta_pendulum.ee_z]; + pivot = [furuta_pendulum.pivot_x; furuta_pendulum.pivot_y; furuta_pendulum.pivot_z]; + + ee_nominal = evalf(substitute(ee,furuta_pendulum.x,[0,0,0,0])); + pivot_nominal = evalf(substitute(pivot,furuta_pendulum.x,[0,0,0,0])); + + mpc.set_value(x_current, [-pi/6,0,0,0]); % Start point + mpc.set_value(x_final, [pi/6,0,0,0]); % End point + + % Transcription + + mpc.method(rockit.MultipleShooting('N',50,'M',1,'intg','rk')); + + mpc.solver(solver, solver_options); + + % Solve + sol = mpc.solve(); + + mpc.export('torq_obs_aca','short_output',true); + + % Sample a state/control trajectory + [tsa, theta1sol] = sol.sample(furuta_pendulum.theta1, 'grid','control'); + [tsa, theta2sol] = sol.sample(furuta_pendulum.theta2', 'grid','control'); + [tsa, dtheta1sol] = sol.sample(furuta_pendulum.dtheta1, 'grid','control'); + [tsa, dtheta2sol] = sol.sample(furuta_pendulum.dtheta2, 'grid','control'); + + [tsb, Torque1sol] = sol.sample(furuta_pendulum.Torque1, 'grid','control'); + % tsb, Torque2sol = sol.sample(furuta_pendulum.Torque2, grid='control') + + theta1sol + + assert(abs(theta1sol(1)-(-0.52359878))<1e-5) + + +end + + diff --git a/test.py b/test.py index 9f2b1df..959ddec 100644 --- a/test.py +++ b/test.py @@ -5,7 +5,7 @@ from impact import * import casadi as ca import numpy as np - +import subprocess import rockit @@ -52,8 +52,10 @@ # Solve sol = mpc.solve() -# mpc.export('torq_obs_aca',short_output=True) - +mpc.export('torq_obs_aca',short_output=True) +env = dict(os.environ) +env["MPLBACKEND"] = "Agg" +assert subprocess.run(["python","hello_world_torq_obs_aca.py"],env=env,cwd="torq_obs_aca_build_dir").returncode==0 # Sample a state/control trajectory tsa, theta1sol = sol.sample(furuta_pendulum.theta1, grid='control') @@ -68,31 +70,8 @@ assert abs(theta1sol[0]-(-0.52359878))<1e-5 -mpc = MPC(T=3.0) - -furuta_pendulum = mpc.add_model('fu_pendulum','furuta.yaml') - - -print(furuta_pendulum.ee_x) -## Parameters -x_current = mpc.parameter('x_current',furuta_pendulum.nx) -x_final = mpc.parameter('x_final',furuta_pendulum.nx) - - -## Objectives -# mpc.add_objective(mpc.sum(furuta_pendulum.Torque1**2 + furuta_pendulum.Torque2**2)) -mpc.add_objective(mpc.sum(furuta_pendulum.Torque1**2 )) -# Initial and final state constraints -mpc.subject_to(mpc.at_t0(furuta_pendulum.x)==x_current) -mpc.subject_to(mpc.at_tf(furuta_pendulum.x)==x_final) - -# Torque limits -mpc.subject_to(-40 <= (furuta_pendulum.Torque1 <= 40 )) -# Constraint to only one turn -mpc.subject_to(-ca.pi<= (furuta_pendulum.theta1 <= ca.pi), include_first=False) - -options = { +for solver, solver_options in [("fatrop",{ "expand": True, "structure_detection": "auto", "fatrop.tol": 1e-4, @@ -100,43 +79,71 @@ "common_options":{"final_options":{"cse":True}}, "jit": False, "jit_options": {"flags":["-O3","-ffast-math"]} - } -mpc.solver("fatrop", options) + }),("ipopt",{}),("sqpmethod",{"qpsol": "osqp"}),("sleqp",{})]: -ee = ca.vertcat(furuta_pendulum.ee_x, furuta_pendulum.ee_y, furuta_pendulum.ee_z) -pivot = ca.vertcat(furuta_pendulum.pivot_x, furuta_pendulum.pivot_y, furuta_pendulum.pivot_z) + mpc = MPC(T=3.0) -ee_nominal = ca.evalf(ca.substitute(ee,furuta_pendulum.x,[0,0,0,0])) -print("ee_nominal",ee_nominal) -pivot_nominal = ca.evalf(ca.substitute(pivot,furuta_pendulum.x,[0,0,0,0])) + furuta_pendulum = mpc.add_model('fu_pendulum','furuta.yaml') -mpc.set_value(x_current, [-np.pi/6,0,0,0]) # Start point -mpc.set_value(x_final, [np.pi/6,0,0,0]) # End point + print(furuta_pendulum.ee_x) + ## Parameters + x_current = mpc.parameter('x_current',furuta_pendulum.nx) + x_final = mpc.parameter('x_final',furuta_pendulum.nx) -# Transcription -mpc.method(MultipleShooting(N=50,M=1,intg='rk')) + ## Objectives + # mpc.add_objective(mpc.sum(furuta_pendulum.Torque1**2 + furuta_pendulum.Torque2**2)) + mpc.add_objective(mpc.sum(furuta_pendulum.Torque1**2 )) -# Solve -sol = mpc.solve() + # Initial and final state constraints + mpc.subject_to(mpc.at_t0(furuta_pendulum.x)==x_current) + mpc.subject_to(mpc.at_tf(furuta_pendulum.x)==x_final) + # Torque limits + mpc.subject_to(-40 <= (furuta_pendulum.Torque1 <= 40 )) + # Constraint to only one turn + mpc.subject_to(-ca.pi<= (furuta_pendulum.theta1 <= ca.pi), include_first=False) -# mpc.export('torq_obs_fatrop',short_output=True) + mpc.solver(solver, solver_options) + ee = ca.vertcat(furuta_pendulum.ee_x, furuta_pendulum.ee_y, furuta_pendulum.ee_z) + pivot = ca.vertcat(furuta_pendulum.pivot_x, furuta_pendulum.pivot_y, furuta_pendulum.pivot_z) -# Sample a state/control trajectory -tsa, theta1sol = sol.sample(furuta_pendulum.theta1, grid='control') -tsa, theta2sol = sol.sample(furuta_pendulum.theta2, grid='control') -tsa, dtheta1sol = sol.sample(furuta_pendulum.dtheta1, grid='control') -tsa, dtheta2sol = sol.sample(furuta_pendulum.dtheta2, grid='control') + ee_nominal = ca.evalf(ca.substitute(ee,furuta_pendulum.x,[0,0,0,0])) + print("ee_nominal",ee_nominal) + pivot_nominal = ca.evalf(ca.substitute(pivot,furuta_pendulum.x,[0,0,0,0])) -tsb, Torque1sol = sol.sample(furuta_pendulum.Torque1, grid='control') -# tsb, Torque2sol = sol.sample(furuta_pendulum.Torque2, grid='control') -print(theta1sol) + mpc.set_value(x_current, [-np.pi/6,0,0,0]) # Start point + mpc.set_value(x_final, [np.pi/6,0,0,0]) # End point -assert abs(theta1sol[0]-(-0.52359878))<1e-5 + # Transcription + mpc.method(MultipleShooting(N=50,M=1,intg='rk')) + + + # Solve + sol = mpc.solve() + + + mpc.export(f'torq_obs_{solver}',short_output=True) + env = dict(os.environ) + env["MPLBACKEND"] = "Agg" + assert subprocess.run(["python",f"hello_world_torq_obs_{solver}.py"],env=env,cwd=f"torq_obs_{solver}_build_dir").returncode==0 + + + # Sample a state/control trajectory + tsa, theta1sol = sol.sample(furuta_pendulum.theta1, grid='control') + tsa, theta2sol = sol.sample(furuta_pendulum.theta2, grid='control') + tsa, dtheta1sol = sol.sample(furuta_pendulum.dtheta1, grid='control') + tsa, dtheta2sol = sol.sample(furuta_pendulum.dtheta2, grid='control') + + tsb, Torque1sol = sol.sample(furuta_pendulum.Torque1, grid='control') + # tsb, Torque2sol = sol.sample(furuta_pendulum.Torque2, grid='control') + + print(theta1sol) + + assert abs(theta1sol[0]-(-0.52359878))<1e-5