param pi := 4*atan(1); param g := 9.8e+2; # acc. due to gravity param tau_bnd := 250; param I1 := 1; param I2 := 1; param l1 := 1; param l2 := 1; param lc1 := l1/2; param lc2 := l2/2; param m1 := 1; param m2 := 1; param n; param dt := 1/n; var q1 {j in 0..n} := -pi/2 + pi*j*dt; var q2 {j in 0..n} := 0; var q1dot {j in 0..n by 1/2}; var q2dot {j in 0..n by 1/2}; var q1dot2 {j in 0..n}; var q2dot2 {j in 0..n}; var d11 {j in 0..n} = m1*lc1^2 + m2*(l1^2 + lc2^2 + 2*l1*lc2*cos(q2[j])) + I1 + I2; param d22 {j in 0..n} = m2*lc2^2 + I2; var d12 {j in 0..n} = m2*(lc2^2 + l1*lc2*cos(q2[j])) + I2; var d21 {j in 0..n} = m2*(lc2^2 + l1*lc2*cos(q2[j])) + I2; var h1 {j in 0..n} = -m2*l1*lc2*sin(q2[j])*q2dot[j]^2 - 2*m2*l1*lc2*sin(q2[j])*q2dot[j] *q1dot[j]; var h2 {j in 0..n} = m2*l1*lc2*sin(q2[j])*q1dot[j]^2; var phi1 {j in 0..n} = (m1*lc1 + m2*l1)*g*cos(q1[j]) + m2*lc2*g*cos(q1[j] + q2[j]); var phi2 {j in 0..n} = m2*lc2*g*cos(q1[j] + q2[j]); var tau {j in 0..n} >= -tau_bnd, <= tau_bnd; var abstau {j in 1..n-1}; #minimize total_torque_squared: sum {j in 1..n-1} sqrt(1e-4 + tau[j]^2) * dt; #minimize total_torque_squared: sum {j in 1..n-1} tau[j]^2 * dt; #minimize total_torque_squared: sum {j in 1..n-1} (q1dot2[j]^2 + q2dot2[j]^2)* dt; #minimize zero: 0; minimize taubound: (2/dt) * sum {j in 1..n-1} abstau[j]*dt; subject to upbnd {j in 1..n-1}: tau[j] <= abstau[j]; subject to lobnd {j in 1..n-1}: -abstau[j] <= tau[j]; subject to q1_init: q1[0] = -pi/2; subject to q2_init: q2[0] = 0; subject to q1dot_init: q1dot[0] = 0; subject to q2dot_init: q2dot[0] = 0; subject to q1_final: q1[n] = pi/2; subject to q2_final: q2[n] = 0; subject to q1dot_final: q1dot[n] = 0; subject to q2dot_final: q2dot[n] = 0; subject to q1def {j in (1/2)..(n-1/2)}: (q1[j+1/2]-q1[j-1/2])/dt = q1dot[j]; subject to q2def {j in (1/2)..(n-1/2)}: (q2[j+1/2]-q2[j-1/2])/dt = q2dot[j]; subject to q1dotdef1 {j in (1/2)..(n-1/2)}: (q1dot[j]-q1dot[j-1/2])/(dt/2) = q1dot2[j-1/2]; subject to q2dotdef1 {j in (1/2)..(n-1/2)}: (q2dot[j]-q2dot[j-1/2])/(dt/2) = q2dot2[j-1/2]; subject to q1dotdef2 {j in (1/2)..(n-1/2)}: (q1dot[j+1/2]-q1dot[j])/(dt/2) = q1dot2[j+1/2]; subject to q2dotdef2 {j in (1/2)..(n-1/2)}: (q2dot[j+1/2]-q2dot[j])/(dt/2) = q2dot2[j+1/2]; subject to Newton1 {j in 0..n}: d11[j]*q1dot2[j] + d12[j]*q2dot2[j] + h1[j] + phi1[j] = 0; subject to Newton2 {j in 0..n}: d21[j]*q1dot2[j] + d22[j]*q2dot2[j] + h2[j] + phi2[j] = tau[j]; #subject to smoothness1 {j in 1..n-1}: -50 <= q1dot2[j] <= 50; #subject to smoothness2 {j in 1..n-1}: -50 <= q2dot2[j] <= 50; #subject to smoothness {j in 1..n-1}: # -30 <= (tau[j-1]-2*tau[j]+tau[j+1])/dt^2 <= 30; #subject to smoothness_lo {j in 0..50}: -0.01 <= tau[j] <= 0.01; #subject to smoothness_hi {j in n-50..n}: -0.01 <= tau[j] <= 0.01; #fix {j in 0..1} tau[j] := 0; #fix {j in n-1..n} tau[j] := 0; #fix {j in 3..n-3} tau[j] := 0; #option loqo_options "verbose=2 inftol=1e-1 iterlim=50"; #option loqo_options "verbose=2 inftol=4 epsdiag=1e-5"; option loqo_options "verbose=2 sigfig=15 inftol=2e-3"; let n := 5000; #option solver snopt; solve; printf {j in 0..n}: "%10f %10f \n", l1*cos(q1[j]), l1*sin(q1[j]) > "xy1.txt"; printf {j in 0..n}: "%10f %10f \n", l1*cos(q1[j])+l2*cos(q1[j]+q2[j]), l1*sin(q1[j])+l2*sin(q1[j]+q2[j]) > "xy2.txt"; printf {j in 0..n}: "%10f \n", tau[j] > "tau.txt"; printf {j in 0..n}: "tau[%5d] = %10f \n", j, tau[j] > "tau_of_j.txt"; display max{j in 1..n-1} tau[j]; display min{j in 1..n-1} tau[j];