ci updates

README.md

@@ -48,15 +48,17 @@ For Windows users, follow these steps to install Miniconda:
 ### Create the Conda environment for the workshop
 
 ```
-conda create --name workshop_dirac python=<PYTHON_VERSION> pip=23.0
+conda create --name workshop_dirac python=<PYTHON_VERSION> -y --channel=defaults --override-channels
 conda activate workshop_dirac
-conda install -y --channel conda-forge cmake clang lld llvmdev ninja
+(Only Windows) $ conda install -y --channel conda-forge cmake clang=15 lld=15 llvmdev=15 ninja 
+((Only linux and Mac) $ conda install -y --channel conda-forge cmake clang lld llvmdev ninja
+
 pip install -r requirements.txt
 ```
 
 ## Verify the environment
 
-4. Run `python torque_obstacle_avoidance_acados.py`
+4. Run `python test.py`
 
   You'll be queried "Do you wish to set up Tera renderer automatically?" Answer 'y'.
 

test.py

@@ -52,7 +52,7 @@
 # Solve
 sol = mpc.solve()
 
-mpc.export('torq_obs_aca',short_output=True)
+# mpc.export('torq_obs_aca',short_output=True)
 
 
 # Sample a state/control trajectory
@@ -68,4 +68,75 @@
 
 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 = {
+        "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)
+
+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)
+
+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]))
+
+
+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
+
+# Transcription
+mpc.method(MultipleShooting(N=50,M=1,intg='rk'))
+
+
+# Solve
+sol = mpc.solve()
+
+
+# mpc.export('torq_obs_fatrop',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')
+
+print(theta1sol)
+
+assert abs(theta1sol[0]-(-0.52359878))<1e-5
+
+