%Stiffness2D.m

%Works through compliance transform in 2D (3x3 matrices)
clear

% R-Leg
syms theta L ctheta
Jtheta=[-L*sin(theta);L*cos(theta);1];
Ctheta=[ctheta];
C=Jtheta*Ctheta*transpose(Jtheta);

%Velocity transform in 2D (3x3)
abR=eye(2);
syms rbax rbay  %x and y portions of vector from original coordinate frame to center of compliance
rbaX=[rbay;-rbax]; %2D cross product "matrix" (really a vector...)
abTv = [abR -abR*rbaX;0 0 1];  
eqn=rbaX-C(1:2,3)/C(3,3); %Solve for translation that puts C(1,3) and C(2,3) == 0 
[rbaxS,rbayS]=solve(eqn(1),eqn(2),'rbax,rbay')
%Verify
abTv=subs(abTv,{rbax,rbay},{rbaxS,rbayS});
Ccenter=abTv*C*transpose(abTv)
%should get 3x3 matrix with (0,0,x)' as last column.  Upper left 2x2 matrix
%should be symmetric, which implies a simple rotation is needed to make it
%(and the whole system) diagonal.

%RP-Leg
syms cl
Jtheta=[-L*sin(theta) cos(theta);L*cos(theta) sin(theta);1 0];
Ctheta=diag([ctheta,cl]);
C=Jtheta*Ctheta*transpose(Jtheta);

%Velocity transform in 2D (3x3)
abR=eye(2);
clear rbax rbay
syms rbax rbay
rbaX=[rbay;-rbax]; %2D cross product "matrix" (really a vector...)
abTv = [abR -abR*rbaX;0 0 1];  
eqn=rbaX-C(1:2,3)/C(3,3);
[rbaxS,rbayS]=solve(eqn(1),eqn(2),'rbax,rbay')
%Verify
abTv=subs(abTv,{rbax,rbay},{rbaxS,rbayS})
Ccenter=abTv*C*transpose(abTv)
%should get 3x3 matrix with (0,0,x)' as last column.  Upper left 2x2 matrix
%should be symmetric, which implies a simple rotation is needed to make it
%diagonal.


%find that rotation - may be better with numbers, not symbolic...
mat=Ccenter(1:2,1:2);

syms a b d
RotMat=[-(d-a+sqrt(a^2+4*b^2-2*a*d+d^2))/(2*b), (a-d+sqrt(a^2+4*b^2-2*a*d+d^2))/(2*b); 1 1];
RotMat2=subs(RotMat,{a,b,d},{mat(1,1),mat(2,1),mat(2,2)});
RotMat2=simplify(subs(RotMat2,'csgn(cl)',1));
normer=simplify(transpose(RotMat2)*RotMat2);
scaler=simplify(sqrt(inv(normer)));
R=simplify(RotMat2*scaler);
%For R and RP leg, 
R=[-sin(theta),cos(theta);cos(theta),sin(theta)];
 
%verify
abTv2=subs([R -R*rbaX;0 0 1],{rbax,rbay},{rbaxS,rbayS})
Ccenter2=simplify(abTv2*C*transpose(abTv2))