In CVX folder, worked through Nao robot forward kinematics. Creating …

…SCP software for inverse control

SCP_IK/ForwardKinRH.m

@@ -0,0 +1,28 @@
+function right = ForwardKinRH(thetas)
+
+    shoulderOffsetY = 0.098;%98/1000;
+    elbowOffsetY = 0.015;%15/1000;
+    upperArmLength = 0.105;%105/1000;
+    shoulderOffsetZ = 0.100;%;100/1000;
+    HandOffsetX = 0.05775;%57.75/1000;
+    HandOffsetZ = 0.01231;%12.31/1000;
+    LowerArmLength = 0.05595;%55.95/1000;
+
+    t1 = thetas(1); t2 = thetas(2); t3 = thetas(3); t4 = thetas(4); t5 = thetas(5); 
+
+          FWD = TranslationMatrix(0,-shoulderOffsetY,shoulderOffsetZ)* ...
+          RotXYZMatrix(0,t1,0)* ...
+          RotXYZMatrix(0,0,t2)* ...
+          TranslationMatrix(upperArmLength, -elbowOffsetY,0)* ...
+          RotXYZMatrix(t3,0,0)* ...
+          RotXYZMatrix(0,0,t4)* ...
+          TranslationMatrix(LowerArmLength,0,0)* ...
+          RotXYZMatrix(t5,0,0)* ...
+          TranslationMatrix(HandOffsetX,0,-HandOffsetZ);
+
+    rotZ = atan2(FWD(2,1),FWD(1,1));
+    rotY = atan2(-FWD(3,1),sqrt(FWD(3,2)^2 + FWD(3,3)^2));
+    rotX = atan2(FWD(3,2),FWD(3,3));
+    right = [FWD(1:3,4);rotX;rotY;rotZ];
+
+end

SCP_IK/GenerateRandomX.asv

@@ -0,0 +1,7 @@
+function y = GenerateRandomX(Xmin, Xmax, T)
+    X = 2*(rand(size(Xmax,1),T)-0.5);
+    for i=1:T
+       X(:,i) = X(:,i).*(Xmax-Xmin)+Xmin; 
+    end
+    y = X;
+end

SCP_IK/GenerateRandomX.m

@@ -0,0 +1,9 @@
+function y = GenerateRandomX(Xmin, Xmax, T)
+    X = 2*(rand(size(Xmax,1),T)-0.5); %now between -1, 1
+    % to derive, draw out and match ratios of (l1, x1, u1) and (l2, x2, u2)
+    % with l1 = -1, u1 = 1, l2 = Xmin, u2 = Xmax
+    for i=1:T
+       X(:,i) = ((X(:,i)+1).*Xmax + (1-X(:,i)).*Xmin)/2;
+    end
+    y = X;
+end

SCP_IK/QuadraticApproxForwardKinRH.asv

@@ -0,0 +1,38 @@
+% Generate a quadratic approximation of the forward kinematics of the right
+% hand, in the trust region, with Xcurrent (at time T) as the center
+function [P,q,r, residuals, Y, Z] = QuadraticApproxForwardKinRH(TrustRegionMin, TrustRegionMax, numParticles, XcurrentAtT)
+    n = size(TrustRegionMin,1);
+    Z = GenerateRandomX(TrustRegionMin, TrustRegionMax, numParticles);
+    m = 6; % Problem specific to the ForwardKinRH function
+    Y = zeros(m,numParticles);
+    for i=1:numParticles
+        Y(:,i) = ForwardKinRH(Z(:,i));
+    end
+
+    cvx_begin
+        variable P1(n,n)
+        variable q1(n)
+        variable r1
+        f = 0;
+        for i=1:numParticles
+           f = f + abs((Z(:,i)-XcurrentAtT)'*P1*(Z(:,i)-XcurrentAtT) + q1'*(Z(:,i)-XcurrentAtT) + r1 - Y(1,i)).^2;
+        end
+
+        minimize f 
+        subject to 
+            P1 == semidefinite(n,n);
+
+    cvx_end
+
+    P = P1;
+    q = q1;
+    r = r1;
+    residuals = cvx_optval;
+
+    figure;plot(Y(1,:));
+    test = zeros(numParticles,1);
+    for i=1:numParticles
+       test(i) = (Z(:,i))'*P1*(Z(:,i)) + q1'*(Z(:,i)) + r1; 
+    end
+    figure; plot(test);
+end

SCP_IK/QuadraticApproxForwardKinRH.m

@@ -0,0 +1,38 @@
+% Generate a quadratic approximation of the forward kinematics of the right
+% hand, in the trust region, with Xcurrent (at time T) as the center
+function [P,q,r, residuals, Y, Z] = QuadraticApproxForwardKinRH(TrustRegionMin, TrustRegionMax, numParticles, XcurrentAtT)
+    n = size(TrustRegionMin,1);
+    Z = GenerateRandomX(TrustRegionMin, TrustRegionMax, numParticles);
+    m = 6; % Problem specific to the ForwardKinRH function
+    Y = zeros(m,numParticles);
+    for i=1:numParticles
+        Y(:,i) = ForwardKinRH(Z(:,i));
+    end
+
+    cvx_begin
+        variable P1(n,n)
+        variable q1(n)
+        variable r1
+        f = 0;
+        for i=1:numParticles
+           f = f + ((Z(:,i)-XcurrentAtT)'*P1*(Z(:,i)-XcurrentAtT) + q1'*(Z(:,i)-XcurrentAtT) + r1 - Y(1,i)).^2; % messing w/ small numbers
+        end
+
+        minimize f 
+        subject to 
+            P1 == semidefinite(n,n);
+
+    cvx_end
+
+    P = P1;
+    q = q1;
+    r = r1;
+    residuals = cvx_optval;
+
+    figure;plot(Y(1,:));
+    test = zeros(numParticles,1);
+    for i=1:numParticles
+       test(i) = (Z(:,i))'*P1*(Z(:,i)) + q1'*(Z(:,i)) + r1; 
+    end
+    figure; plot(test);
+end

SCP_IK/RotXYZMatrix.m

@@ -0,0 +1,18 @@
+function [Ro] = RotXYZMatrix(xAngle,yAngle,zAngle)
+Rx = [1,                0,          0;
+    0,                cos(xAngle), -sin(xAngle);
+    0,                sin(xAngle), cos(xAngle);];
+
+Ry = [cos(yAngle),       0,          sin(yAngle);
+    0,                1,          0;
+    -sin(yAngle),      0,          cos(yAngle);];
+
+Rz = [cos(zAngle),       -sin(zAngle),0;
+    sin(zAngle)        cos(zAngle), 0;
+    0,                0,          1;];
+R = Rx*Ry*Rz;
+
+R = [R, [0;0;0];
+    [0,0,0],1];
+Ro = R;
+end

SCP_IK/SCP_IK_main.asv

@@ -0,0 +1,71 @@
+%%%%%%%%%%%% Sequential Convex Optimization for Inverse Motion Control %%%%%%%%%%%%%
+%
+% Tuned for the Nao humanoid robot http://en.wikipedia.org/wiki/Nao_%28robot%29
+% Enter a 6D target pose for the right-arm end effector: (px, py, pz, roll, pitch, yaw)
+% Receive a sequence of joint control commands to realize this, while
+% minimizing the energy expenditure of the trajectory.
+
+function finalTrajectory = SCP_IK_main(start, target)
+
+PenaltyItrs = 6;
+ConvexItrs = 6; 
+mu = 10; % initial penalty weighting factor
+penaltyScale = 10; % penalty increasing weighting factor
+constraintSlackTol = 0.5e-3; % less than half a percent for range [-1,1]
+convexResidualTol = 0.125; % will accept this % mean error on convex approx. to subproblems
+
+%%%%% PROBLEM STATEMENT %%%%%
+% Minimize the energy expenditure, measured by instantaneous changes in
+% joint angles, such that our final position is our target position, and
+% the joint angles stay within a specified acceptable range for the right
+% arm. More specifically,
+%
+% minimize sum(norm(X(:,t+1)-X(:,t),2))
+% subject to
+%           ForwardKinRH(X(:,T) = target
+%           Xmin <= X(:,t) <= Xmax, t = 1,...,T 
+% 
+% Where T time slices are used to interpolate the motion. The forward
+% kinematics constraint on the end effector target is the nonconvex portion
+% of this problem. 
+%
+
+%%%%% PROBLEM SPECIFIC VARS %%%%%
+% Xmax, Xmin: represent joint angle ranges in radians
+% T: number of time steps of our control path
+Xmax = [2.0857, 0.3142, 2.0857, 1.5446, 1.8238]';
+Xmin = [-2.0857, -1.3265, -2.0857, 0.0349, -1.8238]';
+T = 10;
+TrustRegionMin = Xmin*0.2;
+TrustRegionMax = Xmax*0.2;
+Xcurrent = GenerateRandom(TrustRegionMin,TrustRegionMax,T);
+Xprevious = Xcurrent;
+
+for i=1:PenaltyItrs
+
+   for j=1:ConvexItrs
+       % Step 1: convexify problem. In our case, get an approximation to
+       % h(x) --> the forward kinematics function
+       % Uses a particle method to generate a quadratic approximation to
+       % the function within the trust region.
+       [P,q,r, residuals] = QuadraticApproxForwardKinRH(TrustRegionMin, TrustRegionMax, numParticles);
+
+
+
+
+
+
+
+   end
+
+   if (TestOriginalProblemConstraints(Xmin,Xmax,T,start,target,X) < constraintSlackTol)
+       break;
+   else
+      mu = k*mu; %increase penalty weighting, re-optimize 
+   end
+
+end
+
+finalTrajectory = Xcurrent;
+
+end

SCP_IK/SCP_IK_main.m

@@ -0,0 +1,71 @@
+%%%%%%%%%%%% Sequential Convex Optimization for Inverse Motion Control %%%%%%%%%%%%%
+%
+% Tuned for the Nao humanoid robot http://en.wikipedia.org/wiki/Nao_%28robot%29
+% Enter a 6D target pose for the right-arm end effector: (px, py, pz, roll, pitch, yaw)
+% Receive a sequence of joint control commands to realize this, while
+% minimizing the energy expenditure of the trajectory.
+
+function finalTrajectory = SCP_IK_main(start, target)
+
+PenaltyItrs = 6;
+ConvexItrs = 6; 
+mu = 10; % initial penalty weighting factor
+penaltyScale = 10; % penalty increasing weighting factor
+constraintSlackTol = 0.5e-3; % less than half a percent for range [-1,1]
+convexResidualTol = 0.125; % will accept this % mean error on convex approx. to subproblems
+
+%%%%% PROBLEM STATEMENT %%%%%
+% Minimize the energy expenditure, measured by instantaneous changes in
+% joint angles, such that our final position is our target position, and
+% the joint angles stay within a specified acceptable range for the right
+% arm. More specifically,
+%
+% minimize sum(norm(X(:,t+1)-X(:,t),2))
+% subject to
+%           ForwardKinRH(X(:,T) = target
+%           Xmin <= X(:,t) <= Xmax, t = 1,...,T 
+% 
+% Where T time slices are used to interpolate the motion. The forward
+% kinematics constraint on the end effector target is the nonconvex portion
+% of this problem. 
+%
+
+%%%%% PROBLEM SPECIFIC VARS %%%%%
+% Xmax, Xmin: represent joint angle ranges in radians
+% T: number of time steps of our control path
+Xmax = [2.0857, 0.3142, 2.0857, 1.5446, 1.8238]';
+Xmin = [-2.0857, -1.3265, -2.0857, 0.0349, -1.8238]';
+T = 10;
+TrustRegionMin = Xmin*0.2;
+TrustRegionMax = Xmax*0.2;
+Xcurrent = GenerateRandom(TrustRegionMin,TrustRegionMax,T);
+Xprevious = Xcurrent;
+
+for i=1:PenaltyItrs
+
+   for j=1:ConvexItrs
+       % Step 1: convexify problem. In our case, get an approximation to
+       % h(x) --> the forward kinematics function @ time T
+       % Uses a particle method to generate a quadratic approximation to
+       % the function within the trust region.
+       [P,q,r, residuals] = QuadraticApproxForwardKinRH(TrustRegionMin, TrustRegionMax, numParticles, Xcurrent(:,T));
+
+
+
+
+
+
+
+   end
+
+   if (TestOriginalProblemConstraints(Xmin,Xmax,T,start,target,X) < constraintSlackTol)
+       break;
+   else
+      mu = k*mu; %increase penalty weighting, re-optimize 
+   end
+
+end
+
+finalTrajectory = Xcurrent;
+
+end

SCP_IK/TestOriginalProblemConstraints.asv

@@ -0,0 +1,20 @@
+% Return the total slack in the original constraints, which are:
+% Xmin <= X <= Xmax
+% ForwardKinRH(X(:,T)) = target
+function y = TestOriginalProblemConstraints(Xmin, Xmax, T, start, target, X) % X is current point
+
+    inequalitySlack = 0; 
+
+    for i=1:T
+       slack1 = X(:,i) - Xmax;
+       inequalitySlack = inequalitySlack + sum(slack1);
+       slack2 = -X(:,i) + Xmin;
+       inequalitySlack = inequalitySlack + sum(slack2);
+    end
+
+    slack3 = sum(abs(ForwardKinRH(X(:,T)) - target));
+    slack4 = 
+
+    y = inequalitySlack + equalitySlack; %scalarized, though we may break it up above if need be
+
+end

SCP_IK/TestOriginalProblemConstraints.m

@@ -0,0 +1,22 @@
+% Return the total slack in the original constraints, which are:
+% Xmin <= X <= Xmax
+% ForwardKinRH(X(:,T)) = target
+function y = TestOriginalProblemConstraints(Xmin, Xmax, T, start, target, X) % X is current point
+
+    inequalitySlack = 0; 
+
+    for i=1:T
+       slack1 = max(X(:,i) - Xmax, 0);
+       inequalitySlack = inequalitySlack + sum(slack1);
+       slack2 = max(-X(:,i) + Xmin, 0);
+       inequalitySlack = inequalitySlack + sum(slack2);
+    end
+
+    slack3 = sum(abs(ForwardKinRH(X(:,T)) - target));
+    slack4 = sum(abs(X(:,1) - start));
+
+    equalitySlack = slack3 + slack4;
+
+    y = inequalitySlack + equalitySlack; %scalarized, though we may break it up above if need be
+
+end

SCP_IK/TranslationMatrix.m

@@ -0,0 +1,6 @@
+function [Trans] = TranslationMatrix(x,y,z)
+    Trans = eye(4,4);
+    Trans(1,4) = x;
+    Trans(2,4) = y;
+    Trans(3,4) = z;
+end