ECE1505 project: better robot and path visualization, making progress…

… on inequality constraints to ECOS format.

ECE1505_Project/GetXYZlinspace.m

@@ -0,0 +1,11 @@
+function xyz_linspace = GetXYZlinspace(NaoRH, theta_linspace) 
+
+    T = size(theta_linspace,2);
+    xyz_linspace = zeros(3,T);
+    for i=1:T
+       fwd = NaoRH.fkine(theta_linspace(:,i)');
+       fwd = fwd(1:3,4);
+       xyz_linspace(:,i) = fwd; 
+    end
+
+end

ECE1505_Project/ObstacleAvoidancePath.m

@@ -0,0 +1,96 @@
+% Generate X-Y-Z constraints based on avoiding a single obstacle in the Y-Z
+% plane. Obstacle is a sphere (circ in y-z plane) symmetric about its origin
+% point p_yz.
+%
+% The generated path follows a very simple scheme (meant for the right
+% arm): we bound the box, clear the distance outwards (y, to the right
+%
+% Assume (without loss of generality in application setting) that the 
+% given obstacle to this function is not in contact with (y_init, z_init),
+% because we would have seen it coming earlier and tried to take evasive
+% action, or even stop. 
+% ex: sphere [1.5;-0.4;1.2;] radius 0.8
+% 
+% Note that moving to the right is moving in the negative-y direction for 
+% the robot in this project. 
+function [Uz, Lz, Uy, Ly] = ObstacleAvoidancePath(y_init, z_init, theta_linspace, p_xyz, radius)
+    % Definitions for the particular robot arm
+    Y_MAX = 3;
+    Y_MIN = -3;
+    Z_MAX = 3;
+    Z_MIN = -3;
+
+    T = size(theta_linspace,2);
+
+    % We start with the end-effector either completely above or 
+    % completely below the obstacle. In either case, Y unconstrained:
+    Uy_phase1 = Y_MAX;
+    Ly_phase1 = Y_MIN;
+    % Now constrain Z depending on start position:
+    if (z_init > (p_xyz(3) + radius)) % starting above obst.
+        Uz_phase1 = Z_MAX;
+        Lz_phase1 = p_xyz(3) + radius;
+    end
+    if (z_init < (p_xyz(3) - radius)) % starting below obst.
+        Uz_phase1 = p_xyz(3) - radius;
+        Lz_phase1 = Z_MIN;
+    end
+    if (abs(z_init - p_xyz(3)) < radius) % uh oh
+        disp('Error: end-effector already in contact with obstacle');
+        Uz = 0; Lz = 0; Uy = 0; Ly = 0;
+        return;
+    end
+
+    % Phase 2: we're either on the left or RIGHT of obs. Constrain Y
+    % as we go up/down
+    Uz_phase2 = Z_MAX;
+    Lz_phase2 = Z_MIN;
+    % Assuming we aim to go outside on the right:
+    Uy_phase2 = p_xyz(2) - radius;
+    Ly_phase2 = Y_MIN;
+
+    % Phase 3: Constrain Z again, let Y be free, so we move sideways
+    % to the target. Again, 2 cases based off initial position:
+    Uy_phase3 = Y_MAX;
+    Ly_phase3 = Y_MIN;
+    if (z_init > (p_xyz(3) + radius)) % starting above obst.
+        % And now we need to stay below obst
+        Uz_phase3 = p_xyz(3) - radius;
+        Lz_phase3 = Z_MIN;
+    end
+    if (z_init < (p_xyz(3) - radius)) % starting below obst.
+        % And now we need to stay above obst
+        Uz_phase3 = Z_MAX;
+        Lz_phase3 = p_xyz(3) + radius;
+    end
+    if (abs(z_init - p_xyz(3)) < radius) % uh oh
+        disp('Error: end-effector already in contact with obstacle');
+        Uz = 0; Lz = 0; Uy = 0; Ly = 0;
+        return;
+    end
+
+    % Put the path approximate path together. For simplicity, split up
+    % into equal 1/3rds of time length. In general we can reinterpret
+    % our solution to the 3 phases by stretching (interpolating) or
+    % compressing them to re-scale. 
+    Uz = zeros(T,1); Lz = zeros(T,1); Uy = zeros(T,1); Ly = zeros(T,1);
+    for i=1:round(T/3)
+        Uz(i) = Uz_phase1;
+        Lz(i) = Lz_phase1;
+        Uy(i) = Uy_phase1;
+        Ly(i) = Ly_phase1;
+    end
+    for i=round(T/3)+1:round(2*T/3)
+        Uz(i) = Uz_phase2;
+        Lz(i) = Lz_phase2;
+        Uy(i) = Uy_phase2;
+        Ly(i) = Ly_phase2;
+    end
+    for i=round(2*T/3)+1:T
+        Uz(i) = Uz_phase3;
+        Lz(i) = Lz_phase3;
+        Uy(i) = Uy_phase3;
+        Ly(i) = Ly_phase3;
+    end
+
+end

ECE1505_Project/QuadraticToSOC.m

@@ -3,5 +3,11 @@
 % Source: http://en.wikipedia.org/wiki/Second-order_cone_programming#Example:_Quadratic_constraint
 % (and some algebra)
 function [Bk0, dk0, bk1, dk1] = QuadraticToSOC(P, q, r)
-
+    A = chol(0.5*P);
+    b = q;
+    c = r;
+    Bk0 = [A; b'/2];
+    dk0 = [zeros(size(A,1),1); (c+1)/2];
+    bk1 = -b'/2;
+    dk1 = (1-c)/2;
 end

ECE1505_Project/QuadraticToSOCTesting.m

@@ -0,0 +1,75 @@
+% Test QuadraticToSOC constraints
+clear P1 P2 P3 P4 P5 P6
+% Create a few constraints 
+n = 100;
+
+P1 = rand(n,n)-0.5; P1 = P1/2 + P1'/2; P1 = P1 + n*eye(n); % ensures P1 >= 0
+P2 = rand(n,n)-0.5; P2 = P2/2 + P2'/2; P2 = P2 + n*eye(n);
+P3 = rand(n,n)-0.5; P3 = P3/2 + P3'/2; P3 = P3 + n*eye(n);
+P4 = rand(n,n)-0.5; P4 = P4/2 + P4'/2; P4 = P4 + n*eye(n);
+P5 = rand(n,n)-0.5; P5 = P5/2 + P5'/2; P5 = P5 + n*eye(n);
+P6 = rand(n,n)-0.5; P6 = P6/2 + P6'/2; P6 = P6 + n*eye(n);
+
+q1 = rand(n,1)-0.5;
+q2 = rand(n,1)-0.5;
+q3 = rand(n,1)-0.5;
+q4 = rand(n,1)-0.5;
+q5 = rand(n,1)-0.5;
+q6 = rand(n,1)-0.5;
+
+r1 = rand(1,1)-0.5 - n;
+r2 = rand(1,1)-0.5 - n;
+r3 = rand(1,1)-0.5 - n;
+r4 = rand(1,1)-0.5 - n;
+r5 = rand(1,1)-0.5 - n;
+r6 = rand(1,1)-0.5 - n;
+
+c = rand(n,1);
+
+% Test program with quadratic constraints
+cvx_begin
+    variable x(n)
+    minimize (c'*x);
+    subject to
+        0.5*x'*P1*x + q1'*x + r1 <= 0;
+        0.5*x'*P2*x + q2'*x + r2 <= 0;
+        0.5*x'*P3*x + q3'*x + r3 <= 0;
+        0.5*x'*P4*x + q4'*x + r4 <= 0;
+        0.5*x'*P5*x + q5'*x + r5 <= 0;
+        0.5*x'*P6*x + q6'*x + r6 <= 0;
+        ones(1,n)*x == 1;
+cvx_end
+
+opt_1 = cvx_optval;
+x_1 = x;
+
+% Now convert to SOC constraints and re-test:
+
+[Bk0_1, dk0_1, bk1_1, dk1_1] = QuadraticToSOC(P1, q1, r1);
+[Bk0_2, dk0_2, bk1_2, dk1_2] = QuadraticToSOC(P2, q2, r2);
+[Bk0_3, dk0_3, bk1_3, dk1_3] = QuadraticToSOC(P3, q3, r3);
+[Bk0_4, dk0_4, bk1_4, dk1_4] = QuadraticToSOC(P4, q4, r4);
+[Bk0_5, dk0_5, bk1_5, dk1_5] = QuadraticToSOC(P5, q5, r5);
+[Bk0_6, dk0_6, bk1_6, dk1_6] = QuadraticToSOC(P6, q6, r6);
+
+cvx_begin
+    variable x(n)
+    minimize (c'*x);
+    subject to
+        norm(Bk0_1*x + dk0_1, 2) <= bk1_1*x + dk1_1;
+        norm(Bk0_2*x + dk0_2, 2) <= bk1_2*x + dk1_2;
+        norm(Bk0_3*x + dk0_3, 2) <= bk1_3*x + dk1_3;
+        norm(Bk0_4*x + dk0_4, 2) <= bk1_4*x + dk1_4;
+        norm(Bk0_5*x + dk0_5, 2) <= bk1_5*x + dk1_5;
+        norm(Bk0_6*x + dk0_6, 2) <= bk1_6*x + dk1_6;
+        ones(1,n)*x == 1;
+cvx_end
+
+opt_2 = cvx_optval;
+x_2 = x;
+
+figure; plot(norm(x_1 - x_2));
+norm(opt_1 - opt_2)
+
+% Good to go!
+

ECE1505_Project/Test2ndOrderFwdKinApprox.asv

@@ -7,7 +7,7 @@ Xmin = [-2.0857, -1.3265, -2.0857, 0.0349]';
 % perturbations around it. 
 
 % 0. Choose a point
-T = 300;
+T = 200;
 thetas = GenerateRandomX(Xmin, Xmax, T);
 
 % 1. Get approximation parameters
@@ -18,12 +18,12 @@ thetas = GenerateRandomX(Xmin, Xmax, T);
 % Approximates p[x,y,z](t) about t=theta
 p_approx_2 = @(t, theta, f, g, H) (f + g'*(t - theta) + 0.5*(t - theta)'*H*(t - theta));
 
-p_approx_3 = @(t, theta, f, g, Hcvx, Hccv) (0.5*t'*Hcvx*t + ();
+p_approx_3 = @(t, theta, f, g, Hcvx, Hccv) (0.5*t'*Hcvx*t + (g-Hccv*theta)'*t + (f-g'*theta+0.5*theta'*Hcvx*theta));
 
 
 
-radius = 0.5; % so our deviation is (t-r) to (t+r)
-Jlim = 300; % how many points within the above deviation to test
+radius = 0.1; % so our deviation is (t-r) to (t+r)
+Jlim = 200; % how many points within the above deviation to test
 errors_x = zeros(Jlim*T,1); errors_y = zeros(Jlim*T,1); errors_z = zeros(Jlim*T,1);
 for i=1:T
 
@@ -43,9 +43,13 @@ for i=1:T
         y_ref = NaoRH_fwd_py(theta); % Tfkine(2,4); %
         z_ref = NaoRH_fwd_pz(theta); % Tfkine(3,4); %
 
-        x_test = p_approx_2(theta_test, theta, NaoRH_fwd_px(theta), g_px, Hcvx_px);
-        y_test = p_approx_2(theta_test, theta, NaoRH_fwd_py(theta), g_py, Hcvx_py);
-        z_test = p_approx_2(theta_test, theta, NaoRH_fwd_pz(theta), g_pz, Hcvx_pz);
+        %x_test = p_approx_2(theta_test, theta, NaoRH_fwd_px(theta), g_px, Hcvx_px);
+        %y_test = p_approx_2(theta_test, theta, NaoRH_fwd_py(theta), g_py, Hcvx_py);
+        %z_test = p_approx_2(theta_test, theta, NaoRH_fwd_pz(theta), g_pz, Hcvx_pz);
+
+        x_test = p_approx_3(theta_test, theta, NaoRH_fwd_px(theta), g_px, Hcvx_px, Hccv_px);
+        y_test = p_approx_3(theta_test, theta, NaoRH_fwd_py(theta), g_py, Hcvx_py, Hccv_px);
+        z_test = p_approx_3(theta_test, theta, NaoRH_fwd_pz(theta), g_pz, Hcvx_pz, Hccv_px);
 
         errors_x((i-1)*Jlim + j) = abs(x_ref - x_test);
         errors_y((i-1)*Jlim + j) = abs(y_ref - y_test);

ECE1505_Project/Test2ndOrderFwdKinApprox.m

@@ -7,7 +7,7 @@
 % perturbations around it. 
 
 % 0. Choose a point
-T = 300;
+T = 200;
 thetas = GenerateRandomX(Xmin, Xmax, T);
 
 % 1. Get approximation parameters
@@ -18,12 +18,12 @@
 % Approximates p[x,y,z](t) about t=theta
 p_approx_2 = @(t, theta, f, g, H) (f + g'*(t - theta) + 0.5*(t - theta)'*H*(t - theta));
 
-p_approx_3 = @(t, theta, f, g, Hcvx, Hccv) (0.5*t'*Hcvx*t + (g-Hcvx*theta)'*t + (f-g'*theta+0.5*theta'*Hcvx*theta));
+p_approx_3 = @(t, theta, f, g, Hcvx, Hccv) (0.5*t'*(Hcvx+Hccv)*t + (g-(Hcvx+Hccv)*theta)'*t + (f-g'*theta+0.5*theta'*(Hcvx+Hccv)*theta));
 
 
 
-radius = 0.15; % so our deviation is (t-r) to (t+r)
-Jlim = 300; % how many points within the above deviation to test
+radius = 0.1; % so our deviation is (t-r) to (t+r)
+Jlim = 200; % how many points within the above deviation to test
 errors_x = zeros(Jlim*T,1); errors_y = zeros(Jlim*T,1); errors_z = zeros(Jlim*T,1);
 for i=1:T