├── .gitattributes ├── Cart-Pole ├── arm1 │ ├── cart-pole.ipynb │ ├── cart_pole_paper_batch.m │ ├── cp_all.pdf │ ├── cp_input.pdf │ ├── log135.mat │ ├── log171.mat │ ├── log45.mat │ ├── log90.mat │ └── refine_solution.m └── arm2 │ ├── deg45.mat │ ├── deg45_refine.mat │ ├── multi-cart-2.ipynb │ ├── plot_multi_cart_paper.m │ ├── refine_solution.m │ └── traj_cartpole3.pdf ├── Control_utils.jl ├── Drone-Load ├── drone-load.ipynb ├── log_NC_5.mat ├── plot_traj.m ├── refine_NC_5.mat ├── refine_batch_fmincon.m ├── refine_solution_det_load.m └── v2_load_traj_5.pdf ├── Drone ├── drone-quaternion.ipynb └── drone.ipynb ├── InverseKinematics ├── IK-Puma.ipynb ├── IK-random.ipynb ├── plot_statistic.m └── puma_test_data.mat ├── Quaternion ├── drone-load-polytope.ipynb ├── freespace.mat ├── logger_3.mat ├── quat-obs.pdf └── visualiza_drone_load_SDP.m └── README.md /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /Cart-Pole/arm1/cart_pole_paper_batch.m: -------------------------------------------------------------------------------- 1 | clear all;close all;clc; 2 | %% 3 | % dec_th3_3: deg45 4 | % th3_23: deg90 5 | % dec_th3_8: deg135 6 | % dec_th3_13: deg171 7 | %% 8 | file_list = ["log45.mat", 9 | "log90.mat", 10 | "log135.mat", 11 | "log171.mat"]; 12 | 13 | % figure() 14 | fig_size = [18 * 0.5, 1.3] * 6 ; 15 | h = figure('Renderer', 'painters', 'unit', 'centimeters', 'Position', [0, 0, fig_size]); 16 | % left_coner_list = [0.05, 0.3, 0.55, 0.8]; 17 | 18 | subplot(1, 1, 1, 'Position', [0.03, 0.01, 0.95, 1]) 19 | 20 | loc_offset = 0; 21 | for k = 1:4 22 | 23 | load(file_list(k)) 24 | 25 | if k == 1 || k == 2 26 | sol_eval= sol_approx; 27 | Ns = double(Ns); 28 | % sol_eval= sol; 29 | ns1 = Ns + 1; 30 | x_pole = sol_eval(1:2:ns1*2); 31 | x_cart = sol_eval(2:2:ns1*2); 32 | y_pole = sol_eval(ns1*2+1:3*ns1); 33 | c_pole = sol_eval(ns1*3+1:4*ns1); 34 | s_pole = sol_eval(ns1*4+1:5*ns1); 35 | 36 | vx_pole = sol_eval(5*ns1+1:2:ns1*7); 37 | vx_cart = sol_eval(5*ns1+2:2:ns1*7); 38 | vy_pole = sol_eval(7*ns1+1:ns1*8); 39 | ca_pole = sol_eval(ns1*8+1:9*ns1); 40 | sa_pole = sol_eval(ns1*9+1:10*ns1); 41 | 42 | offset = 10*ns1; 43 | lam_x = sol_eval(offset+1:offset+Ns); 44 | lam_y = sol_eval(offset+Ns+1:offset+2*Ns); 45 | tau = sol_eval(offset+Ns*2 + 1:offset+3*Ns); 46 | else 47 | 48 | end 49 | 50 | x_pole = x_pole + loc_offset; 51 | x_cart = x_cart + loc_offset; 52 | 53 | 54 | hold on 55 | plot(x_cart, x_cart * 0, "bo") 56 | 57 | for j = 1:length(x_pole) 58 | haha = plot([x_pole(j), x_cart(j)], [y_pole(j), 0], "b-", "LineWidth", 1); 59 | end 60 | plot(x_pole, y_pole, "r-o", "LineWidth", 2) 61 | plot(x_pole(1), y_pole(1), "go", "LineWidth", 1, 'MarkerFaceColor', 'g') 62 | plot(x_pole(end), y_pole(end), "go", "LineWidth", 1, 'MarkerFaceColor', 'g') 63 | daspect([1, 1, 1]) 64 | ylim([-0.5, 0.5]) 65 | xlim([-0.5, 7.5]) 66 | loc_offset = loc_offset + 2; 67 | 68 | % (obj - opt) / obj 69 | end 70 | grid on 71 | box on 72 | set(gca,'xtick',[]) 73 | set(h, 'Units','pixels'); 74 | set(h, 'PaperPositionMode','Auto','PaperUnits','centimeters','PaperSize', fig_size) 75 | print("cp_all", "-dpdf") 76 | %% 77 | deglist = [45, 90, 135, 171]; 78 | radlist = [0.25, 0.5, 0.75, 0.95]; 79 | 80 | fig_size = [16, 12]; 81 | h = figure('Renderer', 'painters', 'unit', 'centimeters', 'Position', [0, 0, fig_size]); 82 | for k = 1:4 83 | % hold on 84 | 85 | load(file_list(k)) 86 | 87 | if k == 1 || k == 2 88 | sol_eval= sol_approx; 89 | Ns = double(Ns); 90 | % sol_eval= sol; 91 | ns1 = Ns + 1; 92 | x_pole = sol_eval(1:2:ns1*2); 93 | x_cart = sol_eval(2:2:ns1*2); 94 | y_pole = sol_eval(ns1*2+1:3*ns1); 95 | c_pole = sol_eval(ns1*3+1:4*ns1); 96 | s_pole = sol_eval(ns1*4+1:5*ns1); 97 | 98 | vx_pole = sol_eval(5*ns1+1:2:ns1*7); 99 | vx_cart = sol_eval(5*ns1+2:2:ns1*7); 100 | vy_pole = sol_eval(7*ns1+1:ns1*8); 101 | ca_pole = sol_eval(ns1*8+1:9*ns1); 102 | sa_pole = sol_eval(ns1*9+1:10*ns1); 103 | 104 | offset = 10*ns1; 105 | lam_x = sol_eval(offset+1:offset+Ns); 106 | lam_y = sol_eval(offset+Ns+1:offset+2*Ns); 107 | tau = sol_eval(offset+Ns*2 + 1:offset+3*Ns); 108 | else 109 | 110 | end 111 | 112 | % x_pole = x_pole + loc_offset; 113 | % x_cart = x_cart + loc_offset; 114 | subplot(4, 1, k) 115 | % subplot(4, 1, k, 'Position', [0.05, (4 - k) * 0.26, 0.8, 0.2]) 116 | k 117 | hold on 118 | plot(linspace(0, 5, 80), lam_x, "-o", "MarkerSize", 2) 119 | plot(linspace(0, 5, 80), lam_y, "-o", "MarkerSize", 2) 120 | plot(linspace(0, 5, 80), tau, "-o", "MarkerSize", 2) 121 | 122 | % title("$\theta_0 = " + num2str(deglist(k)) + " \deg$", "Interpreter","Latex") 123 | title("$\theta_0 = " + num2str(radlist(k)) + " \pi$", "Interpreter","Latex") 124 | 125 | % plot(lam_x, "b.", "MarkerSize", 5) 126 | % plot(lam_y, "b.", "MarkerSize", 5) 127 | % plot(tau, "b.", "MarkerSize", 5) 128 | % (obj - opt) / obj 129 | box on 130 | grid on 131 | end 132 | xlabel("Time (s)", "Interpreter","Latex") 133 | legend({"$\lambda_x$", "$\lambda_y$", "$u$"}, "Interpreter","Latex") 134 | %% 135 | set(h, 'Units','pixels'); 136 | set(h, 'PaperPositionMode','Auto','PaperUnits','centimeters','PaperSize', fig_size) 137 | print("cp_input", "-dpdf") -------------------------------------------------------------------------------- /Cart-Pole/arm1/cp_all.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Cart-Pole/arm1/cp_all.pdf -------------------------------------------------------------------------------- /Cart-Pole/arm1/cp_input.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Cart-Pole/arm1/cp_input.pdf -------------------------------------------------------------------------------- /Cart-Pole/arm1/log135.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Cart-Pole/arm1/log135.mat -------------------------------------------------------------------------------- /Cart-Pole/arm1/log171.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Cart-Pole/arm1/log171.mat -------------------------------------------------------------------------------- /Cart-Pole/arm1/log45.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Cart-Pole/arm1/log45.mat -------------------------------------------------------------------------------- /Cart-Pole/arm1/log90.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Cart-Pole/arm1/log90.mat -------------------------------------------------------------------------------- /Cart-Pole/arm1/refine_solution.m: -------------------------------------------------------------------------------- 1 | function [x_pole, x_cart, y_pole, c_pole, s_pole, vx_pole, vx_cart, vy_pole, ca_pole, sa_pole, lam_x, lam_y, tau, obj, sol_refine] = refine_solution(log) 2 | %% 3 | dof = double(log.dof); 4 | Ns = double(log.Ns); 5 | 6 | t0 = log.param.theta_0; 7 | xc_0 = log.param.xc_0; 8 | yc_0 = 0.0; 9 | 10 | lx = log.param.lx; 11 | ly = log.param.ly; 12 | m = log.param.mass; 13 | I = log.param.inertial; 14 | dt = log.param.control_param.dt; 15 | g = -9.81; 16 | %% 17 | yalmip('clear') 18 | % R = sdpvar(dof+1, 3, 3); 19 | % F = sdpvar(dof+1, 3, 3); 20 | % p = sdpvar(dof+1, 3, 1); 21 | % v = sdpvar(dof+1, 3, 1); 22 | % tau = sdpvar(dof, 3, 1); 23 | % f = sdpvar(dof, 1); 24 | % p_load = sdpvar(dof+1, 3, 1); 25 | % v_load = sdpvar(dof+1, 3, 1); 26 | 27 | x = sdpvar(dof+1, Ns+1); 28 | y = sdpvar(dof, Ns+1); 29 | c = sdpvar(dof, Ns+1); 30 | s = sdpvar(dof, Ns+1); 31 | 32 | vx = sdpvar(dof+1, Ns+1); 33 | vy = sdpvar(dof, Ns+1); 34 | ca = sdpvar(dof, Ns+1); 35 | sa = sdpvar(dof, Ns+1); 36 | 37 | lam_x = sdpvar(dof, Ns); 38 | lam_y = sdpvar(dof, Ns); 39 | tau = sdpvar(Ns, 1); 40 | 41 | ns = dof * (Ns+1); 42 | nf = Ns; 43 | nlam = dof * Ns; 44 | 45 | %% 46 | % load 47 | eq = []; 48 | ineq = []; 49 | f = 0; 50 | 51 | eq = [eq; 52 | x(2, 1) - xc_0; 53 | vx(2, 1) - 0; 54 | c(1, 1) - cos(t0); 55 | s(1, 1) - sin(t0); 56 | cos(t0) * lx - sin(t0) * ly + x(1, 1) - xc_0; 57 | sin(t0) * lx + cos(t0) * ly + y(1, 1) - yc_0; 58 | vx(1, 1) - 0; 59 | vy(1, 1) - 0; 60 | ca(1, 1) - 1.0; 61 | sa(1, 1) - 0]; 62 | 63 | 64 | for k = 1:Ns 65 | Jx = (-c(1, k+1) * ly - s(1, k+1) * lx); 66 | Jy = (-s(1, k+1) * ly + c(1, k+1) * lx); 67 | eq = [eq; 68 | %% pole 69 | c(1, k+1) - (c(1, k) * ca(1, k) - s(1, k) * sa(1, k)); 70 | s(1, k+1) - (s(1, k) * ca(1, k) + c(1, k) * sa(1, k)); 71 | x(1, k+1) - (dt * (c(1, k) * vx(1, k) - s(1, k) * vy(1, k)) + x(1, k)); 72 | y(1, k+1) - (dt * (s(1, k) * vx(1, k) + c(1, k) * vy(1, k)) + y(1, k)); 73 | %% cart 74 | x(2, k+1) - (x(2, k) + dt * vx(2, k)); 75 | %% kinematic constraints 76 | c(1,k+1) * lx - s(1,k+1) * ly + x(1, k+1) - x(2, k+1); 77 | s(1,k+1) * lx + c(1,k+1) * ly + y(1, k+1) - 0; 78 | s(1,k+1)^2 + c(1,k+1)^2 - 1.0; 79 | sa(1,k+1)^2 + ca(1,k+1)^2 - 1.0; 80 | %% pole 81 | ((sa(1, k+1) - sa(1, k)) * I - (Jx * lam_x(1, k) + Jy * lam_y(1, k)) * dt^2); 82 | (m * vx(1, k+1) - m * ( ca(1, k) * vx(1, k) + sa(1, k) * vy(1, k) ) - ( c(1, k+1)*lam_x(1, k) + s(1, k+1)*lam_y(1, k) + m * s(1, k+1) * g)* dt ); 83 | (m * vy(1, k+1) - m * (-sa(1, k) * vx(1, k) + ca(1, k) * vy(1, k) ) - (- s(1, k+1)*lam_x(1, k) + c(1, k+1)*lam_y(1, k) + m * c(1, k+1) * g)* dt ); 84 | (m * vx(2, k+1) - m * vx(2, k) - (lam_x(1, k) + tau(k))* dt ); 85 | ]; 86 | 87 | f = f + ( x(2, k)^2 + x(1, k)^2 + (y(1, k) - abs(ly))^2 + (c(1, k) - 1)^2 + s(1, k)^2 ) * log.param.run_cost(1); 88 | f = f + (vx(2, k)^2 + vx(1, k)^2 + vy(1, k)^2 + (ca(1, k) - 1)^2 + sa(1, k)^2) * log.param.run_cost(1); 89 | f = f + log.param.run_cost(1) * tau(k)^2; 90 | end 91 | %% 92 | f = f + ( x(2, end)^2 + x(1, end)^2 + (y(1, end) - abs(ly))^2 + (c(1, end) - 1)^2 + s(1, end)^2) * log.param.terminal_cost(1); 93 | f = f + (vx(2, end)^2 + vx(1, end)^2 + vy(1, end)^2 + (ca(1, end) - 1)^2 + sa(1, end)^2) * log.param.terminal_cost(1); 94 | 95 | for k = 1:Ns 96 | ineq = [ineq; 97 | 20 - tau(k); 98 | 20 + tau(k); 99 | 50 - lam_x(k); 100 | 50 + lam_x(k); 101 | 50 - lam_y(k); 102 | 50 + lam_y(k); 103 | ca(1, k)]; 104 | end 105 | %% 106 | % sol_eval = log.sol_approx; 107 | sol_eval = log.sol; 108 | 109 | ns = dof * (Ns+1); 110 | 111 | offset = (dof + 1) * (Ns + 1); 112 | x_num = sol_eval(1:offset); 113 | 114 | y_num = sol_eval(offset+1:offset + ns); 115 | offset = offset + ns; 116 | 117 | c_num = sol_eval(offset+1:offset + ns); 118 | offset = offset + ns; 119 | 120 | s_num = sol_eval(offset+1:offset + ns); 121 | offset = offset + ns; 122 | 123 | vx_num = sol_eval(offset+1:offset + (dof + 1) * (Ns + 1)); 124 | offset = offset + (dof + 1) * (Ns + 1); 125 | 126 | vy_num = sol_eval(offset+1:offset + ns); 127 | offset = offset + ns; 128 | 129 | ca_num = sol_eval(offset+1:offset + ns); 130 | offset = offset + ns; 131 | 132 | sa_num = sol_eval(offset+1:offset + ns); 133 | offset = offset + ns; 134 | 135 | 136 | lam_x_num = sol_eval(offset+1:offset+dof * Ns); 137 | offset = offset + dof * Ns; 138 | 139 | lam_y_num = sol_eval(offset+1:offset+dof * Ns); 140 | offset = offset + dof * Ns; 141 | 142 | tau_num = sol_eval(offset+1:offset+dof * Ns); 143 | %% 144 | tau_num(tau_num > 20) = 20; 145 | tau_num(tau_num < -20) = -20; 146 | 147 | lam_y_num(lam_y_num > 50) = 50; 148 | lam_y_num(lam_y_num < -50) = -50; 149 | 150 | lam_x_num(lam_x_num > 50) = 50; 151 | lam_x_num(lam_x_num < -50) = -50; 152 | 153 | norm_1 = sqrt(c_num.^2 + s_num.^2); 154 | norm_2 = sqrt(ca_num.^2 + sa_num.^2); 155 | 156 | c_num = c_num ./ norm_1; 157 | s_num = s_num ./ norm_1; 158 | 159 | ca_num = ca_num ./ norm_2; 160 | sa_num = sa_num ./ norm_2; 161 | 162 | assign(x, reshape(x_num, size(x))); 163 | assign(y, reshape(y_num, size(y))); 164 | assign(c, reshape(c_num, size(c))); 165 | assign(s, reshape(s_num, size(s))); 166 | 167 | assign(vx, reshape(vx_num, size(vx))); 168 | assign(vy, reshape(vy_num, size(vy))); 169 | assign(ca, reshape(ca_num, size(ca))); 170 | assign(sa, reshape(sa_num, size(sa))); 171 | 172 | assign(lam_x, reshape(lam_x_num, size(lam_x))); 173 | assign(lam_y, reshape(lam_y_num, size(lam_y))); 174 | assign(tau, reshape(tau_num, size(tau))); 175 | %% 176 | % ops = sdpsettings('solver','fmincon');%,'fmincon.algorithm','sqp'); 177 | ops = sdpsettings('solver','ipopt'); 178 | ops.print_level = 12; 179 | ops.verbose = true; 180 | 181 | % ops.fmincon.MaxIterations = 1e7; 182 | ops.ipopt.max_iter = 1e4 * 10; 183 | ops.ipopt.max_cpu_time = 1e100; 184 | 185 | ops.usex0 = 1; 186 | optimize([eq == 0; ineq >= 0],f,ops); 187 | %% 188 | x_pole = value(x(1, :)); 189 | x_cart = value(x(2, :)); 190 | y_pole = value(y(1, :)); 191 | c_pole = value(c(1, :)); 192 | s_pole = value(s(1, :)); 193 | 194 | vx_pole = value(vx(1, :)); 195 | vx_cart = value(vx(2, :)); 196 | vy_pole = value(vy(1, :)); 197 | ca_pole = value(ca(1, :)); 198 | sa_pole = value(sa(1, :)); 199 | 200 | lam_x = value(lam_x); 201 | lam_y = value(lam_y); 202 | tau = value(tau); 203 | 204 | obj = value(f); 205 | %% 206 | sol_refine = [x_pole(:); x_cart(:); y_pole(:); c_pole(:); s_pole(:);... 207 | vx_pole(:); vx_cart(:); vy_pole(:); ca_pole(:); sa_pole(:);... 208 | lam_x(:); lam_y(:); tau(:)]; 209 | end -------------------------------------------------------------------------------- /Cart-Pole/arm2/deg45.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Cart-Pole/arm2/deg45.mat -------------------------------------------------------------------------------- /Cart-Pole/arm2/deg45_refine.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Cart-Pole/arm2/deg45_refine.mat -------------------------------------------------------------------------------- /Cart-Pole/arm2/multi-cart-2.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "id": "21e7bfcc", 7 | "metadata": {}, 8 | "outputs": [ 9 | { 10 | "data": { 11 | "text/plain": [ 12 | "Main.Control" 13 | ] 14 | }, 15 | "execution_count": 1, 16 | "metadata": {}, 17 | "output_type": "execute_result" 18 | } 19 | ], 20 | "source": [ 21 | "using DynamicPolynomials\n", 22 | "using TSLTSSOS\n", 23 | "using MAT\n", 24 | "using JuMP\n", 25 | "include(\"./Control_utils.jl\")" 26 | ] 27 | }, 28 | { 29 | "cell_type": "code", 30 | "execution_count": 2, 31 | "id": "e131a844", 32 | "metadata": {}, 33 | "outputs": [ 34 | { 35 | "data": { 36 | "text/plain": [ 37 | "CartPoleTO (generic function with 1 method)" 38 | ] 39 | }, 40 | "execution_count": 2, 41 | "metadata": {}, 42 | "output_type": "execute_result" 43 | } 44 | ], 45 | "source": [ 46 | "function CartPoleTO(cartpole_param)\n", 47 | " ## Double inverted pendulum # nearly optimal!\n", 48 | " Ns = cartpole_param.control_param.Ns # 80\n", 49 | " dof = 2\n", 50 | "\n", 51 | " lx = cartpole_param.lx\n", 52 | " ly = cartpole_param.ly\n", 53 | "\n", 54 | " dt = cartpole_param.control_param.dt\n", 55 | " m = cartpole_param.mass\n", 56 | " I = cartpole_param.inertial\n", 57 | " g = -9.81\n", 58 | "\n", 59 | " xc_0 = cartpole_param.xc_0\n", 60 | " yc_0 = 0.0\n", 61 | "\n", 62 | " t0 = cartpole_param.theta_0\n", 63 | "\n", 64 | " @polyvar x[1:dof+1, 1:Ns+1]\n", 65 | " @polyvar y[1:dof, 1:Ns+1]\n", 66 | " @polyvar c[1:dof, 1:Ns+1]\n", 67 | " @polyvar s[1:dof, 1:Ns+1]\n", 68 | "\n", 69 | " @polyvar vx[1:dof+1, 1:Ns+1]\n", 70 | " @polyvar vy[1:dof, 1:Ns+1]\n", 71 | " @polyvar ca[1:dof, 1:Ns+1]\n", 72 | " @polyvar sa[1:dof, 1:Ns+1]\n", 73 | "\n", 74 | " @polyvar lam_x[1:dof, 1:Ns] # serial manipulator\n", 75 | " @polyvar lam_y[1:dof, 1:Ns]\n", 76 | " @polyvar tau[1:Ns]\n", 77 | "\n", 78 | "\n", 79 | " ns = dof * (Ns+1)\n", 80 | " nf = Ns\n", 81 | " nlam = dof * Ns\n", 82 | " var = append!( reshape(x, ns + Ns + 1), reshape(y, ns), reshape(c, ns), reshape(s, ns),\n", 83 | " reshape(vx, ns + Ns + 1), reshape(vy, ns), reshape(ca, ns), reshape(sa, ns),\n", 84 | " reshape(lam_x, nlam), reshape(lam_y, nlam), reshape(tau, nf))\n", 85 | "\n", 86 | " eq = []\n", 87 | " ineq = []\n", 88 | "\n", 89 | " f = 0\n", 90 | "\n", 91 | " # cart states\n", 92 | " append!( eq, [ x[end, 1] - xc_0])\n", 93 | " append!( eq, [vx[end, 1] - 0.0])\n", 94 | "\n", 95 | " # pole states\n", 96 | " for k in 1:dof\n", 97 | " append!(eq, [c[k, 1] - cos(t0[k])])\n", 98 | " append!(eq, [s[k, 1] - sin(t0[k])])\n", 99 | " append!(eq, [vx[k, 1] - 0.0])\n", 100 | " append!(eq, [vy[k, 1] - 0.0])\n", 101 | " append!(eq, [ca[k, 1] - 1.0])\n", 102 | " append!(eq, [sa[k, 1] - 0.0])\n", 103 | "\n", 104 | " if k == 1\n", 105 | " append!(eq, [cos(t0[k]) * lx - sin(t0[k]) * ly + x[k, 1] - x[end, 1] ])\n", 106 | " append!(eq, [sin(t0[k]) * lx + cos(t0[k]) * ly + y[k, 1] - 0 ])\n", 107 | " else\n", 108 | " append!(eq, [cos(t0[k]) * lx - sin(t0[k]) * ly + x[k, 1] - x[k-1, 1]])\n", 109 | " append!(eq, [sin(t0[k]) * lx + cos(t0[k]) * ly + y[k, 1] - y[k-1, 1]])\n", 110 | " end\n", 111 | " end\n", 112 | "\n", 113 | " for k in 1:Ns\n", 114 | " \n", 115 | " ######## Cart Start ########\n", 116 | " append!( eq, [x[end, k+1] - (x[end, k] + dt * vx[end, k])] )\n", 117 | " append!( eq, [m * vx[end, k+1] - m * vx[end, k] - (lam_x[1, k] + tau[k])* dt ] )\n", 118 | " ######## Cart End ########\n", 119 | "\n", 120 | " for j in 1:dof\n", 121 | "\n", 122 | " ######## kinematics ########\n", 123 | " append!( eq, [c[j, k+1] - (c[j, k] * ca[j, k] - s[j, k] * sa[j, k])] )\n", 124 | " append!( eq, [s[j, k+1] - (s[j, k] * ca[j, k] + c[j, k] * sa[j, k])] )\n", 125 | " append!( eq, [x[j, k+1] - (dt * (c[j, k] * vx[j, k] - s[j, k] * vy[j, k]) + x[j, k])] )\n", 126 | " append!( eq, [y[j, k+1] - (dt * (s[j, k] * vx[j, k] + c[j, k] * vy[j, k]) + y[j, k])] )\n", 127 | " \n", 128 | " ## sin cos constraints\n", 129 | " append!( eq, [ s[j,k+1]^2 + c[j,k+1]^2 - 1.0 ])\n", 130 | " append!( eq, [sa[j,k+1]^2 + ca[j,k+1]^2 - 1.0 ])\n", 131 | "\n", 132 | " ######## kinematics constraints ########\n", 133 | " if j == 1\n", 134 | " append!( eq, [c[j,k+1] * lx - s[j,k+1] * ly + x[j, k+1] - x[end, k+1] ])\n", 135 | " append!( eq, [s[j,k+1] * lx + c[j,k+1] * ly + y[j, k+1] - 0 ])\n", 136 | " else\n", 137 | " append!( eq, [c[j,k+1] * lx - s[j,k+1] * ly + x[j, k+1] - x[j-1, k+1] ])\n", 138 | " append!( eq, [s[j,k+1] * lx + c[j,k+1] * ly + y[j, k+1] - y[j-1, k+1] ])\n", 139 | " end\n", 140 | "\n", 141 | " ######## propagation of sdynamics ########\n", 142 | " ## rotation\n", 143 | " Jx1 = (-c[j, k+1] * ly - s[j, k+1] * lx)\n", 144 | " Jy1 = (-s[j, k+1] * ly + c[j, k+1] * lx)\n", 145 | " append!( eq, [(sa[j, k+1] - sa[j, k]) * I - (Jx1 * lam_x[j, k] + Jy1 * lam_y[j, k]) * dt^2] )\n", 146 | " \n", 147 | " ## position\n", 148 | " Fx1 = c[j, k+1]*lam_x[j, k] + s[j, k+1]*lam_y[j, k]\n", 149 | " Fy1 = - s[j, k+1]*lam_x[j, k] + c[j, k+1]*lam_y[j, k]\n", 150 | "\n", 151 | " if j < dof\n", 152 | " Fx2 = c[j, k+1]*lam_x[j+1, k] + s[j+1, k+1]*lam_y[j, k]\n", 153 | " Fy2 = - s[j, k+1]*lam_x[j+1, k] + c[j+1, k+1]*lam_y[j, k]\n", 154 | " else\n", 155 | " Fx2 = 0.\n", 156 | " Fy2 = 0.\n", 157 | " end\n", 158 | " \n", 159 | " append!( eq, [m * vx[j, k+1] - m * ( ca[j, k] * vx[j, k] + sa[j, k] * vy[j, k] ) - (Fx1 + Fx2 + m * s[1, k+1] * g)* dt ] )\n", 160 | " append!( eq, [m * vy[j, k+1] - m * (-sa[j, k] * vx[j, k] + ca[j, k] * vy[j, k] ) - (Fy1 + Fy2 + m * c[1, k+1] * g)* dt ] )\n", 161 | "\n", 162 | " f = f + ( x[j, k]^2 + (y[j, k] - j * abs(ly))^2 + (c[j, k] - 1)^2 + s[j, k]^2) * cartpole_param.run_cost[1]\n", 163 | " f = f + (vx[j, k]^2 + vx[j, k]^2 + vy[j, k]^2 + (ca[j, k] - 1)^2 + sa[j, k]^2) * cartpole_param.run_cost[1]\n", 164 | " f = f + cartpole_param.run_cost[1] * tau[k]^2\n", 165 | " end\n", 166 | "\n", 167 | " f = f + ( x[end, k]^2 + vx[end, k]^2) * cartpole_param.run_cost[1]\n", 168 | " end\n", 169 | "\n", 170 | " # println(\"diao\")\n", 171 | " p_goal = [0; dof]\n", 172 | " \n", 173 | " if cartpole_param.terminal_cons\n", 174 | " # cart states\n", 175 | " append!( eq, [ x[2, 1] - 0.0])\n", 176 | " append!( eq, [vx[2, 1] - 0.0])\n", 177 | "\n", 178 | " # pole states\n", 179 | " for k in 1:dof\n", 180 | " append!(eq, [ c[k, 1] - cos(t0[k])])\n", 181 | " append!(eq, [ s[k, 1] - sin(t0[k])])\n", 182 | " append!(eq, [vx[k, 1] - 0.0])\n", 183 | " append!(eq, [vy[k, 1] - 0.0])\n", 184 | " append!(eq, [ca[k, 1] - 1.0])\n", 185 | " append!(eq, [sa[k, 1] - 0.0])\n", 186 | "\n", 187 | " if k == 1\n", 188 | " append!(eq, [cos(0.) * lx - sin(0.) * ly + x[k, 1] - x[end, 1] ])\n", 189 | " append!(eq, [sin(0.) * lx + cos(0.) * ly + y[k, 1] - 0 ])\n", 190 | " else\n", 191 | " append!(eq, [cos(0.) * lx - sin(0.) * ly + x[k, 1] - x[k-1, 1]])\n", 192 | " append!(eq, [sin(0.) * lx + cos(0.) * ly + y[k, 1] - y[k-1, 1]])\n", 193 | " end\n", 194 | " end\n", 195 | " else\n", 196 | " f = f + ( x[end, end]^2 + vx[end, end]^2) * cartpole_param.run_cost[1]\n", 197 | " for j in 1:dof\n", 198 | " f = f + ( x[j, end]^2 + (y[j, end] - j * abs(ly))^2 + (c[j, end] - 1)^2 + s[j, end]^2) * cartpole_param.run_cost[1]\n", 199 | " f = f + (vx[j, end]^2 + vx[j, end]^2 + vy[j, end]^2 + (ca[j, end] - 1)^2 + sa[j, end]^2) * cartpole_param.run_cost[1]\n", 200 | " end\n", 201 | " end\n", 202 | " \n", 203 | " # for k = 1:length(var)-Ns\n", 204 | " # append!( ineq, [100 - var[k].^2 ])\n", 205 | " # append!( ineq, [100 - var[k] ])\n", 206 | " # append!( ineq, [100 + var[k]])\n", 207 | " # end\n", 208 | "\n", 209 | " for k = 1:Ns+1\n", 210 | " append!( ineq, [0 + ca[1, k] ])\n", 211 | " end\n", 212 | "\n", 213 | " pop = append!([f], ineq, eq)\n", 214 | "\n", 215 | " order = 2\n", 216 | "\n", 217 | " eq_len = length(eq)\n", 218 | "\n", 219 | " time_start = time()\n", 220 | " opt,sol,data,model,sol_approx =cs_tssos_first_export_tsl(pop, var, order, numeq=eq_len, CS = \"MF\", TS = false, solution=true, QUIET=false, MomentOne=false)\n", 221 | " time_end = time()\n", 222 | " \n", 223 | " elapsed = time_end - time_start\n", 224 | " \n", 225 | " flag = primal_status(model)\n", 226 | " flag = string(flag)\n", 227 | " \n", 228 | " flag2 = termination_status(model)\n", 229 | " flag2 = string(flag2)\n", 230 | "\n", 231 | " \n", 232 | " moment = []\n", 233 | " for k = 1:length(data.Mmatrix)\n", 234 | " append!(moment, [convert(Matrix{Float64}, data.Mmatrix[k])]) # data.Mmatrix[k])\n", 235 | " end\n", 236 | "\n", 237 | " return Dict(\"sol\" => sol, \"sol_approx\" => sol_approx,\n", 238 | " \"Ns\" => Ns, \"dof\" => dof, \"Ns\" => Ns, \"moment\" => moment,\n", 239 | " \"opt\" => opt, \"param\" => cartpole_param, \"elapsed\" => elapsed,\n", 240 | " \"primal\" => flag, \"termination\" => flag2)\n", 241 | " \n", 242 | "end" 243 | ] 244 | }, 245 | { 246 | "cell_type": "code", 247 | "execution_count": 6, 248 | "id": "a849906e", 249 | "metadata": {}, 250 | "outputs": [ 251 | { 252 | "name": "stdout", 253 | "output_type": "stream", 254 | "text": [ 255 | "*********************************** TSSOS ***********************************\n", 256 | "Version 1.0.0, developed by Jie Wang, 2020--2022\n", 257 | "TSSOS is launching...\n", 258 | "-----------------------------------------------------------------------------\n", 259 | "The clique sizes of varibles:\n", 260 | "[25, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 2]\n", 261 | "[9, 18, 18, 9, 16, 9, 1, 2, 5, 1, 19, 47, 29, 19, 4, 3, 9, 3, 12, 2, 2, 21, 2]\n", 262 | "-----------------------------------------------------------------------------\n", 263 | "Obtained the variable cliques in 0.404388 seconds. The maximal size of cliques is 25.\n", 264 | "Assembling the SDP...\n", 265 | "There are 664108 affine constraints.\n", 266 | "SDP assembling time: 9.8136156 seconds.\n", 267 | "Solving the SDP...\n", 268 | "Problem\n", 269 | " Name : \n", 270 | " Objective sense : max \n", 271 | " Type : CONIC (conic optimization problem)\n", 272 | " Constraints : 664108 \n", 273 | " Cones : 0 \n", 274 | " Scalar variables : 63504 \n", 275 | " Matrix variables : 1197 \n", 276 | " Integer variables : 0 \n", 277 | "\n", 278 | "Optimizer started.\n", 279 | "Presolve started.\n", 280 | "Linear dependency checker started.\n", 281 | "Linear dependency checker terminated.\n", 282 | "Eliminator started.\n", 283 | "Freed constraints in eliminator : 0\n", 284 | "Eliminator terminated.\n", 285 | "Eliminator - tries : 1 time : 0.00 \n", 286 | "Lin. dep. - tries : 1 time : 0.06 \n", 287 | "Lin. dep. - number : 0 \n", 288 | "Presolve terminated. Time: 0.16 \n", 289 | "GP based matrix reordering started.\n", 290 | "GP based matrix reordering terminated.\n", 291 | "Problem\n", 292 | " Name : \n", 293 | " Objective sense : max \n", 294 | " Type : CONIC (conic optimization problem)\n", 295 | " Constraints : 664108 \n", 296 | " Cones : 0 \n", 297 | " Scalar variables : 63504 \n", 298 | " Matrix variables : 1197 \n", 299 | " Integer variables : 0 \n", 300 | "\n", 301 | "Optimizer - threads : 8 \n", 302 | "Optimizer - solved problem : the primal \n", 303 | "Optimizer - Constraints : 664108\n", 304 | "Optimizer - Cones : 1\n", 305 | "Optimizer - Scalar variables : 63505 conic : 63505 \n", 306 | "Optimizer - Semi-definite variables: 1197 scalarized : 4607646 \n", 307 | "Factor - setup time : 12019.53 dense det. time : 1226.30 \n", 308 | "Factor - ML order time : 236.72 GP order time : 9490.53 \n", 309 | "Factor - nonzeros before factor : 7.77e+09 after factor : 9.55e+09 \n", 310 | "Factor - dense dim. : 2 flops : 1.67e+14 \n", 311 | "Factor - GP saved nzs : 8.50e+10 GP saved flops : 2.60e+16 \n", 312 | "ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME \n", 313 | "0 7.2e+02 1.0e+00 1.0e+00 0.00e+00 0.000000000e+00 0.000000000e+00 1.0e+00 12025.34\n", 314 | "1 7.1e+02 9.8e-01 9.8e-01 3.96e-01 9.865965192e-02 9.654829662e-02 9.8e-01 13145.14\n", 315 | "2 7.0e+02 9.7e-01 9.9e-01 -3.29e-01 8.178901605e-01 7.871902866e-01 9.7e-01 14275.59\n", 316 | "3 5.7e+02 8.0e-01 9.0e-01 -6.88e-01 4.924901961e+00 4.685806767e+00 8.0e-01 15408.22\n", 317 | "4 3.5e+02 4.8e-01 6.6e-01 -8.59e-01 1.553054799e+01 1.464307930e+01 4.8e-01 16559.67\n", 318 | "5 1.4e+02 1.9e-01 4.1e-01 -9.40e-01 6.636365341e+01 6.269402159e+01 1.9e-01 17719.06\n", 319 | "6 7.5e+01 1.0e-01 2.6e-01 -8.38e-01 1.062884552e+02 1.007203428e+02 1.0e-01 18885.20\n", 320 | "7 4.3e+01 6.0e-02 1.9e-01 -7.00e-01 1.841814466e+02 1.747069963e+02 6.0e-02 20022.19\n", 321 | "8 2.2e+01 3.1e-02 1.2e-01 -5.75e-01 3.024184702e+02 2.893938332e+02 3.1e-02 21212.28\n", 322 | "9 1.2e+01 1.7e-02 6.5e-02 -3.89e-01 4.728213013e+02 4.580369557e+02 1.7e-02 22449.94\n", 323 | "10 7.6e+00 1.1e-02 4.0e-02 -9.89e-02 6.173800906e+02 6.036469273e+02 1.1e-02 23589.59\n", 324 | "11 4.9e+00 6.8e-03 2.3e-02 1.23e-01 7.639659505e+02 7.523493195e+02 6.8e-03 24727.45\n", 325 | "12 1.8e+00 2.6e-03 6.8e-03 3.27e-01 1.060029610e+03 1.053106394e+03 2.6e-03 25888.64\n", 326 | "13 9.6e-01 1.3e-03 2.9e-03 6.28e-01 1.263997721e+03 1.259504056e+03 1.3e-03 27029.97\n", 327 | "14 8.1e-01 1.1e-03 2.3e-03 7.15e-01 1.291759896e+03 1.287699229e+03 1.1e-03 28178.24\n", 328 | "15 3.0e-01 4.2e-04 5.9e-04 7.35e-01 1.413064401e+03 1.411070655e+03 4.2e-04 29334.34\n", 329 | "16 1.6e-01 2.3e-04 2.6e-04 7.70e-01 1.387664048e+03 1.386337771e+03 2.3e-04 30481.49\n", 330 | "17 1.3e-01 1.8e-04 1.9e-04 7.75e-01 1.350930842e+03 1.349794944e+03 1.8e-04 31620.52\n", 331 | "18 4.6e-02 6.4e-05 4.7e-05 7.85e-01 1.186591573e+03 1.186059373e+03 6.4e-05 32771.02\n", 332 | "19 2.1e-02 3.0e-05 1.6e-05 8.34e-01 9.932973813e+02 9.930115037e+02 3.0e-05 33925.61\n", 333 | "20 1.1e-02 1.6e-05 6.3e-06 9.04e-01 8.214515187e+02 8.212919908e+02 1.6e-05 35069.14\n", 334 | "21 7.1e-03 1.0e-05 3.2e-06 9.64e-01 7.069841913e+02 7.068805498e+02 1.0e-05 36209.99\n", 335 | "22 3.0e-03 4.2e-06 8.8e-07 1.01e+00 5.441910921e+02 5.441481794e+02 4.2e-06 37362.80\n", 336 | "23 1.3e-03 1.8e-06 2.3e-07 1.09e+00 4.089511150e+02 4.089341125e+02 1.8e-06 38516.24\n", 337 | "24 6.7e-04 9.4e-07 9.3e-08 1.02e+00 3.433948176e+02 3.433849946e+02 9.4e-07 39660.13\n", 338 | "25 3.2e-04 4.5e-07 3.5e-08 8.59e-01 3.021087147e+02 3.021027288e+02 4.5e-07 40805.70\n", 339 | "26 1.9e-04 2.7e-07 1.8e-08 6.87e-01 2.823854191e+02 2.823809255e+02 2.7e-07 42086.33\n", 340 | "27 1.0e-04 1.4e-07 8.1e-09 6.06e-01 2.647281456e+02 2.647249167e+02 1.4e-07 43368.11\n", 341 | "28 5.4e-05 7.6e-08 3.7e-09 5.42e-01 2.521585833e+02 2.521562042e+02 7.6e-08 44502.53\n", 342 | "29 3.0e-05 4.2e-08 1.8e-09 5.03e-01 2.434975105e+02 2.434957025e+02 4.2e-08 45637.92\n", 343 | "30 1.3e-05 1.9e-08 6.5e-10 4.80e-01 2.350321099e+02 2.350308501e+02 1.8e-08 46770.38\n", 344 | "31 7.0e-06 1.1e-08 3.0e-10 4.66e-01 2.300988457e+02 2.300978862e+02 9.7e-09 47932.30\n", 345 | "32 5.2e-06 7.7e-09 1.5e-10 8.59e-01 2.285844685e+02 2.285838513e+02 5.9e-09 53255.00\n", 346 | "33 5.2e-06 7.7e-09 1.5e-10 9.02e-01 2.285844685e+02 2.285838513e+02 5.9e-09 54410.34\n", 347 | "34 5.2e-06 7.7e-09 1.5e-10 9.93e-01 2.285844685e+02 2.285838513e+02 5.9e-09 55570.41\n", 348 | "35 5.2e-06 7.7e-09 1.5e-10 1.00e+00 2.285959480e+02 2.285953327e+02 5.9e-09 56732.94\n", 349 | "36 5.2e-06 7.6e-09 1.4e-10 1.00e+00 2.286019439e+02 2.286013296e+02 5.9e-09 57890.24\n", 350 | "37 5.2e-06 7.6e-09 1.4e-10 1.00e+00 2.286050003e+02 2.286043865e+02 5.9e-09 59050.22\n", 351 | "38 5.2e-06 7.6e-09 1.4e-10 1.00e+00 2.286053859e+02 2.286047722e+02 5.9e-09 60208.16\n", 352 | "Optimizer terminated. Time: 61371.55\n", 353 | "\n", 354 | "SDP solving time: 61468.7768751 seconds.\n", 355 | "termination status: SLOW_PROGRESS\n", 356 | "solution status: UNKNOWN_RESULT_STATUS\n", 357 | "TSL!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n", 358 | "optimum = 228.60538591573945\n", 359 | "TSL!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n", 360 | "The local solver failed!\n", 361 | "*********************************** TSSOS ***********************************\n", 362 | "Version 1.0.0, developed by Jie Wang, 2020--2022\n", 363 | "TSSOS is launching...\n", 364 | "-----------------------------------------------------------------------------\n", 365 | "The clique sizes of varibles:\n", 366 | "[25, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 2]\n", 367 | "[9, 18, 18, 9, 16, 9, 1, 2, 5, 1, 19, 47, 29, 19, 4, 3, 9, 3, 12, 2, 2, 21, 2]\n", 368 | "-----------------------------------------------------------------------------\n", 369 | "Obtained the variable cliques in 1.0795108 seconds. The maximal size of cliques is 25.\n", 370 | "Assembling the SDP...\n", 371 | "There are 664108 affine constraints.\n", 372 | "SDP assembling time: 11.0404639 seconds.\n", 373 | "Solving the SDP...\n", 374 | "Problem\n", 375 | " Name : \n", 376 | " Objective sense : max \n", 377 | " Type : CONIC (conic optimization problem)\n", 378 | " Constraints : 664108 \n", 379 | " Cones : 0 \n", 380 | " Scalar variables : 63504 \n", 381 | " Matrix variables : 1197 \n", 382 | " Integer variables : 0 \n", 383 | "\n", 384 | "Optimizer started.\n", 385 | "Presolve started.\n", 386 | "Linear dependency checker started.\n", 387 | "Linear dependency checker terminated.\n", 388 | "Eliminator started.\n", 389 | "Freed constraints in eliminator : 0\n", 390 | "Eliminator terminated.\n", 391 | "Eliminator - tries : 1 time : 0.00 \n", 392 | "Lin. dep. - tries : 1 time : 0.13 \n", 393 | "Lin. dep. - number : 0 \n", 394 | "Presolve terminated. Time: 0.28 \n", 395 | "GP based matrix reordering started.\n", 396 | "GP based matrix reordering terminated.\n", 397 | "Problem\n", 398 | " Name : \n", 399 | " Objective sense : max \n", 400 | " Type : CONIC (conic optimization problem)\n", 401 | " Constraints : 664108 \n", 402 | " Cones : 0 \n", 403 | " Scalar variables : 63504 \n", 404 | " Matrix variables : 1197 \n", 405 | " Integer variables : 0 \n", 406 | "\n", 407 | "Optimizer - threads : 8 \n", 408 | "Optimizer - solved problem : the primal \n", 409 | "Optimizer - Constraints : 664108\n", 410 | "Optimizer - Cones : 1\n", 411 | "Optimizer - Scalar variables : 63505 conic : 63505 \n", 412 | "Optimizer - Semi-definite variables: 1197 scalarized : 4607646 \n", 413 | "Factor - setup time : 11617.67 dense det. time : 1211.28 \n", 414 | "Factor - ML order time : 222.89 GP order time : 9155.94 \n", 415 | "Factor - nonzeros before factor : 7.77e+09 after factor : 9.55e+09 \n", 416 | "Factor - dense dim. : 2 flops : 1.67e+14 \n", 417 | "Factor - GP saved nzs : 8.50e+10 GP saved flops : 2.60e+16 \n", 418 | "ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME \n", 419 | "0 7.2e+02 1.0e+00 1.0e+00 0.00e+00 0.000000000e+00 0.000000000e+00 1.0e+00 11622.44\n", 420 | "1 7.1e+02 9.8e-01 9.8e-01 3.83e-01 1.008639338e-01 9.869382156e-02 9.8e-01 12752.66\n", 421 | "2 7.0e+02 9.7e-01 9.9e-01 -3.63e-01 8.070465130e-01 7.726355881e-01 9.7e-01 13887.08\n", 422 | "3 5.7e+02 7.9e-01 9.0e-01 -7.22e-01 3.757147164e+00 3.504083517e+00 7.9e-01 15022.80\n", 423 | "4 3.3e+02 4.6e-01 6.5e-01 -8.83e-01 1.159395662e+01 1.059390532e+01 4.6e-01 16158.97\n", 424 | "5 1.4e+02 1.9e-01 4.1e-01 -9.47e-01 3.775111442e+01 3.406790691e+01 1.9e-01 17294.59\n", 425 | "6 1.0e+02 1.4e-01 3.3e-01 -8.99e-01 5.292467627e+01 4.853428001e+01 1.4e-01 18431.50\n", 426 | "7 6.4e+01 9.0e-02 2.5e-01 -7.99e-01 8.600717856e+01 7.906837926e+01 9.0e-02 19566.25\n", 427 | "8 2.5e+01 3.5e-02 1.2e-01 -6.08e-01 1.880757425e+02 1.768070916e+02 3.5e-02 20726.75\n", 428 | "9 1.5e+01 2.1e-02 7.2e-02 -2.92e-01 2.914039752e+02 2.797600017e+02 2.1e-02 21871.36\n", 429 | "10 5.4e+00 7.6e-03 2.3e-02 -3.16e-02 5.754011013e+02 5.667336977e+02 7.6e-03 23065.74\n", 430 | "11 3.5e+00 4.8e-03 1.2e-02 5.04e-01 6.604526329e+02 6.542011120e+02 4.8e-03 24199.36\n", 431 | "12 1.8e+00 2.5e-03 4.9e-03 6.47e-01 7.245314465e+02 7.207246784e+02 2.5e-03 25339.64\n", 432 | "13 1.3e+00 1.8e-03 3.3e-03 7.54e-01 7.218573972e+02 7.187838738e+02 1.8e-03 26479.00\n", 433 | "14 9.5e-01 1.3e-03 2.1e-03 7.84e-01 7.184409944e+02 7.160337497e+02 1.3e-03 27615.20\n", 434 | "15 7.1e-01 9.9e-04 1.4e-03 8.12e-01 7.161731064e+02 7.142374771e+02 9.9e-04 28748.92\n", 435 | "16 3.7e-01 5.2e-04 5.7e-04 8.33e-01 7.040807627e+02 7.029182341e+02 5.2e-04 29885.55\n", 436 | "17 2.7e-01 3.7e-04 3.5e-04 8.77e-01 6.734118651e+02 6.725299419e+02 3.7e-04 31028.74\n", 437 | "18 1.5e-01 2.0e-04 1.5e-04 8.99e-01 6.106309359e+02 6.101056276e+02 2.0e-04 32170.00\n", 438 | "19 1.1e-01 1.6e-04 1.0e-04 9.38e-01 5.715431734e+02 5.711304389e+02 1.6e-04 33310.56\n", 439 | "20 4.2e-02 5.8e-05 2.4e-05 9.58e-01 4.702083456e+02 4.700446101e+02 5.8e-05 34456.42\n", 440 | "21 2.4e-02 3.3e-05 9.8e-06 1.07e+00 3.828017570e+02 3.827149968e+02 3.3e-05 35596.34\n", 441 | "22 8.7e-03 1.2e-05 1.9e-06 1.17e+00 2.674635360e+02 2.674391921e+02 1.2e-05 36753.02\n", 442 | "23 3.2e-03 4.5e-06 3.8e-07 1.33e+00 1.697221435e+02 1.697152541e+02 4.5e-06 37904.08\n", 443 | "24 2.1e-03 3.0e-06 2.1e-07 1.09e+00 1.472903768e+02 1.472854124e+02 3.0e-06 39047.88\n", 444 | "25 8.6e-04 1.2e-06 6.2e-08 9.45e-01 1.206650711e+02 1.206624961e+02 1.2e-06 40195.49\n", 445 | "26 5.5e-04 7.6e-07 3.4e-08 7.28e-01 1.126271784e+02 1.126252359e+02 7.6e-07 41336.78\n", 446 | "27 2.9e-04 4.1e-07 1.5e-08 6.73e-01 1.050150572e+02 1.050137290e+02 4.1e-07 42475.36\n", 447 | "28 2.0e-04 2.8e-07 9.4e-09 6.08e-01 1.018604139e+02 1.018593305e+02 2.8e-07 43653.52\n", 448 | "29 9.7e-05 1.4e-07 3.7e-09 5.74e-01 9.721945081e+01 9.721871651e+01 1.4e-07 44861.47\n", 449 | "30 6.7e-05 9.4e-08 2.3e-09 5.16e-01 9.569172312e+01 9.569110365e+01 9.4e-08 47209.42\n", 450 | "31 2.6e-05 3.7e-08 7.7e-10 4.72e-01 9.304740623e+01 9.304697580e+01 3.7e-08 48350.74\n", 451 | "32 1.8e-05 2.6e-08 4.9e-10 4.06e-01 9.214071155e+01 9.214033767e+01 2.5e-08 49483.83\n", 452 | "33 1.2e-05 1.9e-08 3.1e-10 4.10e-01 9.137861321e+01 9.137829015e+01 1.8e-08 50618.80\n", 453 | "34 7.0e-06 1.3e-08 1.4e-10 4.28e-01 9.024694501e+01 9.024670286e+01 9.2e-09 51764.11\n", 454 | "35 7.0e-06 1.3e-08 1.4e-10 5.48e-01 9.024694501e+01 9.024670286e+01 9.2e-09 55102.33\n", 455 | "36 4.3e-06 8.3e-09 7.3e-11 9.36e-01 9.040633850e+01 9.040617663e+01 5.9e-09 56304.22\n", 456 | "37 3.2e-06 1.1e-08 4.9e-11 9.45e-01 9.081415111e+01 9.081402426e+01 4.6e-09 57488.33\n", 457 | "38 3.1e-06 6.1e-09 4.2e-11 9.48e-01 9.103092273e+01 9.103080621e+01 4.3e-09 58652.28\n", 458 | "39 3.1e-06 6.1e-09 4.2e-11 9.23e-01 9.103092273e+01 9.103080621e+01 4.3e-09 59889.61\n", 459 | "40 3.1e-06 6.1e-09 4.2e-11 9.99e-01 9.103092273e+01 9.103080621e+01 4.3e-09 61147.39\n", 460 | "Optimizer terminated. Time: 62389.70\n", 461 | "\n", 462 | "SDP solving time: 62476.0058013 seconds.\n", 463 | "termination status: SLOW_PROGRESS\n", 464 | "solution status: UNKNOWN_RESULT_STATUS\n", 465 | "TSL!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n", 466 | "optimum = 91.03092273428169\n", 467 | "TSL!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n", 468 | "The local solver failed!\n" 469 | ] 470 | } 471 | ], 472 | "source": [ 473 | "# mpc_param = Control.param_base(0.125, 8, 1)\n", 474 | "mpc_param = Control.param_base(0.25, 20, 1)\n", 475 | "weights = ones(11, 1)\n", 476 | "\n", 477 | "dof = 2\n", 478 | "t0 = Matrix{Float64}(undef, dof, 1)\n", 479 | "t0[:] .= pi / 4\n", 480 | "\n", 481 | "for m = [0.5, 0.25]\n", 482 | " cartpole_param = Control.param_cart_multi(mpc_param, m, m, 0.0, -m, weights, 200 * weights, false, 0, t0, dof)\n", 483 | " log = CartPoleTO(cartpole_param)\n", 484 | "\n", 485 | " if m == 0.5\n", 486 | " matwrite(\"log_050_eq.mat\", log)\n", 487 | " end\n", 488 | "\n", 489 | " if m == 0.25\n", 490 | " matwrite(\"log_025_eq.mat\", log)\n", 491 | " end\n", 492 | "end\n" 493 | ] 494 | }, 495 | { 496 | "cell_type": "code", 497 | "execution_count": 7, 498 | "id": "aaf7199f", 499 | "metadata": {}, 500 | "outputs": [ 501 | { 502 | "name": "stdout", 503 | "output_type": "stream", 504 | "text": [ 505 | "*********************************** TSSOS ***********************************\n", 506 | "Version 1.0.0, developed by Jie Wang, 2020--2022\n", 507 | "TSSOS is launching...\n", 508 | "-----------------------------------------------------------------------------\n", 509 | "The clique sizes of varibles:\n", 510 | "[25, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 2]\n", 511 | "[9, 18, 18, 9, 16, 9, 1, 2, 5, 1, 19, 47, 29, 19, 4, 3, 9, 3, 12, 2, 2, 21, 2]\n", 512 | "-----------------------------------------------------------------------------\n", 513 | "Obtained the variable cliques in 0.4196878 seconds. The maximal size of cliques is 25.\n", 514 | "Assembling the SDP...\n", 515 | "There are 664108 affine constraints.\n", 516 | "SDP assembling time: 8.9717231 seconds.\n", 517 | "Solving the SDP...\n", 518 | "Problem\n", 519 | " Name : \n", 520 | " Objective sense : max \n", 521 | " Type : CONIC (conic optimization problem)\n", 522 | " Constraints : 664108 \n", 523 | " Cones : 0 \n", 524 | " Scalar variables : 63504 \n", 525 | " Matrix variables : 1197 \n", 526 | " Integer variables : 0 \n", 527 | "\n", 528 | "Optimizer started.\n", 529 | "Presolve started.\n", 530 | "Linear dependency checker started.\n", 531 | "Linear dependency checker terminated.\n", 532 | "Eliminator started.\n", 533 | "Freed constraints in eliminator : 0\n", 534 | "Eliminator terminated.\n", 535 | "Eliminator - tries : 1 time : 0.00 \n", 536 | "Lin. dep. - tries : 1 time : 0.06 \n", 537 | "Lin. dep. - number : 0 \n", 538 | "Presolve terminated. Time: 0.14 \n", 539 | "GP based matrix reordering started.\n", 540 | "GP based matrix reordering terminated.\n", 541 | "Problem\n", 542 | " Name : \n", 543 | " Objective sense : max \n", 544 | " Type : CONIC (conic optimization problem)\n", 545 | " Constraints : 664108 \n", 546 | " Cones : 0 \n", 547 | " Scalar variables : 63504 \n", 548 | " Matrix variables : 1197 \n", 549 | " Integer variables : 0 \n", 550 | "\n", 551 | "Optimizer - threads : 8 \n", 552 | "Optimizer - solved problem : the primal \n", 553 | "Optimizer - Constraints : 664108\n", 554 | "Optimizer - Cones : 1\n", 555 | "Optimizer - Scalar variables : 63505 conic : 63505 \n", 556 | "Optimizer - Semi-definite variables: 1197 scalarized : 4607646 \n", 557 | "Factor - setup time : 10468.11 dense det. time : 1162.09 \n", 558 | "Factor - ML order time : 238.16 GP order time : 8061.55 \n", 559 | "Factor - nonzeros before factor : 7.77e+09 after factor : 9.55e+09 \n", 560 | "Factor - dense dim. : 2 flops : 1.67e+14 \n", 561 | "Factor - GP saved nzs : 8.50e+10 GP saved flops : 2.60e+16 \n", 562 | "ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME \n", 563 | "0 7.2e+02 1.0e+00 1.0e+00 0.00e+00 0.000000000e+00 0.000000000e+00 1.0e+00 10473.02\n", 564 | "1 7.1e+02 9.8e-01 9.8e-01 3.90e-01 9.965526176e-02 9.750850842e-02 9.8e-01 11607.36\n", 565 | "2 7.0e+02 9.7e-01 9.9e-01 -3.41e-01 8.278843614e-01 7.957626371e-01 9.7e-01 12748.30\n", 566 | "3 5.7e+02 8.0e-01 9.0e-01 -6.99e-01 4.633576345e+00 4.389321081e+00 8.0e-01 13889.95\n", 567 | "4 3.4e+02 4.7e-01 6.6e-01 -8.67e-01 1.461588624e+01 1.367837566e+01 4.7e-01 15031.61\n", 568 | "5 1.5e+02 2.0e-01 4.2e-01 -9.43e-01 5.007336766e+01 4.669655464e+01 2.0e-01 16177.55\n", 569 | "6 1.0e+02 1.4e-01 3.2e-01 -9.33e-01 7.329570002e+01 6.891836719e+01 1.4e-01 17319.81\n", 570 | "7 7.1e+01 9.9e-02 2.7e-01 -8.22e-01 1.035762404e+02 9.706902075e+01 9.9e-02 18457.95\n", 571 | "8 3.0e+01 4.2e-02 1.4e-01 -6.55e-01 2.096188357e+02 1.986979818e+02 4.2e-02 19651.06\n", 572 | "9 2.2e+01 3.0e-02 1.1e-01 -4.46e-01 2.791150686e+02 2.670704861e+02 3.0e-02 20797.03\n", 573 | "10 8.7e+00 1.2e-02 4.5e-02 -3.22e-01 5.354398494e+02 5.222197021e+02 1.2e-02 21952.88\n", 574 | "11 5.7e+00 7.9e-03 2.7e-02 1.43e-01 6.812695447e+02 6.703308187e+02 7.9e-03 23093.36\n", 575 | "12 3.7e+00 5.2e-03 1.5e-02 3.45e-01 8.160747890e+02 8.074685764e+02 5.2e-03 24248.59\n", 576 | "13 1.8e+00 2.6e-03 6.0e-03 5.08e-01 1.012575195e+03 1.007186451e+03 2.6e-03 25391.27\n", 577 | "14 1.1e+00 1.6e-03 3.2e-03 6.92e-01 1.095555150e+03 1.091693582e+03 1.6e-03 26533.00\n", 578 | "15 7.7e-01 1.1e-03 1.9e-03 7.48e-01 1.118958361e+03 1.116005947e+03 1.1e-03 27675.39\n", 579 | "16 5.8e-01 8.2e-04 1.3e-03 7.86e-01 1.124819723e+03 1.122398388e+03 8.2e-04 28817.45\n", 580 | "17 2.9e-01 4.1e-04 5.0e-04 8.06e-01 1.110558152e+03 1.109117575e+03 4.1e-04 29957.09\n", 581 | "18 2.1e-01 3.0e-04 3.2e-04 8.38e-01 1.063542437e+03 1.062406377e+03 3.0e-04 31098.89\n", 582 | "19 1.1e-01 1.5e-04 1.2e-04 8.45e-01 9.554253832e+02 9.547576930e+02 1.5e-04 32239.38\n", 583 | "20 7.7e-02 1.1e-04 7.7e-05 8.68e-01 8.796202185e+02 8.791105688e+02 1.1e-04 33382.28\n", 584 | "21 2.6e-02 3.7e-05 1.7e-05 8.89e-01 7.069717565e+02 7.067700278e+02 3.7e-05 34533.06\n", 585 | "22 1.6e-02 2.3e-05 8.0e-06 9.93e-01 5.944680813e+02 5.943439789e+02 2.3e-05 35674.08\n", 586 | "23 6.7e-03 9.4e-06 2.1e-06 1.04e+00 4.503959649e+02 4.503469240e+02 9.4e-06 36821.41\n", 587 | "24 4.4e-03 6.1e-06 1.1e-06 1.14e+00 3.791829470e+02 3.791527436e+02 6.1e-06 37962.77\n", 588 | "25 1.7e-03 2.4e-06 2.5e-07 1.13e+00 2.799807544e+02 2.799696635e+02 2.4e-06 39110.53\n", 589 | "26 7.7e-04 1.1e-06 8.3e-08 9.86e-01 2.317552579e+02 2.317493628e+02 1.1e-06 40257.33\n", 590 | "27 5.8e-04 8.1e-07 5.8e-08 7.70e-01 2.210409028e+02 2.210359155e+02 8.1e-07 41400.72\n", 591 | "28 2.8e-04 3.9e-07 2.2e-08 7.16e-01 1.995697056e+02 1.995664865e+02 3.9e-07 42543.27\n", 592 | "29 2.3e-04 3.2e-07 1.7e-08 6.11e-01 1.955948982e+02 1.955919924e+02 3.2e-07 43683.31\n", 593 | "30 9.1e-05 1.3e-07 5.3e-09 5.93e-01 1.804834278e+02 1.804816671e+02 1.3e-07 44834.53\n", 594 | "31 5.7e-05 8.0e-08 3.0e-09 5.20e-01 1.755331315e+02 1.755317266e+02 7.9e-08 45976.92\n", 595 | "32 2.5e-05 3.4e-08 1.1e-09 4.99e-01 1.691931590e+02 1.691922100e+02 3.4e-08 47125.73\n", 596 | "33 1.2e-05 1.7e-08 4.4e-10 4.50e-01 1.655823219e+02 1.655816164e+02 1.6e-08 48267.05\n", 597 | "34 8.0e-06 1.2e-08 2.8e-10 4.46e-01 1.641081281e+02 1.641075253e+02 1.1e-08 49444.67\n", 598 | "35 7.5e-06 1.1e-08 2.5e-10 8.43e-01 1.640271790e+02 1.640266087e+02 1.1e-08 54980.28\n", 599 | "36 4.2e-06 6.9e-09 1.1e-10 8.50e-01 1.636140738e+02 1.636137367e+02 5.8e-09 56116.47\n", 600 | "37 4.0e-06 9.1e-09 9.2e-11 9.86e-01 1.639546105e+02 1.639543002e+02 5.4e-09 58366.59\n", 601 | "38 4.0e-06 9.1e-09 9.2e-11 9.85e-01 1.639546105e+02 1.639543002e+02 5.4e-09 59533.25\n", 602 | "39 4.0e-06 9.1e-09 9.2e-11 1.00e+00 1.639546105e+02 1.639543002e+02 5.4e-09 60695.98\n", 603 | "Optimizer terminated. Time: 61867.39\n", 604 | "\n", 605 | "SDP solving time: 61959.2964561 seconds.\n", 606 | "termination status: SLOW_PROGRESS\n", 607 | "solution status: UNKNOWN_RESULT_STATUS\n", 608 | "TSL!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n", 609 | "optimum = 163.95461049635765\n", 610 | "TSL!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n", 611 | "The local solver failed!\n", 612 | "*********************************** TSSOS ***********************************\n", 613 | "Version 1.0.0, developed by Jie Wang, 2020--2022\n", 614 | "TSSOS is launching...\n", 615 | "-----------------------------------------------------------------------------\n", 616 | "The clique sizes of varibles:\n", 617 | "[25, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 2]\n", 618 | "[9, 18, 18, 9, 16, 9, 1, 2, 5, 1, 19, 47, 29, 19, 4, 3, 9, 3, 12, 2, 2, 21, 2]\n", 619 | "-----------------------------------------------------------------------------\n", 620 | "Obtained the variable cliques in 0.4019486 seconds. The maximal size of cliques is 25.\n", 621 | "Assembling the SDP...\n", 622 | "There are 664108 affine constraints.\n", 623 | "SDP assembling time: 9.8606407 seconds.\n", 624 | "Solving the SDP...\n", 625 | "Problem\n", 626 | " Name : \n", 627 | " Objective sense : max \n", 628 | " Type : CONIC (conic optimization problem)\n", 629 | " Constraints : 664108 \n", 630 | " Cones : 0 \n", 631 | " Scalar variables : 63504 \n", 632 | " Matrix variables : 1197 \n", 633 | " Integer variables : 0 \n", 634 | "\n", 635 | "Optimizer started.\n", 636 | "Presolve started.\n", 637 | "Linear dependency checker started.\n", 638 | "Linear dependency checker terminated.\n", 639 | "Eliminator started.\n", 640 | "Freed constraints in eliminator : 0\n", 641 | "Eliminator terminated.\n", 642 | "Eliminator - tries : 1 time : 0.00 \n", 643 | "Lin. dep. - tries : 1 time : 0.47 \n", 644 | "Lin. dep. - number : 0 \n", 645 | "Presolve terminated. Time: 0.98 \n", 646 | "GP based matrix reordering started.\n", 647 | "GP based matrix reordering terminated.\n", 648 | "Problem\n", 649 | " Name : \n", 650 | " Objective sense : max \n", 651 | " Type : CONIC (conic optimization problem)\n", 652 | " Constraints : 664108 \n", 653 | " Cones : 0 \n", 654 | " Scalar variables : 63504 \n", 655 | " Matrix variables : 1197 \n", 656 | " Integer variables : 0 \n", 657 | "\n", 658 | "Optimizer - threads : 8 \n", 659 | "Optimizer - solved problem : the primal \n", 660 | "Optimizer - Constraints : 664108\n", 661 | "Optimizer - Cones : 1\n", 662 | "Optimizer - Scalar variables : 63505 conic : 63505 \n", 663 | "Optimizer - Semi-definite variables: 1197 scalarized : 4607646 \n", 664 | "Factor - setup time : 7042.00 dense det. time : 1178.28 \n", 665 | "Factor - ML order time : 224.78 GP order time : 4649.25 \n", 666 | "Factor - nonzeros before factor : 7.77e+09 after factor : 9.55e+09 \n", 667 | "Factor - dense dim. : 2 flops : 1.67e+14 \n", 668 | "Factor - GP saved nzs : 8.50e+10 GP saved flops : 2.60e+16 \n", 669 | "ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME \n", 670 | "0 7.2e+02 1.0e+00 1.0e+00 0.00e+00 0.000000000e+00 0.000000000e+00 1.0e+00 7050.25\n", 671 | "1 7.1e+02 9.9e-01 9.8e-01 4.19e-01 9.589190812e-02 9.393350774e-02 9.9e-01 8229.97\n", 672 | "2 7.0e+02 9.7e-01 9.9e-01 -2.98e-01 7.631892523e-01 7.359098704e-01 9.7e-01 9429.27\n", 673 | "3 5.8e+02 8.1e-01 9.1e-01 -6.66e-01 5.165351460e+00 4.939979549e+00 8.1e-01 10622.25\n", 674 | "4 3.7e+02 5.2e-01 6.8e-01 -8.45e-01 1.526671928e+01 1.450797818e+01 5.2e-01 11809.97\n", 675 | "5 1.5e+02 2.1e-01 4.2e-01 -9.38e-01 6.695454763e+01 6.383426245e+01 2.1e-01 13027.31\n", 676 | "6 8.0e+01 1.1e-01 2.9e-01 -9.33e-01 1.218376301e+02 1.160810799e+02 1.1e-01 14217.47\n", 677 | "7 5.8e+01 8.1e-02 2.4e-01 -7.03e-01 1.658326958e+02 1.578691181e+02 8.1e-02 15406.52\n", 678 | "8 2.9e+01 4.1e-02 1.5e-01 -7.13e-01 2.755708877e+02 2.626981224e+02 4.1e-02 16612.31\n" 679 | ] 680 | } 681 | ], 682 | "source": [ 683 | "for m = [0.4, 0.75]\n", 684 | " cartpole_param = Control.param_cart_multi(mpc_param, m, m, 0.0, -m, weights, 200 * weights, false, 0, t0, dof)\n", 685 | " log = CartPoleTO(cartpole_param)\n", 686 | "\n", 687 | " if m == 0.4\n", 688 | " matwrite(\"log_040_0125.mat\", log)\n", 689 | " end\n", 690 | "\n", 691 | " if m == 0.75\n", 692 | " matwrite(\"log_075_0125.mat\", log)\n", 693 | " end\n", 694 | "end" 695 | ] 696 | }, 697 | { 698 | "cell_type": "markdown", 699 | "id": "b7a4f306", 700 | "metadata": {}, 701 | "source": [ 702 | "TODO: Find a way to compute in parallel, modify the thread number for usage on vino" 703 | ] 704 | }, 705 | { 706 | "cell_type": "code", 707 | "execution_count": 4, 708 | "id": "5c348f3f", 709 | "metadata": {}, 710 | "outputs": [ 711 | { 712 | "data": { 713 | "text/plain": [ 714 | "\"2\"" 715 | ] 716 | }, 717 | "execution_count": 4, 718 | "metadata": {}, 719 | "output_type": "execute_result" 720 | } 721 | ], 722 | "source": [ 723 | "string(2)\n", 724 | "# \"log.mat\" * \"2\"" 725 | ] 726 | } 727 | ], 728 | "metadata": { 729 | "kernelspec": { 730 | "display_name": "Julia 1.8.3", 731 | "language": "julia", 732 | "name": "julia-1.8" 733 | }, 734 | "language_info": { 735 | "file_extension": ".jl", 736 | "mimetype": "application/julia", 737 | "name": "julia", 738 | "version": "1.8.3" 739 | } 740 | }, 741 | "nbformat": 4, 742 | "nbformat_minor": 5 743 | } 744 | -------------------------------------------------------------------------------- /Cart-Pole/arm2/plot_multi_cart_paper.m: -------------------------------------------------------------------------------- 1 | clear all;clc;close all; 2 | %% 3 | log = load("deg45.mat"); 4 | load("deg45_refine.mat") 5 | % [sol_refine, obj] = refine_solution(log); 6 | % save("deg45_refine.mat", "sol_refine") 7 | %% 8 | load("deg45.mat") 9 | % figure() 10 | sol_eval= sol_refine; 11 | % sol_eval= sol; 12 | ns1 = Ns + 1; 13 | 14 | %% 15 | fig_size = [7, 3] * 3 ; 16 | h = figure('Renderer', 'painters', 'unit', 'centimeters', 'Position', [0, 0, fig_size]); 17 | left_coner_list = [0.08, 0.55]; 18 | 19 | for id = 1:2 20 | subplot(1, 2, id, 'Position', [left_coner_list(id), 0.05, 0.4, 1]); 21 | hold on 22 | if id == 1 23 | [x, y, c, s, vx, vy, ca, sa, lam_x, lam_y, tau] = get_sol_eval(sol, ns1, dof, Ns); 24 | title("SDP solution", "Interpreter", "latex") 25 | else 26 | [x, y, c, s, vx, vy, ca, sa, lam_x, lam_y, tau] = get_sol_eval(sol_refine, ns1, dof, Ns); 27 | title("IPOPT refined solution", "Interpreter", "latex") 28 | end 29 | 30 | 31 | x_batch = reshape(x, [dof+1., ns1 + 0.]); 32 | y_batch = reshape(y, [dof+.0, ns1 + 0.]); 33 | for k = 1:length(x_batch) 34 | % plot([x_batch(3, k), x_batch(1, k)], [0, y_batch(1, k)], "b-", "LineWidth", 2) 35 | plot([x_batch(3, k), x_batch(1, k)], [0, y_batch(1, k)], "-", "LineWidth", 1, "color", [1, 1, 1] * 0.25) 36 | plot([x_batch(2, k), x_batch(1, k)], [y_batch(2, k), y_batch(1, k)], "-", "LineWidth", 1, "color", [1, 1, 1] * 0.75) 37 | end 38 | 39 | plot(x(dof+1:dof+1:end), x(dof+1:dof+1:end) * 0, 'o', "LineWidth", 1, "color", [1, 1, 1] * 0.5, "MarkerSize", 3) 40 | for k = 2 41 | plot(x(k:dof+1:end), y(k:dof:end), 'r-o', "LineWidth", 1, "MarkerSize", 4) 42 | plot(x(k), y(k), 'go', "LineWidth", 1, "MarkerFaceColor", 'g') 43 | plot(x(end-1), y(end), 'go', "LineWidth", 1, "MarkerFaceColor", 'g') 44 | end 45 | 46 | xlabel("$x$", "Interpreter","latex") 47 | ylabel("$y$", "Interpreter","latex") 48 | 49 | grid on 50 | box on 51 | daspect([1, 1, 1]) 52 | 53 | xlim([-1.2, 1.2]) 54 | ylim([-0.8, 1.2]) 55 | end 56 | 57 | set(h, 'Units','pixels'); 58 | set(h, 'PaperPositionMode','Auto','PaperUnits','centimeters','PaperSize', fig_size) 59 | print("traj_cartpole3", "-dpdf") 60 | %% 61 | function [x, y, c, s, vx, vy, ca, sa, lam_x, lam_y, tau] = get_sol_eval(sol_eval, ns1, dof, Ns) 62 | x = sol_eval(1:ns1*(dof+1)); 63 | y = sol_eval(ns1*(dof+1)+1:(dof * 2+1)*ns1); 64 | c = sol_eval(ns1*(dof*2+1)+1:(dof*3+1)*ns1); 65 | s = sol_eval(ns1*(dof*3+1)+1:(dof*4+1)*ns1); 66 | 67 | offset = length([x; y; c; s]); 68 | vx = sol_eval(offset + 1:offset + ns1*(dof+1)); 69 | vy = sol_eval(offset + ns1*(dof+1)+1:offset + (dof * 2+1)*ns1); 70 | ca = sol_eval(offset + ns1*(dof*2+1)+1:offset + (dof*3+1)*ns1); 71 | sa = sol_eval(offset + ns1*(dof*3+1)+1:offset + (dof*4+1)*ns1); 72 | offset = length([x; y; c; s; ... 73 | vx; vy; ca; sa]); 74 | % ns = dof * (Ns+1); 75 | lam_x = sol_eval(offset + 1: offset + dof * Ns); 76 | lam_y = sol_eval(offset + dof * Ns + 1: offset + dof * Ns * 2); 77 | tau = sol_eval(offset + dof * Ns * 2: end); 78 | end 79 | -------------------------------------------------------------------------------- /Cart-Pole/arm2/refine_solution.m: -------------------------------------------------------------------------------- 1 | function [sol_refine, obj] = refine_solution(log) 2 | %% 3 | dof = double(log.dof); 4 | Ns = double(log.Ns); 5 | 6 | t0 = log.param.theta_0; 7 | xc_0 = log.param.xc_0; 8 | yc_0 = 0.0; 9 | 10 | lx = log.param.lx; 11 | ly = log.param.ly; 12 | m = log.param.mass; 13 | I = log.param.inertial; 14 | dt = log.param.control_param.dt; 15 | g = -9.81; 16 | %% 17 | yalmip('clear') 18 | % R = sdpvar(dof+1, 3, 3); 19 | % F = sdpvar(dof+1, 3, 3); 20 | % p = sdpvar(dof+1, 3, 1); 21 | % v = sdpvar(dof+1, 3, 1); 22 | % tau = sdpvar(dof, 3, 1); 23 | % f = sdpvar(dof, 1); 24 | % p_load = sdpvar(dof+1, 3, 1); 25 | % v_load = sdpvar(dof+1, 3, 1); 26 | 27 | x = sdpvar(dof+1, Ns+1); x_ = x'; 28 | y = sdpvar(dof, Ns+1); y_ = y'; 29 | c = sdpvar(dof, Ns+1); c_ = c'; 30 | s = sdpvar(dof, Ns+1); s_ = s'; 31 | 32 | vx = sdpvar(dof+1, Ns+1); vx_ = vx'; 33 | vy = sdpvar(dof, Ns+1); vy_ = vy'; 34 | ca = sdpvar(dof, Ns+1); ca_ = ca'; 35 | sa = sdpvar(dof, Ns+1); sa_ = sa'; 36 | 37 | lam_x = sdpvar(dof, Ns); lam_x_ = lam_x'; 38 | lam_y = sdpvar(dof, Ns); lam_y_ = lam_y'; 39 | tau = sdpvar(Ns, 1); tau_ = tau'; 40 | 41 | var = [x(:); y(:); c(:); s(:);... 42 | vx(:); vy(:); ca(:); sa(:);... 43 | lam_x(:); lam_y(:); tau(:)]; 44 | 45 | % var = [x_(:); y_(:); c_(:); s_(:);... 46 | % vx_(:); vy_(:); ca_(:); sa_(:);... 47 | % lam_x_(:); lam_y_(:); tau_(:)]; 48 | 49 | ns = dof * (Ns+1); 50 | nf = Ns; 51 | nlam = dof * Ns; 52 | 53 | %% 54 | % load 55 | eq = []; 56 | ineq = []; 57 | f = 0; 58 | 59 | eq = [eq; 60 | x(end, 1) - xc_0; 61 | vx(end, 1) - 0]; 62 | % c(1, 1) - cos(t0); 63 | % s(1, 1) - sin(t0); 64 | % cos(t0) * lx - sin(t0) * ly + x(1, 1) - xc_0; 65 | % sin(t0) * lx + cos(t0) * ly + y(1, 1) - yc_0; 66 | % vx(1, 1) - 0; 67 | % vy(1, 1) - 0; 68 | % ca(1, 1) - 1.0; 69 | % sa(1, 1) - 0]; 70 | 71 | for k = 1:dof 72 | eq = [eq; 73 | c(k, 1) - cos(t0(k)); 74 | s(k, 1) - sin(t0(k)); 75 | vx(k, 1) - 0.0; 76 | vy(k, 1) - 0.0; 77 | ca(k, 1) - 1.0; 78 | sa(k, 1) - 0.0]; 79 | if k == 1 80 | eq = [eq; 81 | (cos(t0(k)) * lx - sin(t0(k)) * ly + x(k, 1) - x(end, 1) ); 82 | (sin(t0(k)) * lx + cos(t0(k)) * ly + y(k, 1) - 0 )]; 83 | else 84 | eq = [eq; 85 | (cos(t0(k)) * lx - sin(t0(k)) * ly + x(k, 1) - x(k-1, 1)); 86 | (sin(t0(k)) * lx + cos(t0(k)) * ly + y(k, 1) - y(k-1, 1))]; 87 | end 88 | end 89 | 90 | 91 | 92 | for k = 1:Ns 93 | %% cart 94 | eq = [eq; 95 | x(end, k+1) - (x(end, k) + dt * vx(end, k)); 96 | (m * vx(end, k+1) - m * vx(end, k) - (lam_x(end, k) + tau(k))* dt )]; 97 | 98 | for j = 1:dof 99 | eq = [eq; 100 | %% pole 101 | c(j, k+1) - (c(j, k) * ca(j, k) - s(j, k) * sa(j, k)); 102 | s(j, k+1) - (s(j, k) * ca(j, k) + c(j, k) * sa(j, k)); 103 | x(j, k+1) - (dt * (c(j, k) * vx(j, k) - s(j, k) * vy(j, k)) + x(j, k)); 104 | y(j, k+1) - (dt * (s(j, k) * vx(j, k) + c(j, k) * vy(j, k)) + y(j, k))]; 105 | %% sin cos 106 | ineq = [ineq; 107 | -(s(j,k+1)^2 + c(j,k+1)^2) + 1.000000; 108 | s(j,k+1)^2 + c(j,k+1)^2 - 1 - 1e-8; 109 | -(sa(j,k+1)^2 + ca(j,k+1)^2) + 1.000000; 110 | sa(j,k+1)^2 + ca(j,k+1)^2 - 1 - 1e-8; 111 | x(j, k+1) + 1; 112 | -x(j, k+1) + 1]; 113 | % tau(k) + 2; 114 | % -tau(k) + 2]; 115 | 116 | %% kinematic constraints 117 | if j == 1 118 | eq = [eq; 119 | c(j,k+1) * lx - s(j,k+1) * ly + x(j, k+1) - x(end, k+1); 120 | s(j,k+1) * lx + c(j,k+1) * ly + y(j, k+1) - 0]; 121 | else 122 | eq = [eq; 123 | c(j,k+1) * lx - s(j,k+1) * ly + x(j, k+1) - x(j-1, k+1); 124 | s(j,k+1) * lx + c(j,k+1) * ly + y(j, k+1) - y(j-1, k+1)]; 125 | end 126 | 127 | Jx1 = (-c(j, k+1) * ly - s(j, k+1) * lx); 128 | Jy1 = (-s(j, k+1) * ly + c(j, k+1) * lx); 129 | eq = [eq; ((sa(j, k+1) - sa(j, k)) * I - (Jx1 * lam_x(j, k) + Jy1 * lam_y(j, k)) * dt^2)]; 130 | 131 | Fx1 = c(j, k+1)*lam_x(j, k) + s(j, k+1)*lam_y(j, k); 132 | Fy1 = - s(j, k+1)*lam_x(j, k) + c(j, k+1)*lam_y(j, k); 133 | 134 | if j < dof 135 | Fx2 = c(j, k+1)*lam_x(j+1, k) + s(j+1, k+1)*lam_y(j, k); 136 | Fy2 = - s(j, k+1)*lam_x(j+1, k) + c(j+1, k+1)*lam_y(j, k); 137 | else 138 | Fx2 = 0.; 139 | Fy2 = 0.; 140 | end 141 | 142 | eq = [eq; 143 | (m * vx(j, k+1) - m * ( ca(j, k) * vx(j, k) + sa(j, k) * vy(j, k) ) - (Fx1 + Fx2 + m * s(1, k+1) * g)* dt ); 144 | (m * vy(j, k+1) - m * (-sa(j, k) * vx(j, k) + ca(j, k) * vy(j, k) ) - (Fy1 + Fy2 + m * c(1, k+1) * g)* dt )]; 145 | 146 | f = f + ( x(j, k)^2 + (y(j, k) - j * abs(ly))^2 + (c(j, k) - 1)^2 + s(j, k)^2) * log.param.run_cost(1); 147 | f = f + (vx(j, k)^2 + vy(j, k)^2 + (ca(j, k) - 1)^2 + sa(j, k)^2) * log.param.run_cost(1); 148 | % f = f + (vx(j, k)^2 + vx(j, k)^2 + vy(j, k)^2 + (ca(j, k) - 1)^2 + sa(j, k)^2) * log.param.run_cost(1); 149 | f = f + log.param.run_cost(1) * tau(k)^2; 150 | end 151 | f = f + ( x(end, k)^2 + vx(end, k)^2) * log.param.run_cost(1); 152 | end 153 | %% 154 | f = f + ( x(end, end)^2 + vx(end, end)^2) * log.param.run_cost(1); 155 | for j = 1:dof 156 | f = f + ( x(j, end)^2 + (y(j, end) - j * abs(ly))^2 + (c(j, end) - 1)^2 + s(j, end)^2) * log.param.run_cost(1); 157 | f = f + (vx(j, end)^2 + vx(j, end)^2 + vy(j, end)^2 + (ca(j, end) - 1)^2 + sa(j, end)^2) * log.param.run_cost(1); 158 | end 159 | 160 | for k = 1:length(var)-Ns 161 | ineq = [ineq; 162 | 100 - var(k); 163 | 100 + var(k)]; 164 | end 165 | 166 | for k = 1:Ns+1 167 | ineq = [ineq; 168 | 0 + ca(k)]; 169 | end 170 | 171 | % ineq = [ineq; 172 | % vx(:) + 5; 173 | % -vx(:) + 5; 174 | % vy(:) + 5; 175 | % -vy(:) + 5]; 176 | 177 | % for k = 1:Ns 178 | % ineq = [ineq; 179 | % 5 - tau(k); 180 | % 5 + tau(k); 181 | % 55 - lam_x(k); 182 | % 55 + lam_x(k); 183 | % 55 - lam_y(k); 184 | % 55 + lam_y(k); 185 | % ca(1, k)]; 186 | % end 187 | 188 | % f = f + 1e5 * sum((var - log.sol_approx).^2); 189 | 190 | % ineq = [ineq; 191 | % var - (log.sol_approx - 6); 192 | % -var + (log.sol_approx + 6)]; 193 | %% 194 | assign(var, log.sol_approx + 1e-2 * randn(size(log.sol_approx))) 195 | c_norm = sqrt(value(c).^2 + value(s).^2); 196 | c = c ./ c_norm; 197 | s = s ./ c_norm; 198 | 199 | ca_norm = sqrt(value(ca).^2 + value(sa).^2); 200 | ca = ca ./ ca_norm; 201 | sa = sa ./ ca_norm; 202 | 203 | %% 204 | ops = sdpsettings('solver','fmincon');%,'fmincon.algorithm','sqp'); 205 | % ops = sdpsettings('solver','ipopt'); 206 | ops.print_level = 12; 207 | ops.verbose = true; 208 | 209 | % % ops.fmincon.MaxIterations = 1e7; 210 | % ops.ipopt.max_iter = 1; % 1e4 * 10; 211 | % ops.ipopt.max_cpu_time = 1e100; 212 | 213 | ops.usex0 = 1; 214 | optimize([eq == 0; ineq >= 0],f,ops); 215 | %% 216 | x_pole = value(x(1, :)); 217 | x_cart = value(x(2, :)); 218 | y_pole = value(y(1, :)); 219 | c_pole = value(c(1, :)); 220 | s_pole = value(s(1, :)); 221 | 222 | vx_pole = value(vx(1, :)); 223 | vx_cart = value(vx(2, :)); 224 | vy_pole = value(vy(1, :)); 225 | ca_pole = value(ca(1, :)); 226 | sa_pole = value(sa(1, :)); 227 | 228 | lam_x = value(lam_x); 229 | lam_y = value(lam_y); 230 | tau = value(tau); 231 | 232 | obj = value(f); 233 | %% 234 | % sol_refine = [x_pole(:); x_cart(:); y_pole(:); c_pole(:); s_pole(:);... 235 | % vx_pole(:); vx_cart(:); vy_pole(:); ca_pole(:); sa_pole(:);... 236 | % lam_x(:); lam_y(:); tau(:)]; 237 | sol_refine = value(var); 238 | end -------------------------------------------------------------------------------- /Cart-Pole/arm2/traj_cartpole3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Cart-Pole/arm2/traj_cartpole3.pdf -------------------------------------------------------------------------------- /Control_utils.jl: -------------------------------------------------------------------------------- 1 | module Control 2 | 3 | mutable struct param_base 4 | dt::Float64 5 | Ns::Int 6 | order::Int 7 | # CS_type::String 8 | # TS_type::String 9 | end 10 | 11 | mutable struct param_cart_multi 12 | control_param::param_base 13 | mass::Float64 14 | inertial::Float64 15 | lx::Float64 16 | ly::Float64 17 | run_cost::Matrix{Float64} 18 | terminal_cost::Matrix{Float64} 19 | terminal_cons::Bool # 20 | # x2 x1 y1 (c1 s1) || vx2 vx1 vy1 (ca1 sa1) || tau 21 | xc_0::Float64 22 | theta_0::Matrix{Float64} 23 | dof::Int 24 | end 25 | 26 | 27 | mutable struct param_cartpole 28 | control_param::param_base 29 | mass::Float64 30 | inertial::Float64 31 | lx::Float64 32 | ly::Float64 33 | run_cost::Matrix{Float64} 34 | terminal_cost::Matrix{Float64} 35 | terminal_cons::Bool # 36 | # x2 x1 y1 (c1 s1) || vx2 vx1 vy1 (ca1 sa1) || tau 37 | xc_0::Float64 38 | theta_0::Float64 39 | end 40 | 41 | 42 | mutable struct param_drone_landing 43 | control_param::param_base 44 | obs_num::Int 45 | obs_pos::Matrix{Float64} 46 | end 47 | 48 | end -------------------------------------------------------------------------------- /Drone-Load/drone-load.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "id": "21e7bfcc", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "using DynamicPolynomials\n", 11 | "using TSSOS\n", 12 | "using MAT\n", 13 | "using LinearAlgebra\n", 14 | "using Printf" 15 | ] 16 | }, 17 | { 18 | "cell_type": "code", 19 | "execution_count": 2, 20 | "id": "bf7bfc61", 21 | "metadata": {}, 22 | "outputs": [ 23 | { 24 | "data": { 25 | "text/plain": [ 26 | "SO3_skew (generic function with 1 method)" 27 | ] 28 | }, 29 | "execution_count": 2, 30 | "metadata": {}, 31 | "output_type": "execute_result" 32 | } 33 | ], 34 | "source": [ 35 | "@polyvar temp1[1:4, 1:4]\n", 36 | "@polyvar temp2[1:4, 1:4]\n", 37 | "temp_p = temp2 * 1.1 + temp1 * 1.1\n", 38 | "\n", 39 | "function Joint2SE3(c, s, x, y, z)\n", 40 | " ## we always rotate about the z axis\n", 41 | " T = temp_p * 0\n", 42 | " T[1,1] = c\n", 43 | " T[2,2] = c\n", 44 | " T[1,2] = -s\n", 45 | " T[2,1] = s\n", 46 | " T[3,3] = 1\n", 47 | " \n", 48 | " T[1,4] = c * x - s * y\n", 49 | " T[2,4] = s * x + c * y\n", 50 | " T[3,4] = z\n", 51 | " T[4,4] = 1\n", 52 | " return T\n", 53 | "end\n", 54 | "\n", 55 | "function SE3_Euler_num(y, p, r, lx, ly, lz)\n", 56 | " T = temp_p * 0\n", 57 | " T[1:3, 1:3] = rotz_num(y) * roty_num(p) * rotx_num(r)\n", 58 | " T[4,4] = 1.0\n", 59 | " T[1,4] = lx\n", 60 | " T[2,4] = ly\n", 61 | " T[3,4] = lz\n", 62 | " return T\n", 63 | "end\n", 64 | "\n", 65 | "function rotx_num(t)\n", 66 | " R = temp_p[1:3, 1:3] * 0\n", 67 | " R[2,2] = cos(t)\n", 68 | " R[3,3] = cos(t)\n", 69 | " R[2,3] = -sin(t)\n", 70 | " R[3,2] = sin(t)\n", 71 | " R[1,1] = 1.0\n", 72 | " return R\n", 73 | "end\n", 74 | "\n", 75 | "function roty_num(t)\n", 76 | " R = temp_p[1:3, 1:3] * 0\n", 77 | " R[1,1] = cos(t)\n", 78 | " R[3,3] = cos(t)\n", 79 | " R[1,3] = sin(t)\n", 80 | " R[3,1] = -sin(t)\n", 81 | " R[2,2] = 1.0\n", 82 | " return R\n", 83 | "end\n", 84 | "\n", 85 | "function rotz_num(t)\n", 86 | " R = temp_p[1:3, 1:3] * 0\n", 87 | " R[1,1] = cos(t)\n", 88 | " R[2,2] = cos(t)\n", 89 | " R[1,2] = -sin(t)\n", 90 | " R[2,1] = sin(t)\n", 91 | " R[3,3] = 1.0\n", 92 | " return R\n", 93 | "end\n", 94 | "\n", 95 | "function quat2rot_num(w, x, y, z)\n", 96 | " R = temp_p[1:3, 1:3] * 0\n", 97 | " nq = sqrt(w^2 + x^2 + y^2 + z^2)\n", 98 | " w = w / nq\n", 99 | " x = x / nq\n", 100 | " y = y / nq\n", 101 | " z = z / nq\n", 102 | " \n", 103 | " R[1,1] = 1 - 2 * (y^2 + z^2)\n", 104 | " R[2,2] = 1 - 2 * (x^2 + z^2)\n", 105 | " R[3,3] = 1 - 2 * (x^2 + y^2)\n", 106 | " \n", 107 | " R[1,2] = 2 * (x * y - w * z)\n", 108 | " R[1,3] = 2 * (w * y + x * z)\n", 109 | " R[2,3] = 2 * (y * z - w * x)\n", 110 | " \n", 111 | " R[2,1] = 2 * (x * y + w * z)\n", 112 | " R[3,1] = 2 * (- w * y + x * z)\n", 113 | " R[3,2] = 2 * (y * z + w * x)\n", 114 | " return R\n", 115 | "end\n", 116 | "\n", 117 | "function cross_(x, y) \n", 118 | " z = temp_p[1:3, 1]\n", 119 | " z[1] = -x[3] * y[2] + x[2] * y[3]\n", 120 | " z[2] = x[3] * y[1] - x[1] * y[3]\n", 121 | " z[3] = -x[2] * y[1] + x[1] * y[2]\n", 122 | " return z\n", 123 | "end\n", 124 | "\n", 125 | "function add_SO3_cons(R, eq)\n", 126 | " T = R' * R\n", 127 | "# print(T[1,1] - 1.0)\n", 128 | " append!(eq, [T[1,1] - 1.0])\n", 129 | " append!(eq, [T[2,2] - 1.0])\n", 130 | " append!(eq, [T[3,3] - 1.0])\n", 131 | " append!(eq, [T[1,2]])\n", 132 | " append!(eq, [T[1,3]])\n", 133 | " append!(eq, [T[2,3]])\n", 134 | "# print(size(eq)) \n", 135 | "# print(\"+++++++++++++++\\n\")\n", 136 | " \n", 137 | " append!(eq, cross_(R[1,:], R[2, :]) - R[3,:])\n", 138 | "# print(size(eq)) \n", 139 | "# print(\"+++++++++++++++\\n\")\n", 140 | " append!(eq, cross_(R[2,:], R[3, :]) - R[1,:])\n", 141 | "# print(size(eq)) \n", 142 | "# print(\"+++++++++++++++\\n\")\n", 143 | " append!(eq, cross_(R[3,:], R[1, :]) - R[2,:])\n", 144 | "# print(size(eq)) \n", 145 | "# print(\"+++++++++++++++\\n\")\n", 146 | " \n", 147 | " return eq\n", 148 | "end\n", 149 | "\n", 150 | "function add_SE3_tran_cons(T1, T2, eq)\n", 151 | " T = T1 - T2\n", 152 | " \n", 153 | " append!(eq, T[1, 1])\n", 154 | " append!(eq, T[1, 2])\n", 155 | " append!(eq, T[1, 3])\n", 156 | " \n", 157 | " \n", 158 | " append!(eq, T[2, 1])\n", 159 | " append!(eq, T[2, 2])\n", 160 | " append!(eq, T[2, 3])\n", 161 | " \n", 162 | " \n", 163 | " append!(eq, T[3, 1])\n", 164 | " append!(eq, T[3, 2])\n", 165 | " append!(eq, T[3, 3])\n", 166 | " \n", 167 | " append!(eq, T[1, 4])\n", 168 | " append!(eq, T[2, 4])\n", 169 | " append!(eq, T[3, 4]) \n", 170 | " return eq\n", 171 | "end\n", 172 | "\n", 173 | "function add_SO3_tran_cons(T1, T2, eq)\n", 174 | " T = T1 - T2\n", 175 | " \n", 176 | "# print(\"eq_init = \")\n", 177 | "# print(size(eq))\n", 178 | "# print(\"\\n\")\n", 179 | " \n", 180 | "# print(\"trans 0 = \")\n", 181 | "# print(size(eq)) \n", 182 | "# print(\"\\n\")\n", 183 | " \n", 184 | " append!(eq, [T[1, 1]])\n", 185 | " append!(eq, [T[1, 2]])\n", 186 | " append!(eq, [T[1, 3]])\n", 187 | "# print(\"trans 1 = \")\n", 188 | "# print(size(eq)) \n", 189 | "# print(\"\\n\")\n", 190 | " \n", 191 | " \n", 192 | " append!(eq, [T[2, 1]])\n", 193 | " append!(eq, [T[2, 2]])\n", 194 | " append!(eq, [T[2, 3]])\n", 195 | "# print(\"trans 2 = \")\n", 196 | "# print(size(eq)) \n", 197 | "# print(\"\\n\")\n", 198 | " \n", 199 | " \n", 200 | " append!(eq, [T[3, 1]])\n", 201 | " append!(eq, [T[3, 2]])\n", 202 | " append!(eq, [T[3, 3]])\n", 203 | "# print(\"trans 3 = \")\n", 204 | "# print(size(eq)) \n", 205 | "# print(\"\\n\")\n", 206 | " \n", 207 | " return eq\n", 208 | "end\n", 209 | "\n", 210 | "function make_SE3(R, x, y, z)\n", 211 | " T = temp_p * 0\n", 212 | " T[1:3, 1:3] = R\n", 213 | " T[1,4] = x\n", 214 | " T[2,4] = y\n", 215 | " T[3,4] = z\n", 216 | " T[4,4] = 1.0\n", 217 | " return T\n", 218 | "end\n", 219 | "\n", 220 | "function SE3_inverse(T)\n", 221 | " Tnew = T\n", 222 | " Tnew[1:3, 1:3] = T[1:3, 1:3]'\n", 223 | " Tnew[1:3, 4] = - T[1:3, 1:3]' * T[1:3, 4]\n", 224 | " return Tnew\n", 225 | "end\n", 226 | "\n", 227 | "function SO3_skew(T)\n", 228 | " \n", 229 | " \n", 230 | "end" 231 | ] 232 | }, 233 | { 234 | "cell_type": "code", 235 | "execution_count": null, 236 | "id": "7cc5b380", 237 | "metadata": {}, 238 | "outputs": [], 239 | "source": [] 240 | }, 241 | { 242 | "cell_type": "code", 243 | "execution_count": 3, 244 | "id": "b7f15ed0", 245 | "metadata": {}, 246 | "outputs": [ 247 | { 248 | "data": { 249 | "text/plain": [ 250 | "test_drone (generic function with 1 method)" 251 | ] 252 | }, 253 | "execution_count": 3, 254 | "metadata": {}, 255 | "output_type": "execute_result" 256 | } 257 | ], 258 | "source": [ 259 | "function test_drone(dof, dt, obs_flag, quat_init, mode, obs_id, dx0_in)\n", 260 | "\n", 261 | " @polyvar R[1:dof+1, 1:3, 1:3] \n", 262 | " @polyvar F[1:dof+1, 1:3, 1:3]\n", 263 | "\n", 264 | " @polyvar p[1:dof+1, 1:3] \n", 265 | " @polyvar v[1:dof+1, 1:3]\n", 266 | "\n", 267 | " @polyvar tau[1:dof, 1:3]\n", 268 | " @polyvar f[1:dof, 1]\n", 269 | "\n", 270 | " Jb = Matrix(1.0I, 3, 3)\n", 271 | " Jb[1,1] = 1.0 / 10\n", 272 | " Jb[2,2] = 2.0 / 10\n", 273 | " Jb[3,3] = 1.0 / 10\n", 274 | "\n", 275 | " m = 1 / 2\n", 276 | "\n", 277 | " @polyvar p_load[1:dof+1, 1:3] \n", 278 | " @polyvar v_load[1:dof+1, 1:3]\n", 279 | " @polyvar lam[1:dof, 1]\n", 280 | " m_load = 0.5\n", 281 | " arm_length = 0.5\n", 282 | "\n", 283 | " var = append!(reshape(R, 9 * (dof+1)), reshape(F, 9 * (dof + 1)), \n", 284 | " reshape(p, 3 * (dof + 1)), reshape(v, 3 * (dof + 1)), reshape(tau, 3 * dof), reshape(f, dof), \n", 285 | " reshape(p_load, 3 * (dof + 1)), reshape(v_load, 3 * (dof + 1)), reshape(lam, dof) )\n", 286 | "\n", 287 | " eq = [temp_p[1,1] * 0]\n", 288 | "\n", 289 | " # R_goal = quat2rot_num(1, 0, 0, 0); \n", 290 | " # R_goal = quat2rot_num(1, 0, 1, 0); \n", 291 | " # R_init = quat2rot_num(0.86, 0, 0.5, 0); \n", 292 | " # R_goal = quat2rot_num(0.5, 0, 0.86, 0); \n", 293 | "\n", 294 | " R_init = quat2rot_num(quat_init[1], quat_init[2], quat_init[3], quat_init[4]); \n", 295 | "\n", 296 | " eq[1] = R[1, 1, 1] - R_init[1, 1]\n", 297 | " append!(eq, [R[1, 1, 2] - R_init[1, 2]])\n", 298 | " append!(eq, [R[1, 1, 3] - R_init[1, 3]])\n", 299 | " append!(eq, [R[1, 2, 1] - R_init[2, 1]])\n", 300 | " append!(eq, [R[1, 2, 2] - R_init[2, 2]])\n", 301 | " append!(eq, [R[1, 2, 3] - R_init[2, 3]])\n", 302 | " append!(eq, [R[1, 3, 1] - R_init[3, 1]])\n", 303 | " append!(eq, [R[1, 3, 2] - R_init[3, 2]])\n", 304 | " append!(eq, [R[1, 3, 3] - R_init[3, 3]])\n", 305 | "\n", 306 | " append!(eq, [F[1, 1, 1] - 1.0])\n", 307 | " append!(eq, [F[1, 1, 2] - 0.0])\n", 308 | " append!(eq, [F[1, 1, 3] - 0.0])\n", 309 | " append!(eq, [F[1, 2, 1] - 0.0])\n", 310 | " append!(eq, [F[1, 2, 2] - 1.0])\n", 311 | " append!(eq, [F[1, 2, 3] - 0.0])\n", 312 | " append!(eq, [F[1, 3, 1] - 0.0])\n", 313 | " append!(eq, [F[1, 3, 2] - 0.0])\n", 314 | " append!(eq, [F[1, 3, 3] - 1.0])\n", 315 | "\n", 316 | " append!(eq, [p[1,1] - 1.0])\n", 317 | " append!(eq, [p[1,2] - 1.0])\n", 318 | " append!(eq, [p[1,3] - 3.0])\n", 319 | "\n", 320 | " append!(eq, [v[1,1] - 0.])\n", 321 | " append!(eq, [v[1,2] - 0.])\n", 322 | " append!(eq, [v[1,3] - 0.])\n", 323 | "\n", 324 | " ## load\n", 325 | " append!(eq, [p_load[1,1] - 1.0])\n", 326 | " append!(eq, [p_load[1,2] - 1.0])\n", 327 | " append!(eq, [p_load[1,3] - 2.5])\n", 328 | "\n", 329 | " dx0 = dx0_in\n", 330 | " dz0 = (arm_length - sqrt(( arm_length^2 - (dx0 * dt)^2 ))) / dt # ((0.5 / dt)^2 - dx0^2)^0.5\n", 331 | " append!(eq, [v_load[1,1] - 0.0])\n", 332 | " append!(eq, [v_load[1,2] - dx0])\n", 333 | " append!(eq, [v_load[1,3] - dz0])\n", 334 | "\n", 335 | " # append!(eq, [sum(arm.^2) - arm_length.^2])\n", 336 | " # append!(eq, [v[1,1] - randn() * 0.1])\n", 337 | " # append!(eq, [v[1,2] - randn() * 0.1])\n", 338 | " # append!(eq, [v[1,3] - randn() * 0.1])\n", 339 | "\n", 340 | " print(size(eq))\n", 341 | " # print(\"hahahah\\n\")\n", 342 | " f_act = 0\n", 343 | "\n", 344 | " g = [0;0;-9.81]\n", 345 | "\n", 346 | " ineq = []\n", 347 | "\n", 348 | " Qc = [1e-1; 10; 1e-1; 1] # R w p v\n", 349 | " Rc = 1e-2 # rt rf\n", 350 | " Pc = [100; 10; 100; 100] * 1.1 / 1.1 # \n", 351 | "\n", 352 | " for k = 1:dof\n", 353 | " ####### drone dynamics ######\n", 354 | " eq = add_SO3_cons(R[k+1,:,:], eq) # SO3 constraints\n", 355 | " eq = add_SO3_cons(F[k+1,:,:], eq) # SO3 constraints\n", 356 | " eq = add_SO3_tran_cons(R[k+1, :, :], R[k, :, :] * F[k,: , :], eq)\n", 357 | "\n", 358 | " p_diff = p[k+1, :] - (p[k, :] + dt * R[k,:, :] * v[k,:]) # position\n", 359 | "\n", 360 | " append!(eq, [p_diff[1]])\n", 361 | " append!(eq, [p_diff[2]])\n", 362 | " append!(eq, [p_diff[3]])\n", 363 | "\n", 364 | " ## dynamics constraints\n", 365 | " Fkp1 = F[k+1, :, :];\n", 366 | " Fk = F[k, :, :];\n", 367 | " Mkp1 = Fkp1 * Jb - Jb * Fkp1'\n", 368 | " Mk = Jb * Fk - Fk' * Jb\n", 369 | " dM = Mkp1 - Mk # SO3\n", 370 | "\n", 371 | " # additional torque by the load\n", 372 | " # print(\"arm\")\n", 373 | " arm = p[k+1, :] - p_load[k+1, :]\n", 374 | "\n", 375 | " append!(eq, [dM[1, 2] + (-tau[k, 3]) * dt^2 ])\n", 376 | " append!(eq, [dM[1, 3] + (tau[k, 2]) * dt^2 ])\n", 377 | " append!(eq, [dM[2, 3] + (-tau[k, 1]) * dt^2 ])\n", 378 | "\n", 379 | " v_diff = m * v[k+1,:] - (m * Fk' * v[k, :] + ([0; 0; 1.0]* f[k] + R[k+1,:,:]' * (m * g + lam[k] * arm )) * dt )\n", 380 | "\n", 381 | " append!(eq, [v_diff[1]])\n", 382 | " append!(eq, [v_diff[2]])\n", 383 | " append!(eq, [v_diff[3]])\n", 384 | " \n", 385 | " ####### load dynamics ######\n", 386 | " # print(\"load - 1 \\n\")\n", 387 | " p_diff = p_load[k+1, :] - (p_load[k, :] + dt * (v_load[k,:])) # position\n", 388 | " append!(eq, [p_diff[1]])\n", 389 | " append!(eq, [p_diff[2]])\n", 390 | " append!(eq, [p_diff[3]])\n", 391 | "\n", 392 | " # print(\"load - 2 \\n\")\n", 393 | " v_diff = m_load * v_load[k+1,:] - (m_load * v_load[k, :] + (-lam[k] * arm + m_load * g) * dt )\n", 394 | " append!(eq, [v_diff[1]])\n", 395 | " append!(eq, [v_diff[2]])\n", 396 | " append!(eq, [v_diff[3]])\n", 397 | "\n", 398 | " ####### holonomic constraints ######\n", 399 | " # print(\"holonomic\")\n", 400 | " append!(eq, [sum(arm.^2) - arm_length.^2])\n", 401 | "\n", 402 | " ####### input constraints ######\n", 403 | "\n", 404 | " tau_max = 5\n", 405 | " # f_max = 7\n", 406 | "\n", 407 | " # append!(ineq, [f_max - f[k]])\n", 408 | " # append!(ineq, [f_max + f[k]])\n", 409 | " append!(ineq, [tau_max - tau[k, 1]])\n", 410 | " append!(ineq, [tau_max + tau[k, 1]])\n", 411 | " append!(ineq, [tau_max - tau[k, 2]])\n", 412 | " append!(ineq, [tau_max + tau[k, 2]])\n", 413 | " append!(ineq, [tau_max - tau[k, 3]])\n", 414 | " append!(ineq, [tau_max + tau[k, 3]])\n", 415 | "\n", 416 | " append!(ineq, [p[k+1, 3]])\n", 417 | "\n", 418 | " if obs_flag == true\n", 419 | " if obs_id == 1\n", 420 | " append!(ineq, [(p[k+1, 1])^2 + (p[k+1, 2] - 0.5)^2 - 0.25])\n", 421 | " append!(ineq, [(p_load[k+1, 1])^2 + (p_load[k+1, 2] - 0.5)^2 - 0.25])\n", 422 | " else\n", 423 | " append!(ineq, [(p[k+1, 1] - 0.6)^2 + (p[k+1, 2] - 0.5)^2 - 0.25])\n", 424 | " append!(ineq, [(p_load[k+1, 1] - 0.6)^2 + (p_load[k+1, 2] - 0.5)^2 - 0.25])\n", 425 | " # print(\"obs-2\")\n", 426 | " end\n", 427 | " # append!(ineq, [(p_load[k+1, 1] - 0.5)^2 + (p_load[k+1, 2] - 0.5)^2 - 0.25])\n", 428 | " end\n", 429 | "\n", 430 | " R_diff = R[k, :, :] - Matrix(1.0I, 3, 3)\n", 431 | " R_diff = R_diff' * R_diff\n", 432 | "\n", 433 | "\n", 434 | " F_cost = F[k, : , :] - Matrix(1.0I, 3, 3)\n", 435 | " F_cost = F_cost' * F_cost\n", 436 | "\n", 437 | " R_cost = R_diff[1,1] + R_diff[2,2] + R_diff[3,3]\n", 438 | " w_cost = F_cost[1, 1] + F_cost[2, 2] + F_cost[3, 3]\n", 439 | "\n", 440 | " p_goal = [0, 0, 0.5]\n", 441 | " p_cost = sum((p[k, :] - p_goal).^2)\n", 442 | " v_cost = sum(v[k, :].^2)\n", 443 | "\n", 444 | " f_act = f_act + Rc * (tau[k, 1]^2 + tau[k, 2]^2 + tau[k, 3]^2) + Rc * f[k, 1]^2\n", 445 | " f_act = f_act + Qc[1] * R_cost\n", 446 | " f_act = f_act + Qc[2] * w_cost\n", 447 | " f_act = f_act + Qc[3] * p_cost\n", 448 | " f_act = f_act + Qc[4] * v_cost\n", 449 | "\n", 450 | " p_goal_load = [0, 0, 0.0] # load\n", 451 | " p_load_cost = sum((p_load[k, :] - p_goal_load).^2)\n", 452 | " v_load_cost = sum(v_load[k, :].^2)\n", 453 | " f_act = f_act + Qc[3] * p_load_cost\n", 454 | " f_act = f_act + Qc[4] * v_load_cost\n", 455 | " end\n", 456 | "\n", 457 | " # orientation error\n", 458 | " p_goal = [0, 0, 0.5]\n", 459 | " p_goal_load = [0, 0, 0.0] # load\n", 460 | "\n", 461 | " R_diff = R[end, :, :] - Matrix(1.0I, 3, 3)\n", 462 | " R_diff = R_diff' * R_diff\n", 463 | " f_R = R_diff[1,1] + R_diff[2,2] + R_diff[3,3]\n", 464 | "\n", 465 | " # angular velocity error\n", 466 | " F_diff = F[end, :, :] - Matrix(1.0I, 3, 3)\n", 467 | " F_diff = F_diff' * F_diff\n", 468 | " f_w = F_diff[1,1] + F_diff[2,2] + F_diff[3,3]\n", 469 | "\n", 470 | " # position cost\n", 471 | " f_p = sum((p[end,:] - p_goal).^2)\n", 472 | " f_v = sum((v[end,:]).^2)\n", 473 | "\n", 474 | " cost = f_act * dt / 0.25\n", 475 | " cost = cost + Pc[1] * f_R + Pc[2] * f_w + Pc[3] * f_p + Pc[4] * f_v\n", 476 | "\n", 477 | " # load cost\n", 478 | " f_p_load = sum((p_load[end,:] - p_goal_load).^2)\n", 479 | " f_v_load = sum((v_load[end,:]).^2)\n", 480 | "\n", 481 | " cost = cost + f_p_load * Pc[3] + f_v_load * Pc[4]\n", 482 | "\n", 483 | " \n", 484 | " pop = append!([cost * 10], ineq, eq)\n", 485 | " eq_len = length(eq)\n", 486 | "\n", 487 | " order = 2\n", 488 | " \n", 489 | " time_start = time()\n", 490 | " if mode == 1\n", 491 | " opt,sol,data =cs_tssos_first(pop, var, order, numeq=eq_len, CS=\"NC\", TS = \"MD\", solution=true, QUIET=false, MomentOne=false)\n", 492 | " elseif mode == 2\n", 493 | " opt,sol,data =cs_tssos_first(pop, var, order, numeq=eq_len, CS=\"NC\", TS = \"block\", solution=true, QUIET=false, MomentOne=false)\n", 494 | " elseif mode == 3\n", 495 | " opt,sol,data =cs_tssos_first(pop, var, order, numeq=eq_len, CS=\"NC\", TS = false, solution=true, QUIET=false, MomentOne=false)\n", 496 | " end\n", 497 | " time_end = time()\n", 498 | " elapsed = time_end - time_start\n", 499 | "\n", 500 | " \n", 501 | " moment = []\n", 502 | " for k = 1:length(data.moment)\n", 503 | " append!(moment, [convert(Matrix{Float64}, data.moment[k])]) # data.Mmatrix[k])\n", 504 | " end\n", 505 | "\n", 506 | "\n", 507 | " log = Dict(\"sol\" => sol, \"dof\" => dof, # \"sol_approx\" => sol_approx,\n", 508 | " \"moment\" => moment, \"opt\" => opt, \"dt\"=>dt, \"Mass\"=>m, \"Inertial\"=>Jb, \n", 509 | " \"quat_init\"=> quat_init, \"Qc\"=> Qc, \"Rc\"=>Rc, \"Pc\"=>Pc, \"obs_flag\"=>obs_flag,\n", 510 | " \"solver_flag\"=>data.flag, \"elapsed\"=>elapsed, \"dx0\"=>dx0, \"dz0\"=>dz0, \"m_load\"=>m_load, \"mode\"=>mode)\n", 511 | " return log\n", 512 | "end" 513 | ] 514 | }, 515 | { 516 | "cell_type": "code", 517 | "execution_count": null, 518 | "id": "36d2a51e", 519 | "metadata": {}, 520 | "outputs": [ 521 | { 522 | "name": "stdout", 523 | "output_type": "stream", 524 | "text": [ 525 | "(30,)*********************************** TSSOS ***********************************\n", 526 | "Version 1.0.0, developed by Jie Wang, 2020--2023\n", 527 | "TSSOS is launching...\n", 528 | "-----------------------------------------------------------------------------\n", 529 | "The clique sizes of varibles:\n", 530 | "[8, 7, 6, 3, 5, 9, 10, 1, 18, 13, 12]\n", 531 | "[6, 26, 21, 9, 48, 1, 2, 5, 1, 3, 1]\n", 532 | "-----------------------------------------------------------------------------\n", 533 | "Obtained the variable cliques in 0.1210698 seconds. The maximal size of cliques is 18.\n", 534 | "Assembling the SDP...\n", 535 | "There are 21243 affine constraints.\n", 536 | "SDP assembling time: 0.951527 seconds.\n", 537 | "Solving the SDP...\n", 538 | "MOSEK warning 705: #34 (nearly) zero elements are specified in sparse row ''(0) of matrix 'A'.\n", 539 | "MOSEK warning 705: #17 (nearly) zero elements are specified in sparse row ''(1) of matrix 'A'.\n", 540 | "MOSEK warning 705: #17 (nearly) zero elements are specified in sparse row ''(2) of matrix 'A'.\n", 541 | "MOSEK warning 705: #2 (nearly) zero elements are specified in sparse row ''(143) of matrix 'A'.\n", 542 | "MOSEK warning 705: #17 (nearly) zero elements are specified in sparse row ''(189) of matrix 'A'.\n", 543 | "MOSEK warning 705: #2 (nearly) zero elements are specified in sparse row ''(299) of matrix 'A'.\n", 544 | "MOSEK warning 705: #17 (nearly) zero elements are specified in sparse row ''(329) of matrix 'A'.\n", 545 | "MOSEK warning 705: #2 (nearly) zero elements are specified in sparse row ''(412) of matrix 'A'.\n", 546 | "MOSEK warning 705: #2 (nearly) zero elements are specified in sparse row ''(430) of matrix 'A'.\n", 547 | "MOSEK warning 705: #3 (nearly) zero elements are specified in sparse row ''(443) of matrix 'A'.\n", 548 | "Warning number 705 is disabled.\n", 549 | "Problem\n", 550 | " Name : \n", 551 | " Objective sense : max \n", 552 | " Type : CONIC (conic optimization problem)\n", 553 | " Constraints : 21243 \n", 554 | " Cones : 0 \n", 555 | " Scalar variables : 24394 \n", 556 | " Matrix variables : 158 \n", 557 | " Integer variables : 0 \n", 558 | "\n", 559 | "Optimizer started.\n", 560 | "Presolve started.\n", 561 | "Linear dependency checker started.\n", 562 | "Linear dependency checker terminated.\n", 563 | "Eliminator started.\n", 564 | "Freed constraints in eliminator : 0\n", 565 | "Eliminator terminated.\n", 566 | "Eliminator started.\n", 567 | "Freed constraints in eliminator : 0\n", 568 | "Eliminator terminated.\n", 569 | "Eliminator - tries : 2 time : 0.00 \n", 570 | "Lin. dep. - tries : 1 time : 0.00 \n", 571 | "Lin. dep. - number : 0 \n", 572 | "Presolve terminated. Time: 0.01 \n", 573 | "Problem\n", 574 | " Name : \n", 575 | " Objective sense : max \n", 576 | " Type : CONIC (conic optimization problem)\n", 577 | " Constraints : 21243 \n", 578 | " Cones : 0 \n", 579 | " Scalar variables : 24394 \n", 580 | " Matrix variables : 158 \n", 581 | " Integer variables : 0 \n", 582 | "\n", 583 | "Optimizer - threads : 8 \n", 584 | "Optimizer - solved problem : the primal \n", 585 | "Optimizer - Constraints : 21243\n", 586 | "Optimizer - Cones : 1\n", 587 | "Optimizer - Scalar variables : 16424 conic : 16424 \n", 588 | "Optimizer - Semi-definite variables: 158 scalarized : 89614 \n", 589 | "Factor - setup time : 1.80 dense det. time : 0.00 \n", 590 | "Factor - ML order time : 0.44 GP order time : 0.00 \n", 591 | "Factor - nonzeros before factor : 3.43e+07 after factor : 5.11e+07 \n", 592 | "Factor - dense dim. : 2 flops : 2.54e+11 \n", 593 | "ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME \n", 594 | "0 1.0e+03 1.0e+00 1.0e+00 0.00e+00 0.000000000e+00 0.000000000e+00 1.0e+00 1.86 \n", 595 | "1 9.5e+02 9.5e-01 9.8e-01 -9.95e-01 -6.335325363e-02 -1.109368066e-01 9.5e-01 4.92 \n", 596 | "2 8.6e+02 8.6e-01 9.2e-01 -9.94e-01 -4.568588623e+00 -4.735401521e+00 8.6e-01 7.98 \n", 597 | "3 6.8e+02 6.8e-01 8.2e-01 -9.94e-01 -7.077159805e+00 -7.537226087e+00 6.8e-01 11.28 \n", 598 | "4 1.3e+02 1.3e-01 3.5e-01 -9.92e-01 -2.107411934e+02 -2.170073207e+02 1.3e-01 14.69 \n", 599 | "5 3.2e+01 3.2e-02 1.3e-01 -8.73e-01 -4.014354871e+02 -4.173711248e+02 3.2e-02 18.14 \n", 600 | "6 1.0e+01 1.0e-02 3.9e-02 -1.46e-01 4.092119023e+03 4.077616433e+03 1.0e-02 21.30 \n", 601 | "7 2.8e+00 2.8e-03 1.1e-02 -7.58e-02 1.162911072e+04 1.161492778e+04 2.8e-03 24.47 \n", 602 | "8 7.2e-01 7.2e-04 2.0e-03 3.01e-01 1.622218988e+04 1.621482098e+04 7.2e-04 27.72 \n", 603 | "9 2.8e-01 2.9e-04 6.1e-04 4.95e-01 1.718304338e+04 1.717854972e+04 2.9e-04 31.06 \n", 604 | "10 9.5e-02 9.5e-05 1.5e-04 5.97e-01 1.771088307e+04 1.770843186e+04 9.5e-05 34.38 \n", 605 | "11 3.5e-02 3.5e-05 4.4e-05 5.39e-01 1.770679676e+04 1.770526062e+04 3.5e-05 37.53 \n", 606 | "12 9.1e-03 9.1e-06 8.3e-06 4.89e-01 1.760575418e+04 1.760492701e+04 9.1e-06 40.70 \n", 607 | "13 3.8e-03 3.8e-06 3.0e-06 4.24e-01 1.754066074e+04 1.754005855e+04 3.8e-06 43.72 \n", 608 | "14 1.4e-03 1.4e-06 9.0e-07 3.95e-01 1.746964788e+04 1.746921730e+04 1.4e-06 47.02 \n", 609 | "15 5.6e-04 5.6e-07 3.3e-07 3.47e-01 1.739610672e+04 1.739576644e+04 5.6e-07 50.05 \n", 610 | "16 3.3e-04 3.4e-07 1.8e-07 3.66e-01 1.735251802e+04 1.735223273e+04 3.3e-07 53.02 \n", 611 | "17 6.9e-05 6.9e-08 2.5e-08 3.93e-01 1.724139683e+04 1.724126352e+04 6.9e-08 56.25 \n", 612 | "18 1.6e-05 1.7e-08 3.6e-09 5.88e-01 1.720519449e+04 1.720513927e+04 1.5e-08 59.39 \n", 613 | "19 3.5e-06 6.4e-09 4.2e-10 7.78e-01 1.719325605e+04 1.719324165e+04 3.3e-09 69.56 \n", 614 | "20 2.0e-06 1.2e-08 1.9e-10 9.63e-01 1.719394725e+04 1.719393887e+04 2.0e-09 74.27 \n", 615 | "21 1.3e-06 4.5e-08 9.6e-11 9.71e-01 1.719490513e+04 1.719489965e+04 1.3e-09 77.58 \n", 616 | "22 7.8e-07 6.2e-08 2.5e-11 9.81e-01 1.719669841e+04 1.719669624e+04 5.1e-10 80.75 \n", 617 | "23 7.8e-07 6.2e-08 2.5e-11 9.89e-01 1.719669841e+04 1.719669624e+04 5.1e-10 84.03 \n", 618 | "24 7.8e-07 6.2e-08 2.5e-11 1.00e+00 1.719669841e+04 1.719669624e+04 5.1e-10 87.31 \n", 619 | "Optimizer terminated. Time: 90.59 \n", 620 | "\n", 621 | "SDP solving time: 94.9084841 seconds.\n", 622 | "termination status: SLOW_PROGRESS\n", 623 | "solution status: FEASIBLE_POINT\n", 624 | "optimum = 17196.698413001406\n", 625 | "\n", 626 | "******************************************************************************\n", 627 | "This program contains Ipopt, a library for large-scale nonlinear optimization.\n", 628 | " Ipopt is released as open source code under the Eclipse Public License (EPL).\n", 629 | " For more information visit https://github.com/coin-or/Ipopt\n", 630 | "******************************************************************************\n", 631 | "\n", 632 | "The local solver failed refining the solution!\n", 633 | "(30,)*********************************** TSSOS ***********************************\n", 634 | "Version 1.0.0, developed by Jie Wang, 2020--2023\n", 635 | "TSSOS is launching...\n", 636 | "-----------------------------------------------------------------------------\n", 637 | "The clique sizes of varibles:\n", 638 | "[8, 7, 6, 3, 5, 9, 10, 1, 18, 13, 12]\n", 639 | "[6, 26, 21, 9, 48, 1, 2, 5, 1, 3, 1]\n", 640 | "-----------------------------------------------------------------------------\n", 641 | "Obtained the variable cliques in 0.0017906 seconds. The maximal size of cliques is 18.\n", 642 | "Assembling the SDP...\n", 643 | "There are 21243 affine constraints.\n", 644 | "SDP assembling time: 0.1267902 seconds.\n", 645 | "Solving the SDP...\n", 646 | "MOSEK warning 705: #34 (nearly) zero elements are specified in sparse row ''(0) of matrix 'A'.\n", 647 | "MOSEK warning 705: #17 (nearly) zero elements are specified in sparse row ''(1) of matrix 'A'.\n", 648 | "MOSEK warning 705: #17 (nearly) zero elements are specified in sparse row ''(2) of matrix 'A'.\n", 649 | "MOSEK warning 705: #2 (nearly) zero elements are specified in sparse row ''(143) of matrix 'A'.\n", 650 | "MOSEK warning 705: #17 (nearly) zero elements are specified in sparse row ''(189) of matrix 'A'.\n", 651 | "MOSEK warning 705: #2 (nearly) zero elements are specified in sparse row ''(299) of matrix 'A'.\n", 652 | "MOSEK warning 705: #17 (nearly) zero elements are specified in sparse row ''(329) of matrix 'A'.\n", 653 | "MOSEK warning 705: #2 (nearly) zero elements are specified in sparse row ''(412) of matrix 'A'.\n", 654 | "MOSEK warning 705: #2 (nearly) zero elements are specified in sparse row ''(430) of matrix 'A'.\n", 655 | "MOSEK warning 705: #3 (nearly) zero elements are specified in sparse row ''(443) of matrix 'A'.\n" 656 | ] 657 | }, 658 | { 659 | "name": "stdout", 660 | "output_type": "stream", 661 | "text": [ 662 | "Warning number 705 is disabled.\n", 663 | "Problem\n", 664 | " Name : \n", 665 | " Objective sense : max \n", 666 | " Type : CONIC (conic optimization problem)\n", 667 | " Constraints : 21243 \n", 668 | " Cones : 0 \n", 669 | " Scalar variables : 24394 \n", 670 | " Matrix variables : 158 \n", 671 | " Integer variables : 0 \n", 672 | "\n", 673 | "Optimizer started.\n", 674 | "Presolve started.\n", 675 | "Linear dependency checker started.\n", 676 | "Linear dependency checker terminated.\n", 677 | "Eliminator started.\n", 678 | "Freed constraints in eliminator : 0\n", 679 | "Eliminator terminated.\n", 680 | "Eliminator started.\n", 681 | "Freed constraints in eliminator : 0\n", 682 | "Eliminator terminated.\n", 683 | "Eliminator - tries : 2 time : 0.00 \n", 684 | "Lin. dep. - tries : 1 time : 0.00 \n", 685 | "Lin. dep. - number : 0 \n", 686 | "Presolve terminated. Time: 0.02 \n", 687 | "Problem\n", 688 | " Name : \n", 689 | " Objective sense : max \n", 690 | " Type : CONIC (conic optimization problem)\n", 691 | " Constraints : 21243 \n", 692 | " Cones : 0 \n", 693 | " Scalar variables : 24394 \n", 694 | " Matrix variables : 158 \n", 695 | " Integer variables : 0 \n", 696 | "\n", 697 | "Optimizer - threads : 8 \n", 698 | "Optimizer - solved problem : the primal \n", 699 | "Optimizer - Constraints : 21243\n", 700 | "Optimizer - Cones : 1\n", 701 | "Optimizer - Scalar variables : 16412 conic : 16412 \n", 702 | "Optimizer - Semi-definite variables: 158 scalarized : 89614 \n", 703 | "Factor - setup time : 1.92 dense det. time : 0.00 \n", 704 | "Factor - ML order time : 0.47 GP order time : 0.00 \n", 705 | "Factor - nonzeros before factor : 3.43e+07 after factor : 5.11e+07 \n", 706 | "Factor - dense dim. : 2 flops : 2.54e+11 \n", 707 | "ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME \n", 708 | "0 1.0e+03 1.0e+00 1.0e+00 0.00e+00 0.000000000e+00 0.000000000e+00 1.0e+00 1.98 \n" 709 | ] 710 | } 711 | ], 712 | "source": [ 713 | "## init velocity\n", 714 | "quat_list = [[1;0;0;0], [0.86;0;0.5;0], [1;0;1;0], [0.5, 0, 0.85, 0], [0,0,1,0]]\n", 715 | "obs_list = [false; true]\n", 716 | "mode_list = [1; 2; 3] # step using pure cs at this moment. \n", 717 | "dx0_list = [-2, -1, 0, 1, 2]\n", 718 | "\n", 719 | "dof = 40\n", 720 | "dt = 0.125\n", 721 | "\n", 722 | "count = 0\n", 723 | "\n", 724 | "for j = 1:5\n", 725 | " for i = 1:5\n", 726 | " count = count + 1\n", 727 | " if count > 12\n", 728 | " logger = test_drone(dof, dt, true, quat_list[j], mode_list[3], 2, dx0_list[i])\n", 729 | " # file_name = \"log_block_\" * string(i) * \"_\" * string(j) * \".mat\"\n", 730 | " file_name = \"log_NC_\" * string(count) * \".mat\"\n", 731 | " matwrite(file_name, logger)\n", 732 | " end\n", 733 | " end\n", 734 | "end" 735 | ] 736 | } 737 | ], 738 | "metadata": { 739 | "kernelspec": { 740 | "display_name": "Julia 1.8.3", 741 | "language": "julia", 742 | "name": "julia-1.8" 743 | }, 744 | "language_info": { 745 | "file_extension": ".jl", 746 | "mimetype": "application/julia", 747 | "name": "julia", 748 | "version": "1.8.3" 749 | } 750 | }, 751 | "nbformat": 4, 752 | "nbformat_minor": 5 753 | } 754 | -------------------------------------------------------------------------------- /Drone-Load/log_NC_5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Drone-Load/log_NC_5.mat -------------------------------------------------------------------------------- /Drone-Load/plot_traj.m: -------------------------------------------------------------------------------- 1 | clear;clc;close all; 2 | %% 3 | case_num = 5; 4 | load("log_NC_" + num2str(case_num) + ".mat") 5 | %% 6 | dof = double(dof); 7 | dx0 = double(dx0); 8 | dz0 = double(dz0); 9 | %% warm start 10 | % [R, F, p, v, tau, f, p_load, v_load, lam, opt_fmincon] = refine_solution_det_load(sol + randn(size(sol)) * 0e-2, dof, dt, Mass, Inertial, Qc, Rc, Pc, double(quat_init'), obs_flag, dx0, dz0); 11 | % [R, F, p, v, tau, f, opt_fmincon] = refine_solution(sol + randn(size(sol)) * 0e-4, dof, dt, Mass, Inertial, Qc, Rc, Pc, double(quat_init'), obs_flag); 12 | % load("ipopt-refine/refine_NC_23.mat") 13 | % load("ipopt-refine3/refine_NC_" + num2str(case_num) + ".mat") 14 | load("refine_NC_" + num2str(case_num) + ".mat") 15 | 16 | %% 17 | R_tssos = sol(1:9 * (dof + 1)); 18 | F_tssos = sol(1+9 * (dof + 1):9 * (dof + 1) * 2); 19 | 20 | shift = 9 * (dof + 1) * 2; 21 | p_tssos = sol( 1+shift : shift+3*(dof+1)); 22 | v_tssos = sol(1+shift+3*(dof+1) : shift+6*(dof+1)); 23 | v_tssos = reshape(v_tssos, [dof+1, 3]); 24 | p_tssos = reshape(p_tssos, [dof+1, 3]); 25 | 26 | shift = shift+6*(dof+1); 27 | control = sol(shift+1:shift + 4 * dof); 28 | 29 | shift = shift+4*dof; 30 | p_tssos_load = sol( 1+shift : shift+3*(dof+1)); 31 | v_tssos_load = sol(1+shift+3*(dof+1) : shift+6*(dof+1)); 32 | v_tssos_load = reshape(v_tssos_load, [dof+1, 3]); 33 | p_tssos_load = reshape(p_tssos_load, [dof+1, 3]); 34 | 35 | 36 | shift = shift + 6 * (dof + 1); 37 | lam_tssos = sol(1+shift : end); 38 | 39 | R_tssos = reshape(R_tssos, [dof+1, 3, 3]); 40 | F_tssos = reshape(F_tssos, [dof+1, 3, 3]); 41 | 42 | 43 | tau_tssos = reshape(control(1:dof*3), [dof, 3]); 44 | f_tssos = reshape(control(dof*3+1:end), [dof, 1]); 45 | %% 46 | % eq_tssos = test_feasibility(R_tssos, F_tssos, p_tssos, v_tssos, tau_tssos, f_tssos, dof); 47 | % eq_fmincon = test_feasibility(R, F, p, v, tau, f, dof); 48 | 49 | %% 50 | w = zeros(dof+1, 1); 51 | for k = 1:dof + 1 52 | FF = squeeze(F(k,:,:)); 53 | w(k) = sqrt(sum(logm(FF).^2, "all") / 2); 54 | end 55 | %% 56 | % subplot(1,2,1) 57 | x = p(1:dof+1); 58 | x_load = p_load(1:dof+1); 59 | % x_load = p_tssos_load(1:dof+1); 60 | % dx = ones(dof+1, 1) * 0.1; 61 | y = p(dof+1+1:2*(dof+1)); 62 | y_load = p_load(dof+1+1:2*(dof+1)); 63 | % y_load = p_tssos_load(dof+1+1:2*(dof+1)); 64 | % dy = zeros(dof+1, 1) * 0.1; 65 | z = p(2*(dof+1)+1:end); 66 | z_load = p_load(2*(dof+1)+1:end); 67 | % z_load = p_tssos_load(2*(dof+1)+1:end); 68 | % dz = zeros(dof+1, 1) * 0.1; 69 | 70 | % figure() 71 | fig_size = [18 * 0.5, 4.0] * 3 ; 72 | h = figure('Renderer', 'painters', 'unit', 'centimeters', 'Position', [0, 0, fig_size]); 73 | 74 | left_coner_list = [0.05, 0.38, 0.68]; 75 | % left_coner_list = [0.1, 0.6]; 76 | 77 | for fig_id = 1:2 78 | % subplot(1, 2, fig_id) 79 | subplot(1, 3, fig_id, 'Position', [left_coner_list(fig_id), 0.01, 0.3, 1]); 80 | hold on 81 | len = 0.1; 82 | for k = 1:dof + 1 83 | Rk = squeeze(R(k, :, :)); 84 | dx = Rk(:, 1) * len; 85 | 86 | if mod(k, 3) == 1 || k <= 10 % || true 87 | plot3([x(k), x(k) + dx(1)], [y(k), y(k) + dx(2)], [z(k), z(k) + dx(3)], "r-", "LineWidth",1); 88 | 89 | dy = Rk(:, 2) * len; 90 | plot3([x(k), x(k) + dy(1)], [y(k), y(k) + dy(2)], [z(k), z(k) + dy(3)], "g-", "LineWidth",1); 91 | 92 | dz = Rk(:, 3) * len; 93 | plot3([x(k), x(k) + dz(1)], [y(k), y(k) + dz(2)], [z(k), z(k) + dz(3)], "b-", "LineWidth",1); 94 | 95 | plot3([x(k), x_load(k)], [y(k), y_load(k)], [z(k), z_load(k)], "b--"); 96 | end 97 | end 98 | plot3(x, y, z, "-") 99 | plot3(x_load, y_load, z_load, ".", "MarkerSize", 6) 100 | plot3(x_load, y_load, z_load) 101 | 102 | [X, Y] = meshgrid(-2:0.1:2, -2:0.1:2); 103 | fig1 = mesh(X, Y, X * 0); 104 | [X, Y, Z] = cylinder(0.5, 200); 105 | surf(X + 0.6, Y + 0.5, Z * 6, 'facecolor','r','LineStyle','none','facealpha',0.2) 106 | 107 | grid on 108 | box on 109 | xlim([-0.2 - 0.5, 1.5 + 0.5]) 110 | ylim([-0.2 - 0.5, 1.5 + 0.5]) 111 | zlim([-0.1, 3.5]) 112 | daspect([1,1,1]) 113 | 114 | if fig_id == 1 115 | view(45, 35) 116 | end 117 | 118 | if fig_id == 2 119 | view(-60, 70) 120 | end 121 | 122 | lw_pos = 1.2; 123 | len_arrow = 0.1; 124 | font_size = 14; 125 | legend_size = 8; 126 | 127 | xlabel("$x$", "Interpreter", "latex", "FontSize", font_size) 128 | ylabel("$y$", "Interpreter", "latex", "FontSize", font_size) 129 | zlabel("$z$", "Interpreter", "latex", "FontSize", font_size) 130 | end 131 | %% 132 | fig_id = 3; 133 | subplot(1, 3, fig_id, 'Position', [0.75, 0.1, 0.20, 0.8]); 134 | hold on 135 | plot(x_load, y_load, "r-o", "MarkerSize", 2) 136 | plot(x, y, "b-o", "MarkerSize", 2) 137 | 138 | plot(p_tssos_load(1:dof+1), p_tssos_load(dof+2:2*(dof + 1)), "r--", "MarkerSize", 2) 139 | plot(p_tssos(1:dof+1), p_tssos(dof+2:2*(dof + 1)), "b--", "MarkerSize", 2) 140 | 141 | % plot(x, y, "b.") 142 | % plot(x_load, y_load, "b.") 143 | tt = 0:0.01:2 * pi; 144 | plot(cos(tt) * 0.5 + 0.6, 0.5 * sin(tt) + 0.5, "k--", "LineWidth", 1) 145 | grid on 146 | box on 147 | daspect([1, 1, 1]) 148 | xlim([-0.1, 1.2]) 149 | ylim([-0.1, 1.6]) 150 | xlabel("$x$", "Interpreter", "latex") 151 | ylabel("$y$", "Interpreter", "latex") 152 | legend("$p^{L}$ - Refined", "$p$ - Refined", "$p^L$ - SDP", "$p$ - SDP", "location", "northwest", "Interpreter", "latex") 153 | 154 | %% 155 | % fig_id = 3; 156 | % subplot(1, 3, fig_id, 'Position', [0.75, 0.1, 0.20, 0.8]); 157 | % hold on 158 | % plot(x_load, y_load, "-o", "MarkerSize", 2) 159 | % plot(x, y, "-o", "MarkerSize", 2) 160 | % % plot(x, y, "b.") 161 | % % plot(x_load, y_load, "b.") 162 | % tt = 0:0.01:2 * pi; 163 | % plot(cos(tt) * 0.5 + 0.6, 0.5 * sin(tt) + 0.5, "k--", "LineWidth", 1) 164 | % grid on 165 | % box on 166 | % daspect([1, 1, 1]) 167 | % xlim([-0.1, 1.2]) 168 | % ylim([-0.1, 1.6]) 169 | % xlabel("$x$", "Interpreter", "latex") 170 | % ylabel("$y$", "Interpreter", "latex") 171 | % legend("Load position", "Drone position", "location", "northwest", "Interpreter", "latex") 172 | %% 173 | set(h, 'Units','pixels'); 174 | set(h, 'PaperPositionMode','Auto','PaperUnits','centimeters','PaperSize', fig_size) 175 | print("v2_load_traj_" + num2str(case_num), "-dpdf") 176 | 177 | %% 178 | (opt_fmincon - opt) / opt_fmincon -------------------------------------------------------------------------------- /Drone-Load/refine_NC_5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Drone-Load/refine_NC_5.mat -------------------------------------------------------------------------------- /Drone-Load/refine_batch_fmincon.m: -------------------------------------------------------------------------------- 1 | clear all;clc;close all; 2 | %% 3 | for k = 5 4 | 5 | "k = " + num2str(k) 6 | file_name = "log_NC_" + num2str(k) + ".mat"; 7 | log = load(file_name); 8 | 9 | dof = double(log.dof); 10 | dx0 = double(log.dx0); 11 | dz0 = double(log.dz0); 12 | 13 | sol = log.sol; 14 | dt = log.dt; 15 | Mass = log.Mass; 16 | Inertial = log.Inertial; 17 | Qc = log.Qc; 18 | Rc = log.Rc; 19 | Pc = log.Pc; 20 | quat_init = log.quat_init; 21 | obs_flag = log.obs_flag; 22 | 23 | [R, F, p, v, tau, f, p_load, v_load, lam, opt_fmincon] = refine_solution_det_load(sol + randn(size(sol)) * 0e-2, dof, dt, Mass, Inertial, Qc, Rc, Pc, double(quat_init'), obs_flag, dx0, dz0); 24 | "k = " + num2str(k) + " completed" 25 | file_name = "refine_NC_" + num2str(k) + ".mat"; 26 | save_file(file_name, R, F, p, v, tau, f, p_load, v_load, lam, opt_fmincon) 27 | end 28 | %% 29 | function save_file(file_name, R, F, p, v, tau, f, p_load, v_load, lam, opt_fmincon) 30 | save(file_name, "R", "F", "p", "v", "tau", "f", "p_load", "v_load", "lam", "opt_fmincon") 31 | end -------------------------------------------------------------------------------- /Drone-Load/refine_solution_det_load.m: -------------------------------------------------------------------------------- 1 | function [R_num, F_num, p_num, v_num, tau_num, f_num, p_num_load, v_num_load, lam_num, obj] = refine_solution_det_load(sol, dof, dt, mass, inertial, Qc, Rc, Pc, quat_init, obs_flag, dx0_in, dz0_in) 2 | % constant 3 | % Jb = diag([1, 3, 6]); 4 | Jb = inertial; 5 | m = mass; 6 | m_load = 0.5; 7 | arm_length = 0.5; 8 | 9 | % dt = 0.0625;%25; 10 | g = [0;0;-9.81]; 11 | R_init = quat2rotm(quat_init); 12 | 13 | R_goal = eye(3); 14 | % 15 | yalmip('clear') 16 | R = sdpvar(dof+1, 3, 3); 17 | F = sdpvar(dof+1, 3, 3); 18 | 19 | p = sdpvar(dof+1, 3, 1); 20 | v = sdpvar(dof+1, 3, 1); 21 | 22 | tau = sdpvar(dof, 3, 1); 23 | f = sdpvar(dof, 1); 24 | 25 | p_load = sdpvar(dof+1, 3, 1); 26 | v_load = sdpvar(dof+1, 3, 1); 27 | 28 | % load 29 | lam = sdpvar(dof, 1); 30 | 31 | 32 | eq = []; 33 | ineq = []; 34 | f_act = 0; 35 | % initial condition 36 | dx0 = dx0_in; %2; 37 | dz0 = dz0_in; % (arm_length - sqrt(( arm_length^2 - (dx0 * dt)^2 ))) / dt; 38 | v0_load = [0; dx0; dz0]; 39 | eq = [eq; 40 | R(1,:,:) == R_init; 41 | F(1,:,:) == eye(3); 42 | p(1,:) == [1;1;3]'; 43 | v(1,:) == [0;0; 0]'; 44 | % load 45 | p_load(1,:) == [1;1;2.5]'; 46 | v_load(1,:) == v0_load'; 47 | ]; 48 | 49 | for k = 1:dof 50 | Rk = squeeze(R(k, :, :)); 51 | Rkp = squeeze(R(k+1,:,:)); 52 | Fk = squeeze(F(k, :, :)); 53 | Fkp = squeeze(F(k+1,:,:)); 54 | 55 | pk = squeeze(p(k, :, :))'; 56 | pkp = squeeze(p(k+1,:,:))'; 57 | vk = squeeze(v(k, :, :))'; 58 | vkp = squeeze(v(k+1,:,:))'; 59 | 60 | pk_load = squeeze(p_load(k, :, :))'; 61 | pkp_load = squeeze(p_load(k+1,:,:))'; 62 | vk_load = squeeze(v_load(k, :, :))'; 63 | vkp_load = squeeze(v_load(k+1,:,:))'; 64 | 65 | tauk = tau(k, :)'; 66 | fk = f(k); 67 | lamk = lam(k); 68 | % SO3 constraints 69 | IR = Rk' * Rk - eye(3); 70 | IF = Fk' * Fk - eye(3); 71 | eq = [eq; 72 | IR(1,1) == 0; 73 | IR(2,2) == 0; 74 | IR(3,3) == 0; 75 | IR(1,2) == 0; 76 | IR(1,3) == 0; 77 | IR(2,3) == 0; 78 | 79 | IF(1,1) == 0; 80 | IF(2,2) == 0; 81 | IF(3,3) == 0; 82 | IF(1,2) == 0; 83 | IF(1,3) == 0; 84 | IF(2,3) == 0]; 85 | dR = Rkp - Rk * Fk; 86 | eq = [eq; 87 | dR(1,1) == 0; 88 | dR(2,2) == 0; 89 | dR(3,3) == 0; 90 | dR(1,2) == 0; 91 | dR(1,3) == 0; 92 | dR(2,3) == 0; 93 | % 94 | pkp - (pk + dt * Rk * vk) == 0; 95 | pkp_load - (pk_load + dt * vk_load) == 0]; 96 | % dynamics constraints 97 | dM = Fkp * Jb - Jb * Fkp' - (Jb * Fk - Fk' * Jb); 98 | 99 | arm = pkp - pkp_load; 100 | 101 | dv = m * vkp - (m * Fk' * vk + ( fk * [0;0;1] + Rkp' * (m * g + lamk * arm) ) * dt ); 102 | dv_load = m_load * vkp_load - (m_load * vk_load + (-lamk * arm + m_load * g) * dt ); 103 | 104 | eq = [eq; 105 | dM(1, 2) + (-tauk(3)) * dt^2 == 0; 106 | dM(1, 3) + ( tauk(2)) * dt^2 == 0; 107 | dM(2, 3) + (-tauk(1)) * dt^2 == 0; 108 | dv == 0; 109 | dv_load == 0; 110 | sum(arm.^2) - arm_length.^2 == 0]; 111 | 112 | p_goal = [0, 0, 0.5]'; 113 | p_goal_load = [0, 0, 0]'; 114 | 115 | R_cost = Qc(1) * trace((Rk - R_goal)' * (Rk - R_goal)); 116 | w_cost = Qc(2) * trace( (Fk - eye(3))' * (Fk - eye(3)) ); 117 | p_cost = Qc(3) * sum((pk - p_goal).^2); 118 | v_cost = Qc(4) * sum(vk.^2); 119 | 120 | p_load_cost = Qc(3) * sum((pk_load - p_goal_load).^2); 121 | v_load_cost = Qc(4) * sum(vk_load.^2); 122 | 123 | ctr_cost = Rc * sum(tauk.^2) + Rc* fk^2; 124 | f_act = f_act + R_cost + w_cost + p_cost + v_cost + ctr_cost + p_load_cost + v_load_cost; 125 | 126 | ineq = [ineq; 127 | -5 <= tauk; 128 | tauk <= 5; 129 | 130 | sum((pkp - pk).^2) <= 0.45^2; 131 | sum((pkp_load - pk_load).^2) <= 0.45^2; 132 | % -7 <= fk; 133 | % fk <= 7; 134 | % (pkp(2) - 0.5)^2 + (pkp(1) - 0.5)^2 >= 0.25; 135 | % (pkp(1))^2 + (pkp(2) - 0.5)^2 >= 0.25; 136 | % (pkp(1) - 0.6)^2 + (pkp(2) - 0.5)^2 >= 0.25; 137 | pkp(3) >= 0]; 138 | if obs_flag 139 | ineq = [ineq; 140 | (pkp(1) - 0.6)^2 + (pkp(2) - 0.5)^2 >= 0.25; 141 | (pkp_load(1) - 0.6)^2 + (pkp_load(2) - 0.5)^2 >= 0.25]; 142 | end 143 | end 144 | 145 | R_cost_term = Pc(1) * trace((Rkp - R_goal)' * (Rkp - R_goal)); 146 | F_cost_term = Pc(2) * trace((Fkp - eye(3))' * (Fkp - eye(3))); 147 | p_cost_term = Pc(3) * sum((pkp - p_goal).^2); 148 | v_cost_term = Pc(4) * sum(vkp.^2); 149 | 150 | p_cost_term_load = Pc(3) * sum((pkp_load - p_goal_load).^2); 151 | v_cost_term_load = Pc(4) * sum(vkp_load.^2); 152 | 153 | 154 | cost = f_act * dt / 0.25 + R_cost_term + F_cost_term + p_cost_term + v_cost_term; 155 | cost = cost + p_cost_term_load + v_cost_term_load; 156 | cost = cost * 10; 157 | %% 158 | dof = double(dof); 159 | R_num = sol(1:9 * (dof + 1)); 160 | F_num = sol(1+9 * (dof + 1):9 * (dof + 1) * 2); 161 | 162 | shift = 9 * (dof + 1) * 2; 163 | p_num = sol( 1+shift : shift+3*(dof+1)); 164 | v_num = sol(1+shift+3*(dof+1) : shift+6*(dof+1)); 165 | p_num = reshape(p_num, [dof+1, 3]); 166 | v_num = reshape(v_num, [dof+1, 3]); 167 | 168 | shift = shift+6*(dof+1); 169 | control = sol(shift+1:shift + 4 * dof); 170 | 171 | shift = shift+4*dof; 172 | p_num_load = sol( 1+shift : shift+3*(dof+1)); 173 | v_num_load = sol(1+shift+3*(dof+1) : shift+6*(dof+1)); 174 | p_num_load = reshape(p_num_load, [dof+1, 3]); 175 | v_num_load = reshape(v_num_load, [dof+1, 3]); 176 | 177 | shift = shift+6*(dof+1); 178 | lam_num = sol(shift+1:end); 179 | 180 | R_num = reshape(R_num, [dof+1, 3, 3]); 181 | F_num = reshape(F_num, [dof+1, 3, 3]); 182 | 183 | %%% project to SO(3) %%% 184 | % for k = 1:dof+1 185 | % Rk_before = squeeze(R_num(k,:,:)); 186 | % [U, ~, V] = svd(Rk_before); 187 | % Rk_hat = U * diag([1, 1, det(U) * det(V)]) * V'; 188 | % R_num(k,:,:) = Rk_hat; 189 | % 190 | % Fk_before = squeeze(F_num(k,:,:)); 191 | % [U, ~, V] = svd(Fk_before); 192 | % Fk_hat = U * diag([1, 1, det(U) * det(V)]) * V'; 193 | % F_num(k,:,:) = Fk_hat; 194 | % end 195 | %%% project to SO(3) %%% 196 | 197 | 198 | tau_num = reshape(control(1:dof*3), [dof, 3]); 199 | tau_num(tau_num > 5) = 5; 200 | tau_num(tau_num < -5) = -5; 201 | f_num = reshape(control(dof*3+1:end), [dof, 1]); 202 | f_num(f_num > 30) = 30; 203 | f_num(f_num < -30) = -30; 204 | 205 | assign(R, R_num) 206 | assign(F, F_num) 207 | assign(p, p_num) 208 | assign(v, v_num) 209 | assign(tau, tau_num) 210 | assign(f, f_num) 211 | 212 | assign(p_load, p_num_load) 213 | assign(v_load, v_num_load) 214 | assign(lam, lam_num) 215 | 216 | %% 217 | % ops = sdpsettings('solver','fmincon');%,'fmincon.algorithm','sqp'); 218 | ops = sdpsettings('solver','ipopt'); 219 | ops.print_level = 12; 220 | ops.verbose = true; 221 | 222 | % ops.fmincon.MaxIterations = 1e7; 223 | ops.ipopt.max_iter = 1e4 * 10; 224 | ops.ipopt.max_cpu_time = 1e100; 225 | 226 | ops.usex0 = 1; 227 | optimize([eq; ineq],cost,ops); 228 | %% 229 | R_num = value(R); 230 | F_num = value(F); 231 | p_num = value(p); 232 | v_num = value(v); 233 | tau_num = value(tau); 234 | 235 | p_num_load = value(p_load); 236 | v_num_load = value(v_load); 237 | lam_num = value(lam); 238 | 239 | f_num = value(f); 240 | obj = value(cost); 241 | end -------------------------------------------------------------------------------- /Drone-Load/v2_load_traj_5.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Drone-Load/v2_load_traj_5.pdf -------------------------------------------------------------------------------- /Drone/drone.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 9, 6 | "id": "21e7bfcc", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "using DynamicPolynomials\n", 11 | "using TSSOS\n", 12 | "using MAT\n", 13 | "using LinearAlgebra\n", 14 | "using Printf" 15 | ] 16 | }, 17 | { 18 | "cell_type": "code", 19 | "execution_count": 10, 20 | "id": "bf7bfc61", 21 | "metadata": {}, 22 | "outputs": [ 23 | { 24 | "data": { 25 | "text/plain": [ 26 | "SO3_skew (generic function with 1 method)" 27 | ] 28 | }, 29 | "execution_count": 10, 30 | "metadata": {}, 31 | "output_type": "execute_result" 32 | } 33 | ], 34 | "source": [ 35 | "@polyvar temp1[1:4, 1:4]\n", 36 | "@polyvar temp2[1:4, 1:4]\n", 37 | "temp_p = temp2 * 1.1 + temp1 * 1.1\n", 38 | "\n", 39 | "function Joint2SE3(c, s, x, y, z)\n", 40 | " ## we always rotate about the z axis\n", 41 | " T = temp_p * 0\n", 42 | " T[1,1] = c\n", 43 | " T[2,2] = c\n", 44 | " T[1,2] = -s\n", 45 | " T[2,1] = s\n", 46 | " T[3,3] = 1\n", 47 | " \n", 48 | " T[1,4] = c * x - s * y\n", 49 | " T[2,4] = s * x + c * y\n", 50 | " T[3,4] = z\n", 51 | " T[4,4] = 1\n", 52 | " return T\n", 53 | "end\n", 54 | "\n", 55 | "function SE3_Euler_num(y, p, r, lx, ly, lz)\n", 56 | " T = temp_p * 0\n", 57 | " T[1:3, 1:3] = rotz_num(y) * roty_num(p) * rotx_num(r)\n", 58 | " T[4,4] = 1.0\n", 59 | " T[1,4] = lx\n", 60 | " T[2,4] = ly\n", 61 | " T[3,4] = lz\n", 62 | " return T\n", 63 | "end\n", 64 | "\n", 65 | "function rotx_num(t)\n", 66 | " R = temp_p[1:3, 1:3] * 0\n", 67 | " R[2,2] = cos(t)\n", 68 | " R[3,3] = cos(t)\n", 69 | " R[2,3] = -sin(t)\n", 70 | " R[3,2] = sin(t)\n", 71 | " R[1,1] = 1.0\n", 72 | " return R\n", 73 | "end\n", 74 | "\n", 75 | "function roty_num(t)\n", 76 | " R = temp_p[1:3, 1:3] * 0\n", 77 | " R[1,1] = cos(t)\n", 78 | " R[3,3] = cos(t)\n", 79 | " R[1,3] = sin(t)\n", 80 | " R[3,1] = -sin(t)\n", 81 | " R[2,2] = 1.0\n", 82 | " return R\n", 83 | "end\n", 84 | "\n", 85 | "function rotz_num(t)\n", 86 | " R = temp_p[1:3, 1:3] * 0\n", 87 | " R[1,1] = cos(t)\n", 88 | " R[2,2] = cos(t)\n", 89 | " R[1,2] = -sin(t)\n", 90 | " R[2,1] = sin(t)\n", 91 | " R[3,3] = 1.0\n", 92 | " return R\n", 93 | "end\n", 94 | "\n", 95 | "function quat2rot_num(w, x, y, z)\n", 96 | " R = temp_p[1:3, 1:3] * 0\n", 97 | " nq = sqrt(w^2 + x^2 + y^2 + z^2)\n", 98 | " w = w / nq\n", 99 | " x = x / nq\n", 100 | " y = y / nq\n", 101 | " z = z / nq\n", 102 | " \n", 103 | " R[1,1] = 1 - 2 * (y^2 + z^2)\n", 104 | " R[2,2] = 1 - 2 * (x^2 + z^2)\n", 105 | " R[3,3] = 1 - 2 * (x^2 + y^2)\n", 106 | " \n", 107 | " R[1,2] = 2 * (x * y - w * z)\n", 108 | " R[1,3] = 2 * (w * y + x * z)\n", 109 | " R[2,3] = 2 * (y * z - w * x)\n", 110 | " \n", 111 | " R[2,1] = 2 * (x * y + w * z)\n", 112 | " R[3,1] = 2 * (- w * y + x * z)\n", 113 | " R[3,2] = 2 * (y * z + w * x)\n", 114 | " return R\n", 115 | "end\n", 116 | "\n", 117 | "function cross_(x, y) \n", 118 | " z = temp_p[1:3, 1]\n", 119 | " z[1] = -x[3] * y[2] + x[2] * y[3]\n", 120 | " z[2] = x[3] * y[1] - x[1] * y[3]\n", 121 | " z[3] = -x[2] * y[1] + x[1] * y[2]\n", 122 | " return z\n", 123 | "end\n", 124 | "\n", 125 | "function add_SO3_cons(R, eq)\n", 126 | " T = R' * R\n", 127 | "# print(T[1,1] - 1.0)\n", 128 | " append!(eq, [T[1,1] - 1.0])\n", 129 | " append!(eq, [T[2,2] - 1.0])\n", 130 | " append!(eq, [T[3,3] - 1.0])\n", 131 | " append!(eq, [T[1,2]])\n", 132 | " append!(eq, [T[1,3]])\n", 133 | " append!(eq, [T[2,3]])\n", 134 | " \n", 135 | "# append!(eq, cross_(R[1,:], R[2, :]) - R[3,:])\n", 136 | "# append!(eq, cross_(R[2,:], R[3, :]) - R[1,:])\n", 137 | "# append!(eq, cross_(R[3,:], R[1, :]) - R[2,:])\n", 138 | "\n", 139 | " return eq\n", 140 | "end\n", 141 | "\n", 142 | "function add_SE3_tran_cons(T1, T2, eq)\n", 143 | " T = T1 - T2\n", 144 | " \n", 145 | " append!(eq, T[1, 1])\n", 146 | " append!(eq, T[1, 2])\n", 147 | " append!(eq, T[1, 3])\n", 148 | " \n", 149 | " \n", 150 | " append!(eq, T[2, 1])\n", 151 | " append!(eq, T[2, 2])\n", 152 | " append!(eq, T[2, 3])\n", 153 | " \n", 154 | " \n", 155 | " append!(eq, T[3, 1])\n", 156 | " append!(eq, T[3, 2])\n", 157 | " append!(eq, T[3, 3])\n", 158 | " \n", 159 | " append!(eq, T[1, 4])\n", 160 | " append!(eq, T[2, 4])\n", 161 | " append!(eq, T[3, 4]) \n", 162 | " return eq\n", 163 | "end\n", 164 | "\n", 165 | "function add_SO3_tran_cons(T1, T2, eq)\n", 166 | " T = T1 - T2\n", 167 | " \n", 168 | "# print(\"eq_init = \")\n", 169 | "# print(size(eq))\n", 170 | "# print(\"\\n\")\n", 171 | " \n", 172 | "# print(\"trans 0 = \")\n", 173 | "# print(size(eq)) \n", 174 | "# print(\"\\n\")\n", 175 | " \n", 176 | " append!(eq, [T[1, 1]])\n", 177 | " append!(eq, [T[1, 2]])\n", 178 | " append!(eq, [T[1, 3]])\n", 179 | "# print(\"trans 1 = \")\n", 180 | "# print(size(eq)) \n", 181 | "# print(\"\\n\")\n", 182 | " \n", 183 | " \n", 184 | " append!(eq, [T[2, 1]])\n", 185 | " append!(eq, [T[2, 2]])\n", 186 | " append!(eq, [T[2, 3]])\n", 187 | "# print(\"trans 2 = \")\n", 188 | "# print(size(eq)) \n", 189 | "# print(\"\\n\")\n", 190 | " \n", 191 | " \n", 192 | " append!(eq, [T[3, 1]])\n", 193 | " append!(eq, [T[3, 2]])\n", 194 | " append!(eq, [T[3, 3]])\n", 195 | "# print(\"trans 3 = \")\n", 196 | "# print(size(eq)) \n", 197 | "# print(\"\\n\")\n", 198 | " \n", 199 | " return eq\n", 200 | "end\n", 201 | "\n", 202 | "function make_SE3(R, x, y, z)\n", 203 | " T = temp_p * 0\n", 204 | " T[1:3, 1:3] = R\n", 205 | " T[1,4] = x\n", 206 | " T[2,4] = y\n", 207 | " T[3,4] = z\n", 208 | " T[4,4] = 1.0\n", 209 | " return T\n", 210 | "end\n", 211 | "\n", 212 | "function SE3_inverse(T)\n", 213 | " Tnew = T\n", 214 | " Tnew[1:3, 1:3] = T[1:3, 1:3]'\n", 215 | " Tnew[1:3, 4] = - T[1:3, 1:3]' * T[1:3, 4]\n", 216 | " return Tnew\n", 217 | "end\n", 218 | "\n", 219 | "function SO3_skew(T)\n", 220 | " \n", 221 | " \n", 222 | "end\n", 223 | "\n", 224 | "# Joint2SE3(c[1], s[1], x[1], y[1], z[1])\n", 225 | "# R = quat2rot_num(randn(), randn(), randn(), randn())\n", 226 | "# R * R'\n", 227 | "# rotz_num(c[1,1])\n", 228 | "# T = SE3_Euler_num(randn(), randn(), randn(), randn(), randn(), randn())\n", 229 | "# cross(R[1, 1,:], R[1, 2, :])" 230 | ] 231 | }, 232 | { 233 | "cell_type": "code", 234 | "execution_count": 11, 235 | "id": "7cc5b380", 236 | "metadata": {}, 237 | "outputs": [ 238 | { 239 | "name": "stdout", 240 | "output_type": "stream", 241 | "text": [ 242 | "(18,)" 243 | ] 244 | }, 245 | { 246 | "ename": "LoadError", 247 | "evalue": "UndefVarError: cs_tssos_first_export_tsl not defined", 248 | "output_type": "error", 249 | "traceback": [ 250 | "UndefVarError: cs_tssos_first_export_tsl not defined", 251 | "", 252 | "Stacktrace:", 253 | " [1] test_SO3(dof::Int64, dt::Float64, obs_flag::Bool, quat_init::Vector{Float64}, mode::Int64, obs_id::Int64)", 254 | " @ Main .\\In[11]:137", 255 | " [2] top-level scope", 256 | " @ In[11]:164", 257 | " [3] eval", 258 | " @ .\\boot.jl:368 [inlined]", 259 | " [4] include_string(mapexpr::typeof(REPL.softscope), mod::Module, code::String, filename::String)", 260 | " @ Base .\\loading.jl:1428" 261 | ] 262 | } 263 | ], 264 | "source": [ 265 | "function test_SO3(dof, dt, obs_flag, quat_init, mode, obs_id)\n", 266 | "\n", 267 | " @polyvar R[1:dof+1, 1:3, 1:3] \n", 268 | " @polyvar F[1:dof+1, 1:3, 1:3]\n", 269 | "\n", 270 | "# @polyvar p[1:dof+1, 1:3] \n", 271 | "# @polyvar v[1:dof+1, 1:3]\n", 272 | "\n", 273 | " @polyvar tau[1:dof, 1:3]\n", 274 | "# @polyvar f[1:dof, 1]\n", 275 | "\n", 276 | " Jb = Matrix(1.0I, 3, 3)\n", 277 | " Jb[1,1] = 1.0 / 10\n", 278 | " Jb[2,2] = 2.0 / 10\n", 279 | " Jb[3,3] = 1.0 / 10\n", 280 | "\n", 281 | " m = 1 / 2\n", 282 | "\n", 283 | " var = append!(reshape(R, 9 * (dof+1)), reshape(F, 9 * (dof + 1)), reshape(tau, 3 * dof))\n", 284 | "\n", 285 | " eq = [temp_p[1,1] * 0]\n", 286 | "\n", 287 | " # R_goal = quat2rot_num(1, 0, 0, 0); \n", 288 | " # R_goal = quat2rot_num(1, 0, 1, 0); \n", 289 | " # R_init = quat2rot_num(0.86, 0, 0.5, 0); \n", 290 | " # R_goal = quat2rot_num(0.5, 0, 0.86, 0); \n", 291 | "\n", 292 | " R_init = quat2rot_num(quat_init[1], quat_init[2], quat_init[3], quat_init[4]); \n", 293 | "\n", 294 | " eq[1] = R[1, 1, 1] - R_init[1, 1]\n", 295 | " append!(eq, [R[1, 1, 2] - R_init[1, 2]])\n", 296 | " append!(eq, [R[1, 1, 3] - R_init[1, 3]])\n", 297 | " append!(eq, [R[1, 2, 1] - R_init[2, 1]])\n", 298 | " append!(eq, [R[1, 2, 2] - R_init[2, 2]])\n", 299 | " append!(eq, [R[1, 2, 3] - R_init[2, 3]])\n", 300 | " append!(eq, [R[1, 3, 1] - R_init[3, 1]])\n", 301 | " append!(eq, [R[1, 3, 2] - R_init[3, 2]])\n", 302 | " append!(eq, [R[1, 3, 3] - R_init[3, 3]])\n", 303 | "\n", 304 | " append!(eq, [F[1, 1, 1] - 1.0])\n", 305 | " append!(eq, [F[1, 1, 2] - 0.0])\n", 306 | " append!(eq, [F[1, 1, 3] - 0.0])\n", 307 | " append!(eq, [F[1, 2, 1] - 0.0])\n", 308 | " append!(eq, [F[1, 2, 2] - 1.0])\n", 309 | " append!(eq, [F[1, 2, 3] - 0.0])\n", 310 | " append!(eq, [F[1, 3, 1] - 0.0])\n", 311 | " append!(eq, [F[1, 3, 2] - 0.0])\n", 312 | " append!(eq, [F[1, 3, 3] - 1.0])\n", 313 | "\n", 314 | " print(size(eq))\n", 315 | " # print(\"hahahah\\n\")\n", 316 | " f_act = 0\n", 317 | "\n", 318 | " g = [0;0;-9.81]\n", 319 | "\n", 320 | " ineq = []\n", 321 | "\n", 322 | " Qc = [1e-1; 10; 1e-1; 1] # R w p v\n", 323 | " Rc = 1e-2 # rt rf\n", 324 | " Pc = [100; 10; 100; 100] * 1.1 / 1.1 # \n", 325 | "\n", 326 | " for k = 1:dof\n", 327 | " # print(\"hahahah\\n\")\n", 328 | " ## kinematics constraints\n", 329 | "# eq = add_SO3_cons(R[k+1,:,:], eq) # SO3 constraints\n", 330 | "# eq = add_SO3_cons(F[k+1,:,:], eq) # SO3 constraints\n", 331 | " eq = add_SO3_tran_cons(R[k+1, :, :], R[k, :, :]' * F[k,: , :], eq)\n", 332 | "\n", 333 | " ## dynamics constraints\n", 334 | " Fkp1 = F[k+1, :, :];\n", 335 | " Fk = F[k, :, :];\n", 336 | " Mkp1 = Fkp1 * Jb - Jb * Fkp1'\n", 337 | " Mk = Jb * Fk - Fk' * Jb\n", 338 | " dM = Mkp1 - Mk # SO3\n", 339 | "\n", 340 | " append!(eq, [dM[1, 2] + (-tau[k, 3]) * dt^2 ])\n", 341 | " append!(eq, [dM[1, 3] + (tau[k, 2]) * dt^2 ])\n", 342 | " append!(eq, [dM[2, 3] + (-tau[k, 1]) * dt^2 ])\n", 343 | "\n", 344 | "\n", 345 | " tau_max = 5\n", 346 | " # f_max = 7\n", 347 | "\n", 348 | "\n", 349 | " R_diff = R[k, :, :] - Matrix(1.0I, 3, 3) # + randn(3, 3) * 10\n", 350 | " R_diff = R_diff' * R_diff\n", 351 | "\n", 352 | "\n", 353 | " F_cost = F[k, : , :] - Matrix(1.0I, 3, 3) # + randn(3, 3) * 10\n", 354 | " F_cost = F_cost' * F_cost\n", 355 | "\n", 356 | " R_cost = R_diff[1,1] + R_diff[2,2] + R_diff[3,3]\n", 357 | " w_cost = F_cost[1, 1] + F_cost[2, 2] + F_cost[3, 3]\n", 358 | "\n", 359 | " f_act = f_act + Rc * (tau[k, 1]^2 + tau[k, 2]^2 + tau[k, 3]^2) # + Rc * f[k, 1]^2\n", 360 | " f_act = f_act + Qc[1] * R_cost\n", 361 | " f_act = f_act + Qc[2] * w_cost\n", 362 | " \n", 363 | " ineq = append!(ineq, [10.0 - sum(F[k+1, :, :].^2)])\n", 364 | " ineq = append!(ineq, [10.0 - sum(R[k+1, :, :].^2)])\n", 365 | " end\n", 366 | "\n", 367 | " # orientation error\n", 368 | " p_goal = [0, 0,0.0]\n", 369 | "\n", 370 | " R_diff = R[end, :, :] - Matrix(1.0I, 3, 3) # + randn(3, 3) * 10\n", 371 | " R_diff = R_diff' * R_diff\n", 372 | " f_R = R_diff[1,1] + R_diff[2,2] + R_diff[3,3]\n", 373 | "\n", 374 | " # angular velocity error\n", 375 | " F_diff = F[end, :, :] - Matrix(1.0I, 3, 3) # + randn(3, 3) * 10\n", 376 | " F_diff = F_diff' * F_diff\n", 377 | " f_w = F_diff[1,1] + F_diff[2,2] + F_diff[3,3]\n", 378 | "\n", 379 | " # position cost\n", 380 | "# f_p = sum((p[end,:] - p_goal).^2)\n", 381 | "# f_v = sum((v[end,:]).^2)\n", 382 | "\n", 383 | " cost = f_act * dt / 0.25\n", 384 | " cost = cost + Pc[1] * f_R + Pc[2] * f_w # + Pc[3] * f_p + Pc[4] * f_v\n", 385 | "\n", 386 | "# cost = cost + 1e-2 * sum(var.^4)\n", 387 | " \n", 388 | " \n", 389 | " \n", 390 | " pop = append!([cost * 10], ineq, eq)\n", 391 | " eq_len = length(eq)\n", 392 | "\n", 393 | " order = 2\n", 394 | " \n", 395 | " time_start = time()\n", 396 | " if mode == 1\n", 397 | " opt,sol,data,model,sol_approx =cs_tssos_first_export_tsl(pop, var, order, numeq=eq_len, CS=\"NC\", TS = \"MD\", solution=true, QUIET=false, MomentOne=false)\n", 398 | " elseif mode == 2\n", 399 | " opt,sol,data,model,sol_approx =cs_tssos_first_export_tsl(pop, var, order, numeq=eq_len, CS=\"NC\", TS = \"block\", solution=true, QUIET=false, MomentOne=false)\n", 400 | " elseif mode == 3\n", 401 | " opt,sol,data,model,sol_approx =cs_tssos_first_export_tsl(pop, var, order, numeq=eq_len, CS=\"NC\", TS = false, solution=true, QUIET=false, MomentOne=false)\n", 402 | " end\n", 403 | " time_end = time()\n", 404 | " elapsed = time_end - time_start\n", 405 | "\n", 406 | " \n", 407 | " moment = []\n", 408 | " for k = 1:length(data.Mmatrix)\n", 409 | " append!(moment, [convert(Matrix{Float64}, data.Mmatrix[k])]) # data.Mmatrix[k])\n", 410 | " end\n", 411 | "\n", 412 | "\n", 413 | " log = Dict(\"sol\" => sol, \"sol_approx\" => sol_approx, \"dof\" => dof, \n", 414 | " \"moment\" => moment, \"opt\" => opt, \"dt\"=>dt, \"Mass\"=>m, \"Inertial\"=>Jb, \n", 415 | " \"quat_init\"=> quat_init, \"Qc\"=> Qc, \"Rc\"=>Rc, \"Pc\"=>Pc, \"obs_flag\"=>obs_flag,\n", 416 | " \"solver_flag\"=>data.flag, \"elapsed\"=>elapsed)\n", 417 | " return log\n", 418 | "end\n", 419 | "quat_list = [[0.86;0;0.5;0], [1;0;0;0], [1;0;1;0], [0.5, 0, 0.85, 0], [0,0,1,0]]\n", 420 | "obs_list = [false; true]\n", 421 | "mode_list = [1; 2; 3] # step using pure cs at this moment. \n", 422 | "\n", 423 | "dof = 2\n", 424 | "dt = 0.1\n", 425 | "h = 1\n", 426 | "k = 1\n", 427 | "\n", 428 | "log = test_SO3(dof, dt, obs_list[h], quat_list[k], 3, 2)" 429 | ] 430 | }, 431 | { 432 | "cell_type": "code", 433 | "execution_count": 12, 434 | "id": "b7f15ed0", 435 | "metadata": {}, 436 | "outputs": [ 437 | { 438 | "data": { 439 | "text/plain": [ 440 | "test_drone (generic function with 1 method)" 441 | ] 442 | }, 443 | "execution_count": 12, 444 | "metadata": {}, 445 | "output_type": "execute_result" 446 | } 447 | ], 448 | "source": [ 449 | "function test_drone(dof, dt, obs_flag, quat_init, mode, obs_id)\n", 450 | "\n", 451 | " @polyvar R[1:dof+1, 1:3, 1:3] \n", 452 | " @polyvar F[1:dof+1, 1:3, 1:3]\n", 453 | "\n", 454 | " @polyvar p[1:dof+1, 1:3] \n", 455 | " @polyvar v[1:dof+1, 1:3]\n", 456 | "\n", 457 | " @polyvar tau[1:dof, 1:3]\n", 458 | " @polyvar f[1:dof, 1]\n", 459 | "\n", 460 | " Jb = Matrix(1.0I, 3, 3)\n", 461 | " Jb[1,1] = 1.0 / 10\n", 462 | " Jb[2,2] = 2.0 / 10\n", 463 | " Jb[3,3] = 1.0 / 10\n", 464 | "\n", 465 | " m = 1 / 2\n", 466 | "\n", 467 | " var = append!(reshape(R, 9 * (dof+1)), reshape(F, 9 * (dof + 1)), reshape(p, 3 * (dof + 1)), reshape(v, 3 * (dof + 1)), reshape(tau, 3 * dof), reshape(f, dof))\n", 468 | "\n", 469 | " eq = [temp_p[1,1] * 0]\n", 470 | "\n", 471 | " # R_goal = quat2rot_num(1, 0, 0, 0); \n", 472 | " # R_goal = quat2rot_num(1, 0, 1, 0); \n", 473 | " # R_init = quat2rot_num(0.86, 0, 0.5, 0); \n", 474 | " # R_goal = quat2rot_num(0.5, 0, 0.86, 0); \n", 475 | "\n", 476 | " R_init = quat2rot_num(quat_init[1], quat_init[2], quat_init[3], quat_init[4]); \n", 477 | "\n", 478 | " eq[1] = R[1, 1, 1] - R_init[1, 1]\n", 479 | " append!(eq, [R[1, 1, 2] - R_init[1, 2]])\n", 480 | " append!(eq, [R[1, 1, 3] - R_init[1, 3]])\n", 481 | " append!(eq, [R[1, 2, 1] - R_init[2, 1]])\n", 482 | " append!(eq, [R[1, 2, 2] - R_init[2, 2]])\n", 483 | " append!(eq, [R[1, 2, 3] - R_init[2, 3]])\n", 484 | " append!(eq, [R[1, 3, 1] - R_init[3, 1]])\n", 485 | " append!(eq, [R[1, 3, 2] - R_init[3, 2]])\n", 486 | " append!(eq, [R[1, 3, 3] - R_init[3, 3]])\n", 487 | "\n", 488 | " append!(eq, [F[1, 1, 1] - 1.0])\n", 489 | " append!(eq, [F[1, 1, 2] - 0.0])\n", 490 | " append!(eq, [F[1, 1, 3] - 0.0])\n", 491 | " append!(eq, [F[1, 2, 1] - 0.0])\n", 492 | " append!(eq, [F[1, 2, 2] - 1.0])\n", 493 | " append!(eq, [F[1, 2, 3] - 0.0])\n", 494 | " append!(eq, [F[1, 3, 1] - 0.0])\n", 495 | " append!(eq, [F[1, 3, 2] - 0.0])\n", 496 | " append!(eq, [F[1, 3, 3] - 1.0])\n", 497 | "\n", 498 | " append!(eq, [p[1,1] - 1.0])\n", 499 | " append!(eq, [p[1,2] - 1.0])\n", 500 | " append!(eq, [p[1,3] - 3.0])\n", 501 | "\n", 502 | " append!(eq, [v[1,1] - 0.])\n", 503 | " append!(eq, [v[1,2] - 0.])\n", 504 | " append!(eq, [v[1,3] - 0.])\n", 505 | " # append!(eq, [v[1,1] - randn() * 0.1])\n", 506 | " # append!(eq, [v[1,2] - randn() * 0.1])\n", 507 | " # append!(eq, [v[1,3] - randn() * 0.1])\n", 508 | "\n", 509 | " print(size(eq))\n", 510 | " # print(\"hahahah\\n\")\n", 511 | " f_act = 0\n", 512 | "\n", 513 | " g = [0;0;-9.81]\n", 514 | "\n", 515 | " ineq = []\n", 516 | "\n", 517 | " Qc = [1e-1; 10; 1e-1; 1] # R w p v\n", 518 | " Rc = 1e-2 # rt rf\n", 519 | " Pc = [100; 10; 100; 100] * 1.1 / 1.1 # \n", 520 | "\n", 521 | " for k = 1:dof\n", 522 | " # print(\"hahahah\\n\")\n", 523 | " ## kinematics constraints\n", 524 | " eq = add_SO3_cons(R[k+1,:,:], eq) # SO3 constraints\n", 525 | " eq = add_SO3_cons(F[k+1,:,:], eq) # SO3 constraints\n", 526 | " eq = add_SO3_tran_cons(R[k+1, :, :], R[k, :, :] * F[k,: , :], eq)\n", 527 | "\n", 528 | " p_diff = p[k+1, :] - (p[k, :] + dt * R[k,:, :] * v[k,:]) # position\n", 529 | "\n", 530 | "# append!(eq, [p_diff[1]])\n", 531 | "# append!(eq, [p_diff[2]])\n", 532 | "# append!(eq, [p_diff[3]])\n", 533 | " # print(\"hahahah\\n\")\n", 534 | "\n", 535 | " ## dynamics constraints\n", 536 | " Fkp1 = F[k+1, :, :];\n", 537 | " Fk = F[k, :, :];\n", 538 | " Mkp1 = Fkp1 * Jb - Jb * Fkp1'\n", 539 | " Mk = Jb * Fk - Fk' * Jb\n", 540 | " dM = Mkp1 - Mk # SO3\n", 541 | "\n", 542 | " append!(eq, [dM[1, 2] + (-tau[k, 3]) * dt^2 ])\n", 543 | " append!(eq, [dM[1, 3] + (tau[k, 2]) * dt^2 ])\n", 544 | " append!(eq, [dM[2, 3] + (-tau[k, 1]) * dt^2 ])\n", 545 | "\n", 546 | "\n", 547 | " tau_max = 5\n", 548 | " # f_max = 7\n", 549 | "\n", 550 | " # append!(ineq, [f_max - f[k]])\n", 551 | " # append!(ineq, [f_max + f[k]])\n", 552 | " append!(ineq, [tau_max - tau[k, 1]])\n", 553 | " append!(ineq, [tau_max + tau[k, 1]])\n", 554 | " append!(ineq, [tau_max - tau[k, 2]])\n", 555 | " append!(ineq, [tau_max + tau[k, 2]])\n", 556 | " append!(ineq, [tau_max - tau[k, 3]])\n", 557 | " append!(ineq, [tau_max + tau[k, 3]])\n", 558 | "\n", 559 | " append!(ineq, [p[k+1, 3]])\n", 560 | "\n", 561 | " if obs_flag == true\n", 562 | " if obs_id == 1\n", 563 | " append!(ineq, [(p[k+1, 1])^2 + (p[k+1, 2] - 0.5)^2 - 0.25])\n", 564 | " else\n", 565 | " append!(ineq, [(p[k+1, 1] - 0.6)^2 + (p[k+1, 2] - 0.5)^2 - 0.25])\n", 566 | " end\n", 567 | "# append!(ineq, [(p[k+1, 1] - 0.5)^2 + (p[k+1, 2] - 0.5)^2 - 0.25])\n", 568 | " end\n", 569 | " # append!(ineq, [p[k+1, 3]^2 - (p[k+1, 2]^2 + p[k+1, 3]^2) / 6])\n", 570 | " # append!(ineq, [(p[k+1, 1] - 0.0)^2 + (p[k+1, 2] - 0.5)^2 - 0.25])\n", 571 | "\n", 572 | " v_diff = m * v[k+1,:] - (m * Fk' * v[k, :] + ([0; 0; 1.0]* f[k]+ m * R[k+1,:,:]' * g) * dt )\n", 573 | "\n", 574 | "# append!(eq, [v_diff[1]])\n", 575 | "# append!(eq, [v_diff[2]])\n", 576 | "# append!(eq, [v_diff[3]])\n", 577 | " # print(\"hahahah\\n\")\n", 578 | "\n", 579 | " R_diff = R[k, :, :] - Matrix(1.0I, 3, 3)\n", 580 | " R_diff = R_diff' * R_diff\n", 581 | "\n", 582 | "\n", 583 | " F_cost = F[k, : , :] - Matrix(1.0I, 3, 3)\n", 584 | " F_cost = F_cost' * F_cost\n", 585 | "\n", 586 | " R_cost = R_diff[1,1] + R_diff[2,2] + R_diff[3,3]\n", 587 | " w_cost = F_cost[1, 1] + F_cost[2, 2] + F_cost[3, 3]\n", 588 | " p_cost = sum(p[k, :].^2)\n", 589 | " v_cost = sum(v[k, :].^2)\n", 590 | "\n", 591 | " f_act = f_act + Rc * (tau[k, 1]^2 + tau[k, 2]^2 + tau[k, 3]^2) + Rc * f[k, 1]^2\n", 592 | " f_act = f_act + Qc[1] * R_cost\n", 593 | " f_act = f_act + Qc[2] * w_cost\n", 594 | " f_act = f_act + Qc[3] * p_cost\n", 595 | " f_act = f_act + Qc[4] * v_cost\n", 596 | " end\n", 597 | "\n", 598 | " # orientation error\n", 599 | " p_goal = [0, 0,0.0]\n", 600 | "\n", 601 | " R_diff = R[end, :, :] - Matrix(1.0I, 3, 3)\n", 602 | " R_diff = R_diff' * R_diff\n", 603 | " f_R = R_diff[1,1] + R_diff[2,2] + R_diff[3,3]\n", 604 | "\n", 605 | " # angular velocity error\n", 606 | " F_diff = F[end, :, :] - Matrix(1.0I, 3, 3)\n", 607 | " F_diff = F_diff' * F_diff\n", 608 | " f_w = F_diff[1,1] + F_diff[2,2] + F_diff[3,3]\n", 609 | "\n", 610 | " # position cost\n", 611 | " f_p = sum((p[end,:] - p_goal).^2)\n", 612 | " f_v = sum((v[end,:]).^2)\n", 613 | "\n", 614 | " cost = f_act * dt / 0.25\n", 615 | " cost = cost + Pc[1] * f_R + Pc[2] * f_w + Pc[3] * f_p + Pc[4] * f_v\n", 616 | "\n", 617 | "# cost = cost + 1e-2 * sum(var.^4)\n", 618 | " \n", 619 | " pop = append!([cost * 10], ineq, eq)\n", 620 | " eq_len = length(eq)\n", 621 | "\n", 622 | " order = 2\n", 623 | " \n", 624 | " time_start = time()\n", 625 | " if mode == 1\n", 626 | " opt,sol,data =cs_tssos_first(pop, var, order, numeq=eq_len, CS=\"NC\", TS = \"MD\", solution=true, QUIET=false, MomentOne=false)\n", 627 | " elseif mode == 2\n", 628 | " opt,sol,data =cs_tssos_first(pop, var, order, numeq=eq_len, CS=\"NC\", TS = \"block\", solution=true, QUIET=false, MomentOne=false)\n", 629 | " elseif mode == 3\n", 630 | " opt,sol,data =cs_tssos_first(pop, var, order, numeq=eq_len, CS=\"NC\", TS = false, solution=true, QUIET=false, MomentOne=false)\n", 631 | " end\n", 632 | " time_end = time()\n", 633 | " elapsed = time_end - time_start\n", 634 | "\n", 635 | " \n", 636 | " moment = []\n", 637 | " for k = 1:length(data.moment)\n", 638 | " append!(moment, [convert(Matrix{Float64}, data.moment[k])]) # data.Mmatrix[k])\n", 639 | " end\n", 640 | "\n", 641 | "\n", 642 | " log = Dict(\"sol\" => sol,\"dof\" => dof, # \"sol_approx\" => sol_approx, \n", 643 | " \"moment\" => moment, \"opt\" => opt, \"dt\"=>dt, \"Mass\"=>m, \"Inertial\"=>Jb, \n", 644 | " \"quat_init\"=> quat_init, \"Qc\"=> Qc, \"Rc\"=>Rc, \"Pc\"=>Pc, \"obs_flag\"=>obs_flag,\n", 645 | " \"solver_flag\"=>data.flag, \"elapsed\"=>elapsed)\n", 646 | " return log\n", 647 | "end" 648 | ] 649 | }, 650 | { 651 | "cell_type": "code", 652 | "execution_count": null, 653 | "id": "b994e108", 654 | "metadata": {}, 655 | "outputs": [ 656 | { 657 | "name": "stdout", 658 | "output_type": "stream", 659 | "text": [ 660 | "(24,)*********************************** TSSOS ***********************************\n", 661 | "Version 1.0.0, developed by Jie Wang, 2020--2023\n", 662 | "TSSOS is launching...\n", 663 | "-----------------------------------------------------------------------------\n", 664 | "The clique sizes of varibles:\n", 665 | "[7, 6, 5, 9, 18, 1, 2]\n", 666 | "[27, 30, 18, 2, 2, 21, 3]\n", 667 | "-----------------------------------------------------------------------------\n", 668 | "Obtained the variable cliques in 0.1092755 seconds. The maximal size of cliques is 18.\n", 669 | "Assembling the SDP...\n", 670 | "There are 23074 affine constraints.\n", 671 | "SDP assembling time: 0.92847 seconds.\n", 672 | "Solving the SDP...\n", 673 | "Problem\n", 674 | " Name : \n", 675 | " Objective sense : max \n", 676 | " Type : CONIC (conic optimization problem)\n", 677 | " Constraints : 23074 \n", 678 | " Cones : 0 \n", 679 | " Scalar variables : 11491 \n", 680 | " Matrix variables : 127 \n", 681 | " Integer variables : 0 \n", 682 | "\n", 683 | "Optimizer started.\n", 684 | "Presolve started.\n", 685 | "Linear dependency checker started.\n", 686 | "Linear dependency checker terminated.\n", 687 | "Eliminator started.\n", 688 | "Freed constraints in eliminator : 0\n", 689 | "Eliminator terminated.\n", 690 | "Eliminator started.\n", 691 | "Freed constraints in eliminator : 0\n", 692 | "Eliminator terminated.\n", 693 | "Eliminator - tries : 2 time : 0.00 \n", 694 | "Lin. dep. - tries : 1 time : 0.00 \n", 695 | "Lin. dep. - number : 0 \n", 696 | "Presolve terminated. Time: 0.01 \n", 697 | "Problem\n", 698 | " Name : \n", 699 | " Objective sense : max \n", 700 | " Type : CONIC (conic optimization problem)\n", 701 | " Constraints : 23074 \n", 702 | " Cones : 0 \n", 703 | " Scalar variables : 11491 \n", 704 | " Matrix variables : 127 \n", 705 | " Integer variables : 0 \n", 706 | "\n", 707 | "Optimizer - threads : 8 \n", 708 | "Optimizer - solved problem : the primal \n", 709 | "Optimizer - Constraints : 23074\n", 710 | "Optimizer - Cones : 4\n", 711 | "Optimizer - Scalar variables : 9995 conic : 9995 \n", 712 | "Optimizer - Semi-definite variables: 124 scalarized : 74275 \n", 713 | "Factor - setup time : 2.67 dense det. time : 0.00 \n", 714 | "Factor - ML order time : 0.67 GP order time : 0.00 \n", 715 | "Factor - nonzeros before factor : 5.56e+07 after factor : 9.76e+07 \n", 716 | "Factor - dense dim. : 2 flops : 7.75e+11 \n", 717 | "ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME \n", 718 | "0 1.0e+03 1.0e+00 1.0e+00 0.00e+00 0.000000000e+00 0.000000000e+00 1.0e+00 2.72 \n", 719 | "1 9.5e+02 9.5e-01 9.7e-01 -9.95e-01 -6.196563055e-02 -1.173767335e-01 9.5e-01 7.98 \n", 720 | "2 8.4e+02 8.4e-01 9.2e-01 -9.95e-01 -8.221202097e+00 -8.412137333e+00 8.4e-01 13.97 \n", 721 | "3 7.5e+02 7.5e-01 8.6e-01 -9.94e-01 -1.378523168e+01 -1.411829829e+01 7.5e-01 19.41 \n", 722 | "4 6.2e+02 6.2e-01 7.9e-01 -9.93e-01 -3.097135263e+01 -3.157345836e+01 6.2e-01 24.92 \n", 723 | "5 3.2e+02 3.2e-01 5.6e-01 -9.89e-01 -2.162090307e+02 -2.183231682e+02 3.2e-01 30.34 \n", 724 | "6 1.8e+02 1.8e-01 4.2e-01 -9.71e-01 -1.007323351e+03 -1.011630452e+03 1.8e-01 35.83 \n", 725 | "7 4.4e+01 4.4e-02 1.9e-01 -9.46e-01 -4.817110940e+03 -4.834888000e+03 4.4e-02 41.20 \n", 726 | "8 2.8e+01 2.8e-02 1.1e-01 -5.22e-01 -4.829870947e+03 -4.845864849e+03 2.8e-02 46.61 \n", 727 | "9 2.0e+01 2.0e-02 5.8e-02 3.20e-01 -2.916835953e+03 -2.925001358e+03 2.0e-02 51.94 \n", 728 | "10 1.0e+01 1.0e-02 2.0e-02 7.64e-01 -1.241861674e+03 -1.245368161e+03 1.0e-02 57.28 \n", 729 | "11 3.4e+00 3.4e-03 3.7e-03 9.63e-01 -2.313766414e+02 -2.325241772e+02 3.4e-03 62.75 \n", 730 | "12 1.4e+00 1.4e-03 1.1e-03 9.83e-01 -1.316518498e+02 -1.321744697e+02 1.4e-03 68.08 \n", 731 | "13 1.6e-01 1.6e-04 3.2e-05 1.06e+00 1.187989038e-01 7.872149215e-02 1.6e-04 74.20 \n", 732 | "14 4.8e-02 4.8e-05 6.2e-06 1.04e+00 5.523257178e+00 5.507987274e+00 4.8e-05 79.69 \n", 733 | "15 8.1e-03 8.1e-06 4.5e-07 1.01e+00 7.886509826e+00 7.883594924e+00 8.1e-06 85.91 \n", 734 | "16 1.9e-03 1.9e-06 5.2e-08 1.00e+00 8.173161061e+00 8.172451134e+00 1.9e-06 91.70 \n", 735 | "17 2.9e-04 2.9e-07 3.1e-09 9.99e-01 8.239495888e+00 8.239387601e+00 2.9e-07 97.88 \n", 736 | "18 2.0e-04 2.0e-07 1.7e-09 1.01e+00 8.242873859e+00 8.242800329e+00 2.0e-07 103.75\n", 737 | "19 3.2e-05 3.2e-08 1.2e-10 9.85e-01 8.248798424e+00 8.248786059e+00 3.2e-08 109.95\n", 738 | "20 2.2e-05 2.2e-08 6.4e-11 1.03e+00 8.249240186e+00 8.249232012e+00 2.2e-08 115.88\n", 739 | "21 3.5e-06 3.6e-09 4.3e-12 1.01e+00 8.249878291e+00 8.249876925e+00 3.6e-09 122.23\n", 740 | "22 1.6e-06 1.7e-09 1.3e-12 1.00e+00 8.249941687e+00 8.249941060e+00 1.6e-09 128.16\n", 741 | "23 2.4e-07 1.6e-09 7.2e-14 1.00e+00 8.249988635e+00 8.249988548e+00 2.4e-10 134.31\n", 742 | "Optimizer terminated. Time: 134.33 \n", 743 | "\n", 744 | "SDP solving time: 138.5455207 seconds.\n", 745 | "optimum = 8.249988634549451\n", 746 | "Global optimality certified with relative optimality gap 0.000139%!\n", 747 | "(24,)*********************************** TSSOS ***********************************\n", 748 | "Version 1.0.0, developed by Jie Wang, 2020--2023\n", 749 | "TSSOS is launching...\n", 750 | "-----------------------------------------------------------------------------\n", 751 | "The clique sizes of varibles:\n", 752 | "[7, 6, 5, 9, 18, 1, 2]\n", 753 | "[27, 30, 18, 2, 2, 21, 3]\n", 754 | "-----------------------------------------------------------------------------\n", 755 | "Obtained the variable cliques in 0.0013674 seconds. The maximal size of cliques is 18.\n", 756 | "Assembling the SDP...\n", 757 | "There are 23074 affine constraints.\n", 758 | "SDP assembling time: 0.090428 seconds.\n", 759 | "Solving the SDP...\n", 760 | "Problem\n", 761 | " Name : \n", 762 | " Objective sense : max \n", 763 | " Type : CONIC (conic optimization problem)\n", 764 | " Constraints : 23074 \n", 765 | " Cones : 0 \n", 766 | " Scalar variables : 11491 \n", 767 | " Matrix variables : 127 \n", 768 | " Integer variables : 0 \n", 769 | "\n", 770 | "Optimizer started.\n", 771 | "Presolve started.\n", 772 | "Linear dependency checker started.\n", 773 | "Linear dependency checker terminated.\n", 774 | "Eliminator started.\n", 775 | "Freed constraints in eliminator : 0\n", 776 | "Eliminator terminated.\n", 777 | "Eliminator started.\n", 778 | "Freed constraints in eliminator : 0\n", 779 | "Eliminator terminated.\n", 780 | "Eliminator - tries : 2 time : 0.00 \n", 781 | "Lin. dep. - tries : 1 time : 0.00 \n", 782 | "Lin. dep. - number : 0 \n", 783 | "Presolve terminated. Time: 0.01 \n", 784 | "Problem\n", 785 | " Name : \n", 786 | " Objective sense : max \n", 787 | " Type : CONIC (conic optimization problem)\n", 788 | " Constraints : 23074 \n", 789 | " Cones : 0 \n", 790 | " Scalar variables : 11491 \n", 791 | " Matrix variables : 127 \n", 792 | " Integer variables : 0 \n", 793 | "\n", 794 | "Optimizer - threads : 8 \n", 795 | "Optimizer - solved problem : the primal \n", 796 | "Optimizer - Constraints : 23074\n", 797 | "Optimizer - Cones : 4\n", 798 | "Optimizer - Scalar variables : 9785 conic : 9785 \n", 799 | "Optimizer - Semi-definite variables: 124 scalarized : 74275 \n", 800 | "Factor - setup time : 2.73 dense det. time : 0.00 \n", 801 | "Factor - ML order time : 0.70 GP order time : 0.00 \n", 802 | "Factor - nonzeros before factor : 5.56e+07 after factor : 9.76e+07 \n", 803 | "Factor - dense dim. : 2 flops : 7.75e+11 \n", 804 | "ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME \n", 805 | "0 1.0e+03 1.0e+00 1.0e+00 0.00e+00 0.000000000e+00 0.000000000e+00 1.0e+00 2.77 \n", 806 | "1 9.5e+02 9.5e-01 9.7e-01 -9.95e-01 -6.189709991e-02 -1.171790959e-01 9.5e-01 8.06 \n", 807 | "2 8.4e+02 8.4e-01 9.2e-01 -9.95e-01 -8.204686717e+00 -8.394648144e+00 8.4e-01 13.20 \n" 808 | ] 809 | } 810 | ], 811 | "source": [ 812 | "quat_list = [[1;0;0;0], [0.86;0;0.5;0], [1;0;1;0], [0.5, 0, 0.85, 0], [0,0,1,0]] # 60deg 0deg 90deg 120deg 180deg\n", 813 | "obs_list = [false; true]\n", 814 | "mode_list = [1; 2; 3]\n", 815 | "\n", 816 | "dof = 3\n", 817 | "dt = 0.25 * 3 / 4\n", 818 | "\n", 819 | "for j = 3 # sparse mode\n", 820 | " for k = 1:5 # 1:3 # initial condition\n", 821 | " for h = 2 # obs or not\n", 822 | " log = test_drone(dof, dt, obs_list[h], quat_list[k], mode_list[j], 2) # new obs at the center\n", 823 | " file_name = \"log_s30_obs_\" * string(j) * \"_\" * string(k) * \"_\" * string(h) * \".mat\"\n", 824 | " matwrite(file_name, log)\n", 825 | " end \n", 826 | " end\n", 827 | "end" 828 | ] 829 | } 830 | ], 831 | "metadata": { 832 | "kernelspec": { 833 | "display_name": "Julia 1.8.3", 834 | "language": "julia", 835 | "name": "julia-1.8" 836 | }, 837 | "language_info": { 838 | "file_extension": ".jl", 839 | "mimetype": "application/julia", 840 | "name": "julia", 841 | "version": "1.8.3" 842 | } 843 | }, 844 | "nbformat": 4, 845 | "nbformat_minor": 5 846 | } 847 | -------------------------------------------------------------------------------- /InverseKinematics/IK-random.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 31, 6 | "id": "21e7bfcc", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "using DynamicPolynomials\n", 11 | "using TSSOS ## TODO: use the official TSSOS\n", 12 | "using MAT\n", 13 | "using LinearAlgebra\n", 14 | "using Printf\n", 15 | "using JuMP" 16 | ] 17 | }, 18 | { 19 | "cell_type": "code", 20 | "execution_count": 32, 21 | "id": "bf7bfc61", 22 | "metadata": {}, 23 | "outputs": [ 24 | { 25 | "data": { 26 | "text/plain": [ 27 | "SE3_inverse (generic function with 1 method)" 28 | ] 29 | }, 30 | "execution_count": 32, 31 | "metadata": {}, 32 | "output_type": "execute_result" 33 | } 34 | ], 35 | "source": [ 36 | "@polyvar temp1[1:4, 1:4]\n", 37 | "@polyvar temp2[1:4, 1:4]\n", 38 | "temp_p = temp2 * 1.1 + temp1 * 1.1\n", 39 | "\n", 40 | "function Joint2SE3(c, s, x, y, z)\n", 41 | " ## we always rotate about the z axis\n", 42 | " T = temp_p * 0\n", 43 | " T[1,1] = c\n", 44 | " T[2,2] = c\n", 45 | " T[1,2] = -s\n", 46 | " T[2,1] = s\n", 47 | " T[3,3] = 1\n", 48 | " \n", 49 | " T[1,4] = c * x - s * y\n", 50 | " T[2,4] = s * x + c * y\n", 51 | " T[3,4] = z\n", 52 | " T[4,4] = 1\n", 53 | " return T\n", 54 | "end\n", 55 | "\n", 56 | "function SE3_Euler_num(y, p, r, lx, ly, lz)\n", 57 | " T = temp_p * 0\n", 58 | " T[1:3, 1:3] = rotz_num(y) * roty_num(p) * rotx_num(r)\n", 59 | " T[4,4] = 1.0\n", 60 | " T[1,4] = lx\n", 61 | " T[2,4] = ly\n", 62 | " T[3,4] = lz\n", 63 | " return T\n", 64 | "end\n", 65 | "\n", 66 | "function rotx_num(t)\n", 67 | " R = temp_p[1:3, 1:3] * 0\n", 68 | " R[2,2] = cos(t)\n", 69 | " R[3,3] = cos(t)\n", 70 | " R[2,3] = -sin(t)\n", 71 | " R[3,2] = sin(t)\n", 72 | " R[1,1] = 1.0\n", 73 | " return R\n", 74 | "end\n", 75 | "\n", 76 | "function roty_num(t)\n", 77 | " R = temp_p[1:3, 1:3] * 0\n", 78 | " R[1,1] = cos(t)\n", 79 | " R[3,3] = cos(t)\n", 80 | " R[1,3] = sin(t)\n", 81 | " R[3,1] = -sin(t)\n", 82 | " R[2,2] = 1.0\n", 83 | " return R\n", 84 | "end\n", 85 | "\n", 86 | "function rotz_num(t)\n", 87 | " R = temp_p[1:3, 1:3] * 0\n", 88 | " R[1,1] = cos(t)\n", 89 | " R[2,2] = cos(t)\n", 90 | " R[1,2] = -sin(t)\n", 91 | " R[2,1] = sin(t)\n", 92 | " R[3,3] = 1.0\n", 93 | " return R\n", 94 | "end\n", 95 | "\n", 96 | "function quat2rot_num(w, x, y, z)\n", 97 | " R = temp_p[1:3, 1:3] * 0\n", 98 | " nq = sqrt(w^2 + x^2 + y^2 + z^2)\n", 99 | " w = w / nq\n", 100 | " x = x / nq\n", 101 | " y = y / nq\n", 102 | " z = z / nq\n", 103 | " \n", 104 | " R[1,1] = 1 - 2 * (y^2 + z^2)\n", 105 | " R[2,2] = 1 - 2 * (x^2 + z^2)\n", 106 | " R[3,3] = 1 - 2 * (x^2 + y^2)\n", 107 | " \n", 108 | " R[1,2] = 2 * (x * y - w * z)\n", 109 | " R[1,3] = 2 * (w * y + x * z)\n", 110 | " R[2,3] = 2 * (y * z - w * x)\n", 111 | " \n", 112 | " R[2,1] = 2 * (x * y + w * z)\n", 113 | " R[3,1] = 2 * (- w * y + x * z)\n", 114 | " R[3,2] = 2 * (y * z + w * x)\n", 115 | " return R\n", 116 | "end\n", 117 | "\n", 118 | "function cross_(x, y) \n", 119 | " z = temp_p[1:3, 1]\n", 120 | " z[1] = -x[3] * y[2] + x[2] * y[3]\n", 121 | " z[2] = x[3] * y[1] - x[1] * y[3]\n", 122 | " z[3] = -x[2] * y[1] + x[1] * y[2]\n", 123 | " return z\n", 124 | "end\n", 125 | "\n", 126 | "function add_SO3_cons(R, eq)\n", 127 | " T = R' * R\n", 128 | "# print(T[1,1] - 1.0)\n", 129 | " append!(eq, [T[1,1] - 1.0])\n", 130 | " append!(eq, [T[2,2] - 1.0])\n", 131 | " append!(eq, [T[3,3] - 1.0])\n", 132 | " append!(eq, [T[1,2]])\n", 133 | " append!(eq, [T[1,3]])\n", 134 | " append!(eq, [T[2,3]])\n", 135 | " \n", 136 | " append!(eq, cross_(R[1,:], R[2, :]) - R[3,:])\n", 137 | " append!(eq, cross_(R[2,:], R[3, :]) - R[1,:])\n", 138 | " append!(eq, cross_(R[3,:], R[1, :]) - R[2,:])\n", 139 | " \n", 140 | " return eq\n", 141 | "end\n", 142 | "\n", 143 | "function add_SE3_tran_cons(T1, T2, eq)\n", 144 | " T = T1 - T2\n", 145 | " \n", 146 | " append!(eq, [T[1, 1]])\n", 147 | " append!(eq, [T[1, 2]])\n", 148 | " append!(eq, [T[1, 3]])\n", 149 | " \n", 150 | " \n", 151 | " append!(eq, [T[2, 1]])\n", 152 | " append!(eq, [T[2, 2]])\n", 153 | " append!(eq, [T[2, 3]])\n", 154 | " \n", 155 | " \n", 156 | " append!(eq, [T[3, 1]])\n", 157 | " append!(eq, [T[3, 2]])\n", 158 | " append!(eq, [T[3, 3]])\n", 159 | " \n", 160 | " append!(eq, [T[1, 4]])\n", 161 | " append!(eq, [T[2, 4]])\n", 162 | " append!(eq, [T[3, 4]]) \n", 163 | " return eq\n", 164 | "end\n", 165 | "\n", 166 | "function add_SO3_tran_cons(T1, T2, eq)\n", 167 | " T = T1 - T2\n", 168 | " \n", 169 | " append!(eq, [T[1, 1]])\n", 170 | " append!(eq, [T[1, 2]])\n", 171 | " append!(eq, [T[1, 3]])\n", 172 | " \n", 173 | " \n", 174 | " append!(eq, [T[2, 1]])\n", 175 | " append!(eq, [T[2, 2]])\n", 176 | " append!(eq, [T[2, 3]])\n", 177 | " \n", 178 | " \n", 179 | " append!(eq, [T[3, 1]])\n", 180 | " append!(eq, [T[3, 2]])\n", 181 | " append!(eq, [T[3, 3]])\n", 182 | " \n", 183 | " return eq\n", 184 | "end\n", 185 | "\n", 186 | "\n", 187 | "function make_SE3(R, x, y, z)\n", 188 | " T = temp_p * 0\n", 189 | " T[1:3, 1:3] = R\n", 190 | " T[1,4] = x\n", 191 | " T[2,4] = y\n", 192 | " T[3,4] = z\n", 193 | " T[4,4] = 1.0\n", 194 | " return T\n", 195 | "end\n", 196 | "\n", 197 | "function SE3_inverse(T)\n", 198 | " Tnew = T\n", 199 | " Tnew[1:3, 1:3] = T[1:3, 1:3]'\n", 200 | " Tnew[1:3, 4] = - T[1:3, 1:3]' * T[1:3, 4]\n", 201 | " return Tnew\n", 202 | "end" 203 | ] 204 | }, 205 | { 206 | "cell_type": "code", 207 | "execution_count": 33, 208 | "id": "df65045e", 209 | "metadata": {}, 210 | "outputs": [ 211 | { 212 | "data": { 213 | "text/plain": [ 214 | "batch_test (generic function with 1 method)" 215 | ] 216 | }, 217 | "execution_count": 33, 218 | "metadata": {}, 219 | "output_type": "execute_result" 220 | } 221 | ], 222 | "source": [ 223 | "function batch_test(dof)\n", 224 | " ## generate random arm \n", 225 | " x_bar = rand(dof, 1) #* 0.5\n", 226 | " y_bar = rand(dof, 1) #* 0\n", 227 | " z_bar = rand(dof, 1) #* 0\n", 228 | " \n", 229 | " ## generate random rotation offset\n", 230 | " T_list = zeros(dof, 4, 4)\n", 231 | " T_list[1, :, :] = Matrix(1.0I, 4, 4)\n", 232 | " T_list[:, 4, 4] .= 1.0\n", 233 | "\n", 234 | " for k = 1:dof\n", 235 | " T_list[k, 1:3, 1:3] = rotx_num(2 * rand(Float64) * pi) * roty_num(2 * rand(Float64) * pi) * rotz_num(2 * rand(Float64) * pi)\n", 236 | " end\n", 237 | " \n", 238 | " ## generate random feasible end pose by forward kinematics\n", 239 | " theta_list = randn(dof, 1)\n", 240 | " T_end = Matrix(1.0I, 4, 4)\n", 241 | " for k = 1:dof\n", 242 | " Act = Joint2SE3(cos(theta_list[k]), sin(theta_list[k]), x_bar[k], y_bar[k], z_bar[k])\n", 243 | " T_end = T_end * Act * T_list[k, :, :]\n", 244 | " end\n", 245 | "\n", 246 | " @polyvar R[1:dof+1, 1:3, 1:3] \n", 247 | " @polyvar c[1:dof]\n", 248 | " @polyvar s[1:dof]\n", 249 | " @polyvar x[1:dof+1]\n", 250 | " @polyvar y[1:dof+1]\n", 251 | " @polyvar z[1:dof+1]\n", 252 | " var = append!(reshape(R, (1+dof) * 9), c, s, x, y, z)\n", 253 | " # print(var)\n", 254 | " \n", 255 | " ## base link\n", 256 | " eq = [temp_p[1,1] * 0]\n", 257 | "\n", 258 | " eq[1] = R[1, 1, 1] - 1.0\n", 259 | " append!(eq, [R[1, 1, 2] - 0.0])\n", 260 | " append!(eq, [R[1, 1, 3] - 0.0])\n", 261 | " append!(eq, [R[1, 2, 1] - 0.0])\n", 262 | " append!(eq, [R[1, 2, 2] - 1.0])\n", 263 | " append!(eq, [R[1, 2, 3] - 0.0])\n", 264 | " append!(eq, [R[1, 3, 1] - 0.0])\n", 265 | " append!(eq, [R[1, 3, 2] - 0.0])\n", 266 | " append!(eq, [R[1, 3, 3] - 1.0])\n", 267 | "\n", 268 | " append!(eq, [x[1] - 0.0])\n", 269 | " append!(eq, [y[1] - 0.0])\n", 270 | " append!(eq, [z[1] - 0.0])\n", 271 | "\n", 272 | " f_act = 0\n", 273 | "\n", 274 | " for k = 1:dof\n", 275 | "\n", 276 | " append!(eq, [c[k]^2 + s[k]^2 - 1]) # joint SO2 constrains\n", 277 | "\n", 278 | " Tk = make_SE3(R[k,:,:], x[k], y[k], z[k])\n", 279 | " Tkp1 = make_SE3(R[k+1,:,:], x[k+1], y[k+1], z[k+1])\n", 280 | " Act = Joint2SE3(c[k], s[k], x_bar[k], y_bar[k], z_bar[k])\n", 281 | "\n", 282 | " # eq = add_SE3_tran_cons(SE3_inverse(Tk) * Tkp1, T_list[k, :, :] * Act, eq)\n", 283 | " eq = add_SE3_tran_cons(Tkp1, Tk * Act * T_list[k, :, :], eq)\n", 284 | " eq = add_SO3_cons(R[k+1,:,:], eq) # SO3 constraints\n", 285 | " \n", 286 | " # ref angle: 0 for all links\n", 287 | " f_act = f_act + (1 - c[k])^2 + s[k]^2\n", 288 | " end\n", 289 | " \n", 290 | " ## end constraints \n", 291 | " for i = 1:3\n", 292 | " for j = 1:3\n", 293 | " append!(eq, [ T_end[i,j] - R[end, i, j] ])\n", 294 | " end\n", 295 | " end\n", 296 | "\n", 297 | " append!(eq, [ x[end] - T_end[1, 4] ])\n", 298 | " append!(eq, [ y[end] - T_end[2, 4] ])\n", 299 | " append!(eq, [ z[end] - T_end[3, 4] ])\n", 300 | "\n", 301 | "\n", 302 | " ineq = []\n", 303 | "\n", 304 | " f = 1.0 * f_act # + f_R + f_p # * 1e-5 # 1e-3 * sum(var.^2)\n", 305 | " pop = append!([f], ineq, eq)\n", 306 | " \n", 307 | "\n", 308 | " eq_len = length(eq)\n", 309 | " order = 2\n", 310 | " \n", 311 | " time_start = time()\n", 312 | " opt,sol,data = cs_tssos_first(pop, var, order, numeq=eq_len, CS=\"NC\", TS=false, MomentOne=false, solution=true, QUIET=false);\n", 313 | " time_end = time()\n", 314 | " \n", 315 | " ## the sparse moment matrix\n", 316 | " moment = []\n", 317 | " for k = 1:length(data.moment)\n", 318 | " append!(moment, [convert(Matrix{Float64}, data.moment[k])]) # data.Mmatrix[k])\n", 319 | " end\n", 320 | "\n", 321 | " elapsed = time_end - time_start\n", 322 | "\n", 323 | " log = Dict(\"sol\" => sol, \"dof\" => dof, \"moment\" => moment, \"elapsed\" => elapsed, \"Tg\" => T_end,\n", 324 | " \"T\" => T_list, \"Th\" => theta_list, \"xb\"=>x_bar, \"yb\"=>x_bar, \"zb\"=>x_bar, \"flag\"=>data.flag)\n", 325 | " return log\n", 326 | "end" 327 | ] 328 | }, 329 | { 330 | "cell_type": "code", 331 | "execution_count": 34, 332 | "id": "31dbbfdc", 333 | "metadata": { 334 | "scrolled": true 335 | }, 336 | "outputs": [ 337 | { 338 | "name": "stdout", 339 | "output_type": "stream", 340 | "text": [ 341 | "*********************************** TSSOS ***********************************\n", 342 | "Version 1.0.0, developed by Jie Wang, 2020--2023\n", 343 | "TSSOS is launching...\n", 344 | "-----------------------------------------------------------------------------\n", 345 | "The clique sizes of varibles:\n", 346 | "[11, 9, 7, 8]\n", 347 | "[11, 15, 18, 3]\n", 348 | "-----------------------------------------------------------------------------\n", 349 | "Obtained the variable cliques in 0.0017297 seconds. The maximal size of cliques is 11.\n", 350 | "Assembling the SDP...\n", 351 | "There are 21529 affine constraints.\n", 352 | "SDP assembling time: 0.1020982 seconds.\n", 353 | "Solving the SDP...\n", 354 | "Problem\n", 355 | " Name : \n", 356 | " Objective sense : max \n", 357 | " Type : CONIC (conic optimization problem)\n", 358 | " Constraints : 21529 \n", 359 | " Cones : 0 \n", 360 | " Scalar variables : 21007 \n", 361 | " Matrix variables : 47 \n", 362 | " Integer variables : 0 \n", 363 | "\n", 364 | "Optimizer started.\n", 365 | "Presolve started.\n", 366 | "Linear dependency checker started.\n", 367 | "Linear dependency checker terminated.\n", 368 | "Eliminator started.\n", 369 | "Freed constraints in eliminator : 0\n", 370 | "Eliminator terminated.\n", 371 | "Eliminator started.\n", 372 | "Freed constraints in eliminator : 0\n", 373 | "Eliminator terminated.\n", 374 | "Eliminator - tries : 2 time : 0.00 \n", 375 | "Lin. dep. - tries : 1 time : 0.00 \n", 376 | "Lin. dep. - number : 0 \n", 377 | "Presolve terminated. Time: 0.02 \n", 378 | "Problem\n", 379 | " Name : \n", 380 | " Objective sense : max \n", 381 | " Type : CONIC (conic optimization problem)\n", 382 | " Constraints : 21529 \n", 383 | " Cones : 0 \n", 384 | " Scalar variables : 21007 \n", 385 | " Matrix variables : 47 \n", 386 | " Integer variables : 0 \n", 387 | "\n", 388 | "Optimizer - threads : 8 \n", 389 | "Optimizer - solved problem : the primal \n", 390 | "Optimizer - Constraints : 21529\n", 391 | "Optimizer - Cones : 1\n", 392 | "Optimizer - Scalar variables : 15494 conic : 15494 \n", 393 | "Optimizer - Semi-definite variables: 47 scalarized : 72084 \n", 394 | "Factor - setup time : 0.66 dense det. time : 0.00 \n", 395 | "Factor - ML order time : 0.19 GP order time : 0.00 \n", 396 | "Factor - nonzeros before factor : 1.35e+07 after factor : 2.30e+07 \n", 397 | "Factor - dense dim. : 2 flops : 4.29e+10 \n", 398 | "ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME \n", 399 | "0 2.1e+01 1.0e+00 1.0e+00 0.00e+00 0.000000000e+00 0.000000000e+00 1.0e+00 0.69 \n", 400 | "1 2.0e+01 9.5e-01 8.1e-01 5.80e+00 4.051605745e-01 3.468405956e-01 9.5e-01 1.56 \n", 401 | "2 1.7e+01 8.2e-01 1.2e+00 -8.17e-02 3.850212653e+00 3.053618841e+00 8.2e-01 2.48 \n", 402 | "3 1.2e+01 5.8e-01 1.9e-01 1.67e+00 3.557168107e+00 3.929506563e+00 5.8e-01 3.45 \n", 403 | "4 1.8e+00 8.7e-02 1.2e-02 3.84e-01 9.840623865e+00 9.895274795e+00 8.7e-02 4.58 \n", 404 | "5 3.8e-01 1.8e-02 1.6e-03 1.39e+00 8.427419099e+00 8.432774856e+00 1.8e-02 5.70 \n", 405 | "6 1.9e-01 9.4e-03 6.3e-04 9.57e-01 7.501375200e+00 7.503738883e+00 9.4e-03 6.61 \n", 406 | "7 3.7e-02 1.8e-03 5.7e-05 9.11e-01 6.546172036e+00 6.546627145e+00 1.8e-03 7.69 \n", 407 | "8 8.4e-03 4.1e-04 6.7e-06 8.16e-01 6.238692507e+00 6.238780992e+00 4.1e-04 8.73 \n", 408 | "9 2.3e-03 1.1e-04 1.1e-06 9.05e-01 6.163447010e+00 6.163459776e+00 1.1e-04 9.66 \n", 409 | "10 9.8e-04 4.8e-05 3.3e-07 8.45e-01 6.153105188e+00 6.153105754e+00 4.8e-05 10.61 \n", 410 | "11 3.9e-04 1.9e-05 9.6e-08 8.13e-01 6.147405791e+00 6.147403251e+00 1.9e-05 11.55 \n", 411 | "12 1.5e-04 7.4e-06 2.6e-08 7.64e-01 6.145887328e+00 6.145884687e+00 7.4e-06 12.52 \n", 412 | "13 7.8e-05 3.8e-06 1.1e-08 6.99e-01 6.145939627e+00 6.145937350e+00 3.8e-06 13.48 \n", 413 | "14 5.5e-05 2.7e-06 7.0e-09 6.52e-01 6.146286081e+00 6.146283893e+00 2.7e-06 14.42 \n", 414 | "15 8.3e-06 4.0e-07 5.0e-10 6.75e-01 6.146671131e+00 6.146670475e+00 4.0e-07 15.53 \n", 415 | "16 1.4e-06 7.6e-08 5.4e-11 6.09e-01 6.146977795e+00 6.146977505e+00 7.5e-08 16.63 \n", 416 | "17 9.4e-07 2.0e-08 7.5e-12 6.64e-01 6.147090332e+00 6.147090253e+00 1.9e-08 17.63 \n", 417 | "18 5.1e-07 7.6e-09 1.6e-12 9.56e-01 6.147153538e+00 6.147153511e+00 6.9e-09 18.61 \n", 418 | "19 2.6e-07 7.7e-09 4.5e-13 9.17e-01 6.147186275e+00 6.147186261e+00 2.8e-09 19.73 \n", 419 | "20 2.1e-07 2.3e-08 1.8e-13 1.00e+00 6.147200444e+00 6.147200436e+00 1.6e-09 20.66 \n", 420 | "21 1.4e-07 1.2e-07 1.1e-13 1.00e+00 6.147207247e+00 6.147207241e+00 1.1e-09 21.75 \n", 421 | "22 6.5e-08 9.7e-08 3.4e-14 9.99e-01 6.147215236e+00 6.147215232e+00 5.3e-10 22.78 \n", 422 | "23 3.3e-08 6.4e-07 1.2e-14 1.00e+00 6.147219333e+00 6.147219331e+00 2.7e-10 23.83 \n", 423 | "24 1.5e-08 2.2e-06 3.4e-15 1.00e+00 6.147222110e+00 6.147222110e+00 1.2e-10 24.89 \n", 424 | "25 6.9e-09 6.6e-06 1.2e-15 1.00e+00 6.147223282e+00 6.147223282e+00 5.9e-11 26.02 \n", 425 | "Optimizer terminated. Time: 26.05 \n", 426 | "\n", 427 | "SDP solving time: 26.2930337 seconds.\n", 428 | "optimum = 6.147223282233778\n", 429 | "Global optimality certified with relative optimality gap 0.005775%!\n" 430 | ] 431 | } 432 | ], 433 | "source": [ 434 | "for dof = [6] # , 7, 10, 15, 20, 25, 30, 40]\n", 435 | " for id = 1:1 # 50\n", 436 | " log = batch_test(dof)\n", 437 | " file_name = \"log_\" * string(dof) * \"_\" * string(id) * \".mat\"\n", 438 | " matwrite(file_name, log)\n", 439 | " end\n", 440 | "end" 441 | ] 442 | } 443 | ], 444 | "metadata": { 445 | "kernelspec": { 446 | "display_name": "Julia 1.8.3", 447 | "language": "julia", 448 | "name": "julia-1.8" 449 | }, 450 | "language_info": { 451 | "file_extension": ".jl", 452 | "mimetype": "application/julia", 453 | "name": "julia", 454 | "version": "1.8.3" 455 | } 456 | }, 457 | "nbformat": 4, 458 | "nbformat_minor": 5 459 | } 460 | -------------------------------------------------------------------------------- /InverseKinematics/plot_statistic.m: -------------------------------------------------------------------------------- 1 | clear all;clc;close all; 2 | %% 3 | dof_list = [6, 7, 10, 15, 20, 25, 30, 40]; 4 | id = 50; 5 | %% 6 | ws_list = {}; 7 | elapsed_list = []; 8 | for d = 1:length(dof_list) 9 | for k = 1:id 10 | file_name = "log_" + num2str(dof_list(d)) + "_" + num2str(k); 11 | ws = load(file_name); 12 | ws_list{d, k} = ws; 13 | elapsed_list(d, k) = ws.elapsed; 14 | end 15 | end 16 | %% 17 | boxplot(elapsed_list', "Positions", dof_list, "Labels", dof_list, "Colors", "b", "Widths", 1, 'Whisker',1000) 18 | box on 19 | grid on 20 | hold on 21 | plot(dof_list, median(elapsed_list'), "k--") 22 | 23 | xlabel("DOF of Manipulator", "fontsize", 12) 24 | ylabel("Time (s)", "fontsize", 12) -------------------------------------------------------------------------------- /InverseKinematics/puma_test_data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/InverseKinematics/puma_test_data.mat -------------------------------------------------------------------------------- /Quaternion/drone-load-polytope.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "21e7bfcc", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "using DynamicPolynomials\n", 11 | "using TSSOS\n", 12 | "using MAT\n", 13 | "using LinearAlgebra\n", 14 | "using Printf\n", 15 | "\n", 16 | "using JuMP" 17 | ] 18 | }, 19 | { 20 | "cell_type": "code", 21 | "execution_count": 2, 22 | "id": "bf7bfc61", 23 | "metadata": {}, 24 | "outputs": [ 25 | { 26 | "data": { 27 | "text/plain": [ 28 | "RightMultiplication (generic function with 1 method)" 29 | ] 30 | }, 31 | "execution_count": 2, 32 | "metadata": {}, 33 | "output_type": "execute_result" 34 | } 35 | ], 36 | "source": [ 37 | "@polyvar temp1[1:4, 1:4]\n", 38 | "@polyvar temp2[1:4, 1:4]\n", 39 | "temp_p = temp2 * 1.1 + temp1 * 1.1\n", 40 | "\n", 41 | "function skew(q)\n", 42 | " qx = temp_p[1:3, 1:3] * 0\n", 43 | " qx[1, 2] = -q[3]\n", 44 | " qx[2, 1] = q[3]\n", 45 | "\n", 46 | " qx[1, 3] = q[2]\n", 47 | " qx[3, 1] = -q[2]\n", 48 | "\n", 49 | " qx[2, 3] = -q[1]\n", 50 | " qx[3, 2] = q[1]\n", 51 | "\n", 52 | " return qx\n", 53 | "end\n", 54 | "\n", 55 | "function LeftMultiplication(q)\n", 56 | " Lq = temp_p * 0\n", 57 | " Lq[1, 1] = q[1]\n", 58 | " Lq[1, 2:4] = -q[2:4]\n", 59 | " Lq[2:4, 1] = q[2:4]\n", 60 | " Lq[2:4, 2:4] = q[1] * Matrix(1.0I, 3, 3) + skew(q[2:4])\n", 61 | " return Lq\n", 62 | "end\n", 63 | "\n", 64 | "function RightMultiplication(q)\n", 65 | " Rq = temp_p * 0\n", 66 | " Rq[1, 1] = q[1]\n", 67 | " Rq[1, 2:4] = -q[2:4]\n", 68 | " Rq[2:4, 1] = q[2:4]\n", 69 | " Rq[2:4, 2:4] = q[1] * Matrix(1.0I, 3, 3) - skew(q[2:4])\n", 70 | " return Rq\n", 71 | "end" 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": 3, 77 | "id": "b7f15ed0", 78 | "metadata": {}, 79 | "outputs": [ 80 | { 81 | "data": { 82 | "text/plain": [ 83 | "1" 84 | ] 85 | }, 86 | "execution_count": 3, 87 | "metadata": {}, 88 | "output_type": "execute_result" 89 | } 90 | ], 91 | "source": [ 92 | "V = temp_p[1:3, :] * 0\n", 93 | "V[1:3, 2:4] = Matrix(1.0I, 3, 3)\n", 94 | "T = temp_p * 0\n", 95 | "T[2:4, 2:4] = -Matrix(1.0I, 3, 3)\n", 96 | "T\n", 97 | "\n", 98 | "Jb = Matrix(1.0I, 3, 3)\n", 99 | "Jb[1,1] = 1.0 / 10\n", 100 | "Jb[2,2] = 2.0 / 10\n", 101 | "Jb[3,3] = 1.0 / 10\n", 102 | "m = 0.5\n", 103 | "\n", 104 | "G = temp_p[1:3, 1] * 0\n", 105 | "G[3] = 1" 106 | ] 107 | }, 108 | { 109 | "cell_type": "code", 110 | "execution_count": 4, 111 | "id": "84e30526", 112 | "metadata": {}, 113 | "outputs": [ 114 | { 115 | "data": { 116 | "text/plain": [ 117 | "13×2 Matrix{Float64}:\n", 118 | " 1.0 2.0\n", 119 | " 2.0 3.0\n", 120 | " 3.0 4.0\n", 121 | " 3.0 7.0\n", 122 | " 3.0 6.0\n", 123 | " 4.0 5.0\n", 124 | " 5.0 7.0\n", 125 | " 6.0 8.0\n", 126 | " 8.0 9.0\n", 127 | " 9.0 10.0\n", 128 | " 7.0 10.0\n", 129 | " 10.0 11.0\n", 130 | " 11.0 12.0" 131 | ] 132 | }, 133 | "execution_count": 4, 134 | "metadata": {}, 135 | "output_type": "execute_result" 136 | } 137 | ], 138 | "source": [ 139 | "fileIn = matopen(\"freespace.mat\")\n", 140 | "dset = read(fileIn)\n", 141 | "close(fileIn)\n", 142 | "A_list = dset[\"A_list\"]\n", 143 | "b_list = dset[\"b_list\"]\n", 144 | "e_list = dset[\"e_in\"]\n", 145 | "# 1 2 3 4 5 7 10 11 12" 146 | ] 147 | }, 148 | { 149 | "cell_type": "code", 150 | "execution_count": null, 151 | "id": "9cfedf1f", 152 | "metadata": {}, 153 | "outputs": [ 154 | { 155 | "name": "stdout", 156 | "output_type": "stream", 157 | "text": [ 158 | "connect\n", 159 | "connect\n", 160 | "connect\n", 161 | "connect\n", 162 | "connect\n", 163 | "connect\n", 164 | "connect\n", 165 | "connect\n", 166 | "*********************************** TSSOS ***********************************\n", 167 | "Version 1.0.0, developed by Jie Wang, 2020--2022\n", 168 | "TSSOS is launching...\n", 169 | "-----------------------------------------------------------------------------\n", 170 | "The clique sizes of varibles:\n", 171 | "[7, 6, 5, 4, 3, 2, 1]\n", 172 | "[126, 2, 380, 8, 140, 700, 298]\n", 173 | "-----------------------------------------------------------------------------\n", 174 | "Obtained the variable cliques in 0.4367673 seconds. The maximal size of cliques is 7.\n", 175 | "Assembling the SDP...\n", 176 | "There are 8914 affine constraints.\n", 177 | "SDP assembling time: 0.8599716 seconds.\n", 178 | "Solving the SDP...\n", 179 | "Problem\n", 180 | " Name : \n", 181 | " Objective sense : max \n", 182 | " Type : CONIC (conic optimization problem)\n", 183 | " Constraints : 8914 \n", 184 | " Cones : 0 \n", 185 | " Scalar variables : 3133 \n", 186 | " Matrix variables : 1654 \n", 187 | " Integer variables : 0 \n", 188 | "\n", 189 | "Optimizer started.\n", 190 | "Presolve started.\n", 191 | "Linear dependency checker started.\n", 192 | "Linear dependency checker terminated.\n", 193 | "Eliminator started.\n", 194 | "Freed constraints in eliminator : 0\n", 195 | "Eliminator terminated.\n", 196 | "Eliminator started.\n", 197 | "Freed constraints in eliminator : 0\n", 198 | "Eliminator terminated.\n", 199 | "Eliminator - tries : 2 time : 0.00 \n", 200 | "Lin. dep. - tries : 1 time : 0.00 \n", 201 | "Lin. dep. - number : 0 \n", 202 | "Presolve terminated. Time: 0.00 \n", 203 | "GP based matrix reordering started.\n", 204 | "GP based matrix reordering terminated.\n", 205 | "Problem\n", 206 | " Name : \n", 207 | " Objective sense : max \n", 208 | " Type : CONIC (conic optimization problem)\n", 209 | " Constraints : 8914 \n", 210 | " Cones : 0 \n", 211 | " Scalar variables : 3133 \n", 212 | " Matrix variables : 1654 \n", 213 | " Integer variables : 0 \n", 214 | "\n", 215 | "Optimizer - threads : 8 \n", 216 | "Optimizer - solved problem : the primal \n", 217 | "Optimizer - Constraints : 8914\n", 218 | "Optimizer - Cones : 299\n", 219 | "Optimizer - Scalar variables : 4028 conic : 2892 \n", 220 | "Optimizer - Semi-definite variables: 1356 scalarized : 18292 \n", 221 | "Factor - setup time : 0.09 dense det. time : 0.00 \n", 222 | "Factor - ML order time : 0.01 GP order time : 0.06 \n", 223 | "Factor - nonzeros before factor : 1.36e+05 after factor : 3.47e+05 \n", 224 | "Factor - dense dim. : 2 flops : 2.28e+07 \n", 225 | "Factor - GP saved nzs : 1.17e+05 GP saved flops : 2.04e+07 \n", 226 | "ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME \n", 227 | "0 1.0e+03 1.0e+00 1.0e+00 0.00e+00 0.000000000e+00 0.000000000e+00 1.0e+00 0.11 \n", 228 | "1 8.3e+02 8.3e-01 9.1e-01 -9.90e-01 -4.478088084e-03 -2.094132901e-01 8.3e-01 0.13 \n", 229 | "2 4.4e+02 4.4e-01 6.2e-01 -9.36e-01 9.222423600e+00 8.142811909e+00 4.4e-01 0.17 \n", 230 | "3 3.1e+02 3.1e-01 4.6e-01 -7.17e-01 3.568465634e+01 3.431559292e+01 3.1e-01 0.19 \n", 231 | "4 1.4e+02 1.4e-01 2.2e-01 -4.58e-01 2.164850232e+02 2.146571039e+02 1.4e-01 0.22 \n", 232 | "5 9.1e+01 9.1e-02 1.4e-01 -2.56e-02 1.496587972e+03 1.494673083e+03 9.1e-02 0.25 \n", 233 | "6 6.2e+01 6.2e-02 1.4e-01 -7.80e-01 9.896075154e+03 9.891299066e+03 6.2e-02 0.28 \n", 234 | "7 1.4e+01 1.4e-02 5.2e-02 -6.71e-01 4.672678768e+04 4.671386188e+04 1.4e-02 0.30 \n", 235 | "8 7.6e+00 7.6e-03 2.5e-02 -1.99e-01 8.810377488e+04 8.809353738e+04 7.6e-03 0.33 \n", 236 | "9 2.8e+00 2.8e-03 6.7e-03 1.93e-01 1.391686195e+05 1.391629831e+05 2.8e-03 0.36 \n", 237 | "10 8.1e-01 8.1e-04 1.1e-03 7.09e-01 1.697949472e+05 1.697930536e+05 8.1e-04 0.39 \n", 238 | "11 2.9e-01 2.9e-04 2.5e-04 1.01e+00 1.784919860e+05 1.784912922e+05 2.9e-04 0.41 \n", 239 | "12 1.1e-01 1.1e-04 5.4e-05 1.08e+00 1.815718609e+05 1.815716042e+05 1.1e-04 0.42 \n", 240 | "13 3.1e-02 3.1e-05 8.3e-06 1.09e+00 1.827028595e+05 1.827027884e+05 3.1e-05 0.47 \n", 241 | "14 8.2e-03 8.2e-06 1.1e-06 1.09e+00 1.829983148e+05 1.829982974e+05 8.2e-06 0.50 \n", 242 | "15 1.8e-03 1.8e-06 1.1e-07 1.10e+00 1.830721249e+05 1.830721216e+05 1.8e-06 0.53 \n", 243 | "16 1.5e-03 1.5e-06 5.7e-08 2.28e-01 1.830760129e+05 1.830760115e+05 1.2e-06 0.55 \n", 244 | "17 2.5e-04 2.5e-07 4.0e-09 1.01e+00 1.830892187e+05 1.830892184e+05 2.1e-07 0.58 \n", 245 | "18 3.2e-05 3.3e-08 1.9e-10 1.00e+00 1.830913273e+05 1.830913272e+05 2.7e-08 0.61 \n", 246 | "19 9.3e-06 2.3e-08 2.9e-11 1.00e+00 1.830915580e+05 1.830915579e+05 7.7e-09 0.63 \n", 247 | "20 3.5e-06 8.6e-09 5.0e-12 1.00e+00 1.830916189e+05 1.830916189e+05 2.6e-09 0.66 \n", 248 | "Optimizer terminated. Time: 0.73 \n", 249 | "\n", 250 | "SDP solving time: 8.0899143 seconds.\n", 251 | "termination status: SLOW_PROGRESS\n", 252 | "solution status: FEASIBLE_POINT\n", 253 | "TSL!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n", 254 | "optimum = 183091.6188839642\n", 255 | "TSL!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n", 256 | "\n", 257 | "******************************************************************************\n", 258 | "This program contains Ipopt, a library for large-scale nonlinear optimization.\n", 259 | " Ipopt is released as open source code under the Eclipse Public License (EPL).\n", 260 | " For more information visit https://github.com/coin-or/Ipopt\n", 261 | "******************************************************************************\n", 262 | "\n", 263 | "Found a locally optimal solution by Ipopt, giving an upper bound: 183168.58335898916 and a relative optimality gap: 0.04201838198099495%.\n" 264 | ] 265 | } 266 | ], 267 | "source": [ 268 | "# Rotation + position with scale \n", 269 | "# Safe corridor WITH connector\n", 270 | "N = 80\n", 271 | "dt = 0.125\n", 272 | "\n", 273 | "@polyvar q[1:N+1, 1:4]\n", 274 | "@polyvar w[1:N+1, 1:4]\n", 275 | "\n", 276 | "@polyvar p[1:N+1, 1:3]\n", 277 | "@polyvar v[1:N+1, 1:3]\n", 278 | "\n", 279 | "@polyvar p_load[1:N+1, 1:3]\n", 280 | "@polyvar v_load[1:N+1, 1:3]\n", 281 | "@polyvar lam[1:N]\n", 282 | "\n", 283 | "@polyvar tau[1:N, 1:3]\n", 284 | "@polyvar f[1:N, 1:3]\n", 285 | "@polyvar r[1:N]\n", 286 | "\n", 287 | "cable_len = 1.0 / 2\n", 288 | "m_load = 0.5\n", 289 | "\n", 290 | "var = append!(reshape(q, 4 * (N+1)), reshape(w, 4*(N+1)), \n", 291 | " reshape(p, 3 * (N+1)), reshape(v, 3*(N+1)), \n", 292 | " reshape(p_load, 3 * (N+1)), reshape(v_load, 3*(N+1)), reshape(lam, N),\n", 293 | " reshape(tau, 3*N), reshape(f, 3 * N), r)\n", 294 | "\n", 295 | "gravity = temp_p[1:3, 1] * 0\n", 296 | "gravity[1] = 0.0\n", 297 | "gravity[2] = 0.0\n", 298 | "gravity[3] = 9.81\n", 299 | "\n", 300 | "eq = [temp_p[1,1] * 0]\n", 301 | "ineq = []\n", 302 | "\n", 303 | "\n", 304 | "quat_list = [[1;0;0;0], [0.86;0;0.5;0], [1;0;1;0], [0.5, 0, 0.85, 0], [0,0,1,0]]\n", 305 | "quat_exec = quat_list[1]\n", 306 | "quat_exec = quat_exec / norm(quat_exec)\n", 307 | "\n", 308 | "eq[1] = q[1, 1] - quat_exec[1]\n", 309 | "append!(eq, [q[1, 2] - quat_exec[2]])\n", 310 | "append!(eq, [q[1, 3] - quat_exec[3]])\n", 311 | "append!(eq, [q[1, 4] - quat_exec[4]])\n", 312 | "\n", 313 | "append!(eq, [w[1, 1] - 2 / dt])\n", 314 | "append!(eq, [w[1, 2] - 0.0])\n", 315 | "append!(eq, [w[1, 3] - 0.0])\n", 316 | "append!(eq, [w[1, 4] - 0.0])\n", 317 | "\n", 318 | "x_init = 0.2\n", 319 | "y_init = 0.2\n", 320 | "\n", 321 | "append!(eq, [p[1, 1] - x_init])\n", 322 | "append!(eq, [p[1, 2] - y_init])\n", 323 | "append!(eq, [p[1, 3] - 0.0])\n", 324 | "\n", 325 | "append!(eq, [v[1, 1] - 0.0])\n", 326 | "append!(eq, [v[1, 2] - 0.0])\n", 327 | "append!(eq, [v[1, 3] - 0.0])\n", 328 | "\n", 329 | "append!(eq, [p_load[1, 1] - x_init])\n", 330 | "append!(eq, [p_load[1, 2] - y_init])\n", 331 | "append!(eq, [p_load[1, 3] - (0 - cable_len)])\n", 332 | "\n", 333 | "append!(eq, [v_load[1, 1] - 0.0])\n", 334 | "append!(eq, [v_load[1, 2] - 0.0])\n", 335 | "append!(eq, [v_load[1, 3] - 0.0])\n", 336 | "\n", 337 | "Cost = 0\n", 338 | "for k = 1:N\n", 339 | "\n", 340 | " qk = q[k, 1:4]\n", 341 | " qkp1 = q[k+1, 1:4]\n", 342 | " wk = w[k, 1:4]\n", 343 | " wkp1 = w[k+1, 1:4]\n", 344 | " tauk = tau[k, 1:3]\n", 345 | "\n", 346 | " pk = p[k, 1:3]\n", 347 | " pkp1 = p[k+1, 1:3]\n", 348 | " vk = v[k, 1:3]\n", 349 | " vkp1 = v[k+1, 1:3]\n", 350 | " fk = f[k, 1:3]\n", 351 | "\n", 352 | " p_load_k = p_load[ k, 1:3]\n", 353 | " p_load_kp1 = p_load[k+1, 1:3]\n", 354 | " v_load_k = v_load[ k, 1:3]\n", 355 | " v_load_kp1 = v_load[k+1, 1:3] \n", 356 | "\n", 357 | " # 1 2 3 4 5 7 10 11 12\n", 358 | "\n", 359 | " if k == 11 || k == 17 || k == 23 || k == 29 || k == 40 || k == 51 || k == 62 || k == 68\n", 360 | " # append!(eq, pkp1 - pk)\n", 361 | " # append!(eq, p_load_k - p_load_kp1)\n", 362 | "\n", 363 | " append!(eq, pk - pkp1)\n", 364 | " append!(eq, p_load_k - p_load_kp1)\n", 365 | " append!(eq, vk - vkp1)\n", 366 | " append!(eq, v_load_k - v_load_kp1)\n", 367 | " append!(eq, qk - qkp1)\n", 368 | " append!(eq, wk - wkp1)\n", 369 | " println(\"connect\")\n", 370 | " else\n", 371 | " Mk = Jb * wk[1] * wk[2:4] + skew(wk[2:4]) * Jb * wk[2:4]\n", 372 | " Mkp1 = Jb * wkp1[1] * wkp1[2:4] + skew(wkp1[2:4]) * Jb * wkp1[2:4]\n", 373 | " \n", 374 | " append!(eq, Mkp1 - Mk - 2 * tauk)\n", 375 | " append!(eq, qkp1 - dt / 2 * LeftMultiplication(qk) * wk)\n", 376 | " \n", 377 | " append!(ineq, [wkp1[1]])\n", 378 | " append!(ineq, [qkp1[1]])\n", 379 | "\n", 380 | " \n", 381 | " vec = pkp1 - p_load_kp1\n", 382 | " append!(eq, (m_load * (v_load_kp1 - v_load_k) / dt + m_load * gravity) - vec * lam[k])\n", 383 | " append!(eq, p_load_kp1 - (p_load_k + v_load_k * dt) )\n", 384 | " append!(eq, [vec[1]^2 + vec[2]^2 + vec[3]^2 - cable_len^2])\n", 385 | "\n", 386 | " append!(ineq, [lam[k] - 0.5])\n", 387 | "\n", 388 | " append!(ineq, [pkp1[3] + 0.2])\n", 389 | " append!(ineq, [- pkp1[3] + 0.2])\n", 390 | " append!(ineq, [- p_load_kp1[3] - cable_len + 0.2])\n", 391 | " append!(ineq, [ p_load_kp1[3] + cable_len + 0.2])\n", 392 | " # append!(ineq, [- p_load_kp1[3] - 0.8])\n", 393 | " # append!(ineq, [ p_load_kp1[3] + 1.2])\n", 394 | "\n", 395 | " vb = temp_p[1:3, 1] * 0\n", 396 | " vb[3] = 1.0\n", 397 | " Q = V * RightMultiplication(qkp1)' * LeftMultiplication(qkp1) * V'\n", 398 | " vb = Q * vb\n", 399 | "\n", 400 | " append!(eq, (m * (vkp1 - vk) / dt + m * gravity) - fk + vec * lam[k])\n", 401 | " append!(eq, vb - r[k] * fk)\n", 402 | " append!(eq, pkp1 - (pk + vk * dt) )\n", 403 | " end\n", 404 | "\n", 405 | " append!(eq, [qkp1[1]^2 + qkp1[2]^2 + qkp1[3]^2 + qkp1[4]^2 - 1])\n", 406 | " append!(eq, [(wkp1[1]^2 + wkp1[2]^2 + wkp1[3]^2 + wkp1[4]^2) - 4 / dt^2])\n", 407 | "\n", 408 | "\n", 409 | " \n", 410 | " if k < 11\n", 411 | " # v1\n", 412 | " append!(ineq, b_list[1] - A_list[1] * p_load_kp1[1:2])\n", 413 | " append!(ineq, b_list[1] - A_list[1] * pkp1[1:2])\n", 414 | " elseif 11 <= k < 17\n", 415 | " # v2\n", 416 | " append!(ineq, b_list[2] - A_list[2] * p_load_kp1[1:2])\n", 417 | " append!(ineq, b_list[2] - A_list[2] * pkp1[1:2])\n", 418 | " elseif 17 <= k < 23\n", 419 | " # v3\n", 420 | " append!(ineq, b_list[3] - A_list[3] * p_load_kp1[1:2])\n", 421 | " append!(ineq, b_list[3] - A_list[3] * pkp1[1:2])\n", 422 | " elseif 23 <= k < 29\n", 423 | " # v4\n", 424 | " append!(ineq, b_list[4] - A_list[4] * p_load_kp1[1:2])\n", 425 | " append!(ineq, b_list[4] - A_list[4] * pkp1[1:2])\n", 426 | " elseif 29 <= k < 40\n", 427 | " append!(ineq, b_list[5] - A_list[5] * p_load_kp1[1:2])\n", 428 | " append!(ineq, b_list[5] - A_list[5] * pkp1[1:2])\n", 429 | " elseif 40 <= k < 51\n", 430 | " append!(ineq, b_list[7] - A_list[7] * p_load_kp1[1:2])\n", 431 | " append!(ineq, b_list[7] - A_list[7] * pkp1[1:2])\n", 432 | " elseif 51 <= k < 62\n", 433 | " append!(ineq, b_list[10] - A_list[10] * p_load_kp1[1:2])\n", 434 | " append!(ineq, b_list[10] - A_list[10] * pkp1[1:2])\n", 435 | " elseif 62 <= k < 68\n", 436 | " append!(ineq, b_list[11] - A_list[11] * p_load_kp1[1:2])\n", 437 | " append!(ineq, b_list[11] - A_list[11] * pkp1[1:2])\n", 438 | " elseif 68 <= k < 80\n", 439 | " append!(ineq, b_list[12] - A_list[12] * p_load_kp1[1:2])\n", 440 | " append!(ineq, b_list[12] - A_list[12] * pkp1[1:2])\n", 441 | " end\n", 442 | "\n", 443 | " # Cost = Cost + (p_load_k[1]^2 + p_load_k[2]^2 + (p_load_k[3] + cable_len)^2)\n", 444 | "\n", 445 | " Cost = Cost + ((p_load_k[3] + cable_len)^2) * 10\n", 446 | " Cost = Cost + (p_load_k[1] - p_load_kp1[1])^2 + (p_load_k[2] - p_load_kp1[2])^2\n", 447 | "\n", 448 | " # Cost = Cost + (v_load_k[1]^2 + v_load_k[2]^2 + v_load_k[3]^2)\n", 449 | " Cost = Cost + (v_load_k - v_load_kp1)' * (v_load_k - v_load_kp1)\n", 450 | " Cost = Cost + lam[k]^2 * 0.01\n", 451 | "\n", 452 | " Cost = Cost + (tauk[1]^2 + tauk[2]^2 + tauk[3]^2) * 0.1\n", 453 | " Cost = Cost + (fk[1]^2 + fk[2]^2 + fk[3]^2) * 0.1\n", 454 | " Cost = Cost + (r[k]^2) * 0.01\n", 455 | "\n", 456 | " Cost = Cost + ((wk[1] - 1)^2 + wk[2]^2 + wk[3]^2 + wk[4]^2) * 10\n", 457 | " Cost = Cost + ((qk[1] - 1)^2 + qk[2]^2 + qk[3]^2 + qk[4]^2) * 0.1\n", 458 | "\n", 459 | " # Cost = Cost + (vk[1]^2 + vk[2]^2 + vk[3]^2) * 1\n", 460 | " Cost = Cost + (vk - vkp1)' * (vk - vkp1)\n", 461 | " # Cost = Cost + (pk[1]^2 + pk[2]^2 + pk[3]^2) * 0.1\n", 462 | " Cost = Cost + pk[3]^2 * 10\n", 463 | " Cost = Cost + (pk[1] - pkp1[1])^2 + (pk[2] - pkp1[2])^2\n", 464 | "\n", 465 | "end\n", 466 | "\n", 467 | "qkp1 = q[end, :]\n", 468 | "wkp1 = w[end, :]\n", 469 | "pkp1 = p[end, :]\n", 470 | "vkp1 = v[end, :]\n", 471 | "\n", 472 | "\n", 473 | "Cost = Cost + ((wkp1[1] - 1)^2 + wkp1[2]^2 + wkp1[3]^2 + wkp1[4]^2) * 10\n", 474 | "Cost = Cost + ((qkp1[1] - 1)^2 + qkp1[2]^2 + qkp1[3]^2 + qkp1[4]^2) * 100\n", 475 | "\n", 476 | "Cost = Cost + (vkp1[1]^2 + vkp1[2]^2 + vkp1[3]^2) * 100\n", 477 | "Cost = Cost + ((pkp1[1] - 4.8)^2 + (pkp1[2] - 4.8)^2 + pkp1[3]^2) * 100\n", 478 | "\n", 479 | "p_load_kp1 = p_load[end, :]\n", 480 | "v_load_kp1 = v_load[end, :]\n", 481 | "Cost = Cost + (v_load_kp1[1]^2 + v_load_kp1[2]^2 + v_load_kp1[3]^2) * 100\n", 482 | "Cost = Cost + ((p_load_kp1[1] - 5)^2 + (p_load_kp1[2] - 5)^2 + (p_load_kp1[3] + 1)^2) * 100\n", 483 | "\n", 484 | "pop = append!([Cost], ineq, eq)\n", 485 | "eq_len = length(eq)\n", 486 | "\n", 487 | "order = 1\n", 488 | "\n", 489 | "time_start = time()\n", 490 | "## TSL note: Use the original PAI for your own applications! \n", 491 | "opt,sol,data =cs_tssos_first(pop, var, order, numeq=eq_len, CS=\"MF\", TS = \"MD\", solution=true, QUIET=false, MomentOne=false)\n", 492 | "# opt,sol,data,model,sol_approx =cs_tssos_first_export_tsl(pop, var, order, numeq=eq_len, CS=\"MF\", TS = false, solution=true, QUIET=false, MomentOne=false)\n", 493 | "time_end = time()\n", 494 | "\n", 495 | "elapsed = time_end - time_start\n", 496 | "\n", 497 | "moment = []\n", 498 | "for k = 1:length(data.Mmatrix)\n", 499 | " append!(moment, [convert(Matrix{Float64}, data.Mmatrix[k])]) # data.Mmatrix[k])\n", 500 | "end\n", 501 | "\n", 502 | "logger = Dict(\"sol\" => sol, \"sol_approx\" => sol_approx,\n", 503 | " \"Ns\" => N, \"moment\" => moment, \"dt\" => dt,\n", 504 | " \"opt\" => opt, \"elapsed\" => elapsed)\n", 505 | " # \"primal\" => flag, \"termination\" => flag2)\n", 506 | "\n", 507 | "\n", 508 | "matwrite(\"logger_3.mat\", logger)" 509 | ] 510 | } 511 | ], 512 | "metadata": { 513 | "kernelspec": { 514 | "display_name": "Julia 1.9.2", 515 | "language": "julia", 516 | "name": "julia-1.9" 517 | }, 518 | "language_info": { 519 | "file_extension": ".jl", 520 | "mimetype": "application/julia", 521 | "name": "julia", 522 | "version": "1.9.2" 523 | } 524 | }, 525 | "nbformat": 4, 526 | "nbformat_minor": 5 527 | } 528 | -------------------------------------------------------------------------------- /Quaternion/freespace.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Quaternion/freespace.mat -------------------------------------------------------------------------------- /Quaternion/logger_3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Quaternion/logger_3.mat -------------------------------------------------------------------------------- /Quaternion/quat-obs.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SangliTeng/LieGroupSDP/324c750083b3e0986540b436736500063a79a092/Quaternion/quat-obs.pdf -------------------------------------------------------------------------------- /Quaternion/visualiza_drone_load_SDP.m: -------------------------------------------------------------------------------- 1 | clear;close all;clc; 2 | load("logger_3.mat") 3 | %% 4 | sv_list = []; 5 | for k = 1:length(moment) 6 | sv = svd(moment{k}); 7 | sv_list = [sv_list; sv(2) / sv(1)]; 8 | end 9 | 10 | figure() 11 | plot( -sort(-log10(sv_list))) 12 | 13 | % sol = sol_approx; 14 | %% 15 | % Ns = dof; 16 | sol_exec = sol; 17 | % load("freespace.mat") 18 | % [sol_refine, obj] = refine_solution_load_cvx_obs(sol, double(Ns), dt, 0.5, diag([1, 2, 1]) / 10, [1, 0, 0, 0]', A_list, b_list) 19 | %% 20 | figure() 21 | subplot(2, 1, 1) 22 | q = sol_exec(1:4*(Ns+1)); 23 | q = reshape(q, [Ns+1, 4]); 24 | hold on 25 | plot(q) 26 | 27 | w = sol_exec(1+4*(Ns+1):2*4*(Ns+1)); 28 | w = reshape(w, [Ns+1, 4]) * dt / 2; 29 | plot(w) 30 | 31 | p = sol_exec(2*4*(Ns+1)+1:2*4*(Ns+1) + 3 * (Ns+1)); 32 | p = reshape(p, [Ns+1, 3]); 33 | 34 | v = sol_exec(2*4*(Ns+1) + 3 * (Ns+1)+1:2*4*(Ns+1) + 2*3*(Ns+1)); 35 | v = reshape(v, [Ns+1, 3]); 36 | 37 | p_load = sol_exec(2*4*(Ns+1) + 2*3*(Ns+1)+1:2*4*(Ns+1) + 3*3*(Ns+1)); 38 | p_load = reshape(p_load, [Ns+1, 3]); 39 | 40 | %% 41 | vertex_obs = { 42 | [[3.4, 2.6]; [3.4, 4.6];[2.4, 4.6]; [2.4, 2.6]; [1.4, 2.2]; [3.8, 0.2];[4.8, 1.2]], 43 | [[1.4, 2.8]; [2.2-0.2, 2.8]; [2.2-0.2, 4.6]; [1.4, 4.6]], 44 | [[1.0, 2.6+0.2]; [1.0, 5.0]; [0.4, 5.0]; [0.4, 2.6+0.2]], 45 | [[1.0, 2.4]; [1.0, 0.0]; [0.4, 0.0]; [0.4, 2.4]], 46 | [[3.8, 3.0+0.2]; [3.8, 5.0]; [4.4, 5.0]; [4.4, 3.0+0.2]], 47 | [[3.8, 2.8]; [3.8, 2.6]; [5.0, 2.6]; [5.0, 2.8]]}; 48 | 49 | vertex_free = { 50 | [[0.4, 0.0]; [0.4, 5.0]; [0.0, 5.0]; [0.0, 0.0]], 51 | [[0.4, 2.4]; [1.0, 2.4]; [1.0, 2.8]; [0.4, 2.8]], 52 | [[1.4, 2.2]; [1.4, 4.6]; [1.0, 4.6]; [1.0, 2.2]], 53 | [[1.4, 2.2]; [2.4, 2.6]; [2.4, 2.8]; [1.4, 2.8]], 54 | [[2.0, 2.8]; [2.4, 2.8]; [2.4, 4.6]; [2.0, 4.6]], 55 | [[1.4, 2.2]; [1.0, 2.2]; [1.0, 0.0]; [3.8, 0.0]; [3.8, 0.2]], 56 | [[3.8, 4.6]; [3.8, 5.0]; [1.0, 5.0]; [1.0, 4.6]], 57 | [[5.0, 0.0]; [5.0, 1.2]; [4.8, 1.2]; [3.8, 0.2]; [3.8, 0.0]], 58 | [[3.4, 2.6]; [4.8, 1.2]; [5.0, 1.2]; [5.0, 2.6]], 59 | [[3.4, 2.6]; [3.8, 2.6]; [3.8, 4.6]; [3.4, 4.6]], 60 | [[3.8, 2.8]; [4.4, 2.8]; [4.4, 3.2]; [3.8, 3.2]], 61 | [[5.0, 2.8]; [5.0, 5.0]; [4.4, 5.0]; [4.4, 2.8]], 62 | }; 63 | figure() 64 | fig_size = [2, 0.6] * 20 ; 65 | h = figure('Renderer', 'painters', 'unit', 'centimeters', 'Position', [5, 5, fig_size]); 66 | 67 | for fig_id = 1:2 68 | 69 | if fig_id == 1 70 | offset = 0.2; 71 | line_style = '-'; 72 | subplot(1, 2, 1, 'Position', [0.1, 0.1, 0.4, 0.9]) 73 | else 74 | offset = 2; 75 | line_style = '-'; 76 | subplot(1, 2, 2, 'Position', [0.1 + 0.5, 0.15, 0.3, 0.8]) 77 | end 78 | % subplot(1, 2, 1, 'Position', [0.1, 0.15, 0.85, 0.8]) 79 | 80 | % subplot(1, 2, fig_id) 81 | 82 | plot3(p(:, 1), p(:, 2), p(:, 3) + offset, '-o', "LineWidth", 1) 83 | hold on 84 | % for k = 1:length(q) 85 | % % if mod(k, 3) == 0 || k <= 10 || k == logger{id}.dof + 1 86 | % len_arrow = 0.1; 87 | % lw_pos = 0.2; 88 | % Rk = quat2rotm(q(k, :)); 89 | % dx = Rk(:, 1) * len_arrow; 90 | % % plot3([x(k), x(k) + dx(1)], [p(k, 2), p(k, 2) + dx(2)], [p(k, 3), p(k, 3) + dx(3)], "r-", "LineWidth",lw_pos); 91 | % plot3([p(k, 1) - dx(1), p(k, 1) + dx(1)], [p(k, 2) - dx(2), p(k, 2) + dx(2)], [p(k, 3) - dx(3), p(k, 3) + dx(3)], "b-o", "LineWidth",lw_pos); 92 | % plot3([p(k, 1) + dx(1)], [ p(k, 2) + dx(2)], [ p(k, 3) + dx(3)], "r-o", "LineWidth",lw_pos); 93 | % 94 | % dy = Rk(:, 2) * len_arrow; 95 | % % plot3([x(k), x(k) + dy(1)], [p(k, 2), p(k, 2) + dy(2)], [p(k, 3), p(k, 3) + dy(3)], "g-", "LineWidth",lw_pos); 96 | % plot3([p(k, 1) - dy(1), p(k, 1) + dy(1)], [p(k, 2) - dy(2), p(k, 2) + dy(2)], [p(k, 3) - dy(3), p(k, 3) + dy(3)], "b-o", "LineWidth",lw_pos); 97 | % 98 | % dz = Rk(:, 3) * len_arrow; 99 | % % plot3([p(k, 1), p(k, 1) + dz(1)], [p(k, 2), p(k, 2) + dz(2)], [p(k, 3), p(k, 3) + dz(3)], "b-", "LineWidth",lw_pos); 100 | % plot3([p(k, 1), p(k, 1) + dz(1)], [p(k, 2), p(k, 2) + dz(2)], [p(k, 3), p(k, 3) + dz(3)], "k-", "LineWidth",lw_pos); 101 | % % else 102 | % 103 | % % end 104 | % end 105 | 106 | hold on 107 | plot3(p_load(:, 1), p_load(:, 2), p_load(:, 3) + offset, '-o', "LineWidth", 1) 108 | daspect([1, 1, 1]) 109 | 110 | for k = 1:length(p)-1 111 | xv = [p(k, 1), p_load(k, 1)]; 112 | yv = [p(k, 2), p_load(k, 2)]; 113 | zv = [p(k, 3), p_load(k, 3)] + offset; 114 | 115 | plot3(xv, yv, zv, 'k--') 116 | end 117 | % (p(:, 1) - p_load(:, 1)).^2 + (p(:, 2) - p_load(:, 2)).^2 + (p(:, 3) - p_load(:, 3)).^2; 118 | 119 | vertex_exec = vertex_obs; 120 | for k = 1:length(vertex_exec) 121 | v = vertex_exec{k}; 122 | 123 | X_top = []; 124 | Y_top = []; 125 | for j = 1:size(v, 1) 126 | v1 = v(j, :); 127 | if j == size(v, 1) 128 | v2 = v(1, :); 129 | else 130 | v2 = v(j+1, :); 131 | end 132 | 133 | if fig_id == 2 134 | plot3([v1(1), v2(1)], [v1(2), v2(2)], [-1, -1], "k-") 135 | plot3([v1(1), v2(1)], [v1(2), v2(2)], [1, 1] * 0, "k-") 136 | end 137 | 138 | X = linspace(v1(1), v2(1), 2); 139 | X = [X(:)'; X(:)']; 140 | Y = linspace(v1(2), v2(2), 2); 141 | Y = [Y(:)'; Y(:)']; 142 | 143 | Z = [0, 0; 1, 1]; 144 | surf(X, Y, Z - 1, 'facecolor','r','LineStyle',line_style,'facealpha',0.1) 145 | 146 | hold on 147 | 148 | end 149 | 150 | pv = polyshape(vertex_obs{k}); 151 | plot(pv, 'facecolor','r','LineStyle',line_style,'facealpha',0.1) 152 | 153 | grid on 154 | box on 155 | xlim([0, 5]) 156 | ylim([0, 5]) 157 | 158 | xticks(0:1:5) 159 | yticks(0:1:5) 160 | 161 | end 162 | daspect([1, 1, 1]) 163 | 164 | if fig_id == 1 165 | view(45, 45 - 10) 166 | elseif fig_id == 2 167 | view(0, 90) 168 | 169 | for k = [1, 2, 3, 4, 5, 7, 10, 11, 12] % 1:length(vertex_free) 170 | pv = polyshape(vertex_free{k}); 171 | plot(pv, 'facecolor','g', 'LineStyle',line_style,'facealpha',0.1) % 172 | end 173 | 174 | for k = [6, 8, 9] % 1:length(vertex_free) 175 | pv = polyshape(vertex_free{k}); 176 | plot(pv, 'facecolor',[1, 1, 1] / 2, 'LineStyle',line_style,'facealpha',0.1) % 177 | end 178 | end 179 | end 180 | set(h, 'Units','pixels'); 181 | set(h, 'PaperPositionMode','Auto','PaperUnits','centimeters','PaperSize', fig_size) 182 | print("quat-obs", "-dpdf") 183 | %% 184 | figure() 185 | lam = sol_exec(2*4*(Ns+1) + 4*3*(Ns+1)+1:2*4*(Ns+1) + 4*3*(Ns+1) + Ns); 186 | 187 | % 188 | tau = sol_exec(2*4*(Ns+1) + 4*3*(Ns+1) + Ns + 1:2*4*(Ns+1) + 4*3*(Ns+1) + Ns + 3*Ns); 189 | tau = reshape(tau, [Ns, 3]) ; 190 | 191 | plot(tau) 192 | % 193 | fk = sol_exec(2*4*(Ns+1) + 4*3*(Ns+1) + Ns + 3*Ns + 1:2*4*(Ns+1) + 4*3*(Ns+1) + Ns + 6*Ns); 194 | fk = reshape(fk, [Ns, 3]); 195 | 196 | plot(fk) 197 | %% 198 | r = sol_exec(end-Ns+1:end); 199 | plot(r) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Convex Geometric Motion Planning on Lie Groups via Sparse Moment Relaxation 2 | 3 | This project solve the motion planning problem using full rigid body dynamics on matrix Lie group. 4 | 5 | To run the code, please install the TSSOS in Julia for global optimization and YALMIP in MATLAB for local search. 6 | MOSEK is required to solve the semidefinite programming. 7 | 8 | ## Requirement: 9 | 10 | - [`TSSOS'](https://github.com/wangjie212/TSSOS.git) 11 | - [`MOSEK'](https://www.mosek.com/downloads/) 12 | - [`YALMIP'](https://yalmip.github.io/download/) 13 | 14 | 15 | ## Citation: 16 | This project is released to reproduce the result of the following conference paper and its journal extension. 17 | 18 | ``` 19 | @INPROCEEDINGS{Teng-RSS-23, 20 | AUTHOR = {Sangli Teng AND Ashkan Jasour AND Ram Vasudevan AND Maani Ghaffari Jadidi}, 21 | TITLE = {{Convex Geometric Motion Planning on Lie Groups via Moment Relaxation}}, 22 | BOOKTITLE = {Proceedings of Robotics: Science and Systems}, 23 | YEAR = {2023}, 24 | ADDRESS = {Daegu, Republic of Korea}, 25 | MONTH = {July}, 26 | DOI = {10.15607/RSS.2023.XIX.058} 27 | } 28 | ``` 29 | --------------------------------------------------------------------------------