impact.export now cross-platform

meco-group · Feb 23, 2025 · f2fdcae · f2fdcae
1 parent f46638e
commit f2fdcae
Show file tree

Hide file tree

Showing 3 changed files with 199 additions and 52 deletions.
diff --git 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
@@ -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
@@ -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,75 +70,80 @@
 
 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,
         "debug": True,
         "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