clear all

clc
syms alpha beta gamma real

% rotational matrices calculated in previous problem set

R_B1 = [1,0,0;0,cos(alpha),-sin(alpha);0,sin(alpha),cos(alpha)];
R_12 = [cos(beta),0,sin(beta);0,1,0;-sin(beta),0,cos(beta)];
R_23 = [cos(gamma),0,sin(gamma);0,1,0;-sin(gamma),0,cos(gamma)];

% write down the 3x1 relative position vectors for link lengths l_i=1
r_3F_in3 = [0,0,-1]';
r_23_in2 = [0,0,-1]';
r_12_in1 = [0,0,-1]';
r_B1_inB = [0,1,0]';

% write down the homogeneous transformations

H_23 = [R_23,r_23_in2;0 0 0 1];
H_12 = [R_12,r_12_in1;0 0 0 1];
H_B1 = [R_B1,r_B1_inB;0 0 0 1];

% create the cumulative transformation matrix

H_B3 = H_B1*H_12*H_23;

q = [alpha;beta;gamma];

% find the foot point position vector

r_BF_inB = H_B3(1:3,:)*[r_3F_in3;1];

% determine the foot point Jacobian J_BF_inB=d(r_BF_inB)/dq

J_BF_inB = [diff(r_BF_inB,q(1)) diff(r_BF_inB,q(2)) diff(r_BF_inB,q(3))];

% what generalized velocity dq do you have to apply in a configuration q = [0;60;-

120]
% to lift the foot in vertical direction with v = [0;0;-1m/s];

qval = pi/180 * ([0;60;-120]);

Jval = eval(subs(J_BF_inB, q, qval));
dx = [0;0;-1];

dq = pinv(Jval) * dx;

-----------------------------------------------------------------------------------
---------

% given are the functions

% r_BF_inB(alpha,beta,gamma) and
% J_BF_inB(alpha,beta,gamma)
% for the foot positon respectively Jacobian

r_BF_inB = @(alpha,beta,gamma)[...
-sin(beta + gamma) - sin(beta);...
sin(alpha)*(cos(beta + gamma) + cos(beta) + 1) + 1;...
-cos(alpha)*(cos(beta + gamma) + cos(beta) + 1)];

J_BF_inB = @(alpha,beta,gamma)[...
0, - cos(beta + gamma) -
cos(beta), -cos(beta + gamma);...
cos(alpha)*(cos(beta + gamma) + cos(beta) + 1), -sin(alpha)*(sin(beta + gamma) +
sin(beta)), -sin(beta + gamma)*sin(alpha);...
sin(alpha)*(cos(beta + gamma) + cos(beta) + 1), cos(alpha)*(sin(beta + gamma) +
sin(beta)), sin(beta + gamma)*cos(alpha)];

% write an algorithm for the inverse kinematics problem to

% find the generalized coordinates q that gives the endeffector position rGoal =
% [0.2,0.5,-2]' and store it in qGoal
q0 = pi/180*([0,-30,60])';
rGoal = [0.2,0.5,-2]';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% ikine problem 1: enter here your algorithm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

terminate = false;
while (~terminate)
r = r_BF_inB(q0(1), q0(2), q0(3));
error = rGoal - r;
normval = norm(error);
if( normval < 1e-3 )
terminate = true;
end
qGoal = q0 + pinv(J_BF_inB(q0(1),q0(2),q0(3))) * error;
q0 = qGoal;
end
disp('joint angles');
disp(qGoal * 180/pi);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ikine problem 3: best solution for an unreachable position
% essentially, do levenberg-marquardt minimisation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
rGoal = [-1.5;1;-2.5]; % unreachable

terminate = false;
previousNormError = 1e9;
lambda = 0.1;
while( ~terminate )
r = r_BF_inB(q0(1), q0(2), q0(3));
error = rGoal - r;
normError = norm(error);
deltaError = abs(previousNormError - normError);
previousNormError = normError;
if( deltaError < 1e-8 )
terminate = true;
end
J = J_BF_inB(q0(1),q0(2),q0(3));
qGoal1 = q0 + inv(J' * J + lambda * eye(3)) * J' * error;
q0 = qGoal1;
end

disp('joint angles');
disp(qGoal1 * 180/pi);

-----------------------------------------------------------------------------------
--------

% given are the functions

% r_BF_inB(alpha,beta,gamma) and
% J_BF_inB(alpha,beta,gamma)
% for the foot positon respectively Jacobian

r_BF_inB = @(alpha,beta,gamma)[...
- sin(beta + gamma) - sin(beta);...
sin(alpha)*(cos(beta + gamma) + cos(beta) + 1) + 1;...
-cos(alpha)*(cos(beta + gamma) + cos(beta) + 1)];

J_BF_inB = @(alpha,beta,gamma)[...
0, - cos(beta + gamma) -
cos(beta), -cos(beta + gamma);...
cos(alpha)*(cos(beta + gamma) + cos(beta) + 1), -sin(alpha)*(sin(beta + gamma) +
sin(beta)), -sin(beta + gamma)*sin(alpha);...
sin(alpha)*(cos(beta + gamma) + cos(beta) + 1), cos(alpha)*(sin(beta + gamma) +
sin(beta)), sin(beta + gamma)*cos(alpha)];

% write an algorithm for the inverse differential kinematics problem to

% find the generalized velocities dq to follow a circle in the body xz plane
% around the start point rCenter with a radius of r=0.5 and a
% frequeny of 0.25Hz. The start configuration is q = pi/180*([0,-60,120])',
% which defines the center of the circle
q0 = pi/180*([0,-60,120])';
dq0 = zeros(3,1);
rCenter = r_BF_inB(q0(1),q0(2),q0(3));
radius = 0.5;
f = 0.25;
rGoal = @(t) rCenter + radius*[sin(2*pi*f*t),0,cos(2*pi*f*t)]';
drGoal = @(t) 2*pi*f*radius*[cos(2*pi*f*t),0,-sin(2*pi*f*t)]';

% define here the time resolution

deltaT = 0.01;
timeArr = 0:deltaT:1/f;

% q, r, and rGoal are stored for every point in time in the following arrays
qArr = zeros(3,length(timeArr));
rArr = zeros(3,length(timeArr));
rGoalArr = zeros(3,length(timeArr));

q = q0;
dq = dq0;
for i=1:length(timeArr)
t = timeArr(i);
% data logging, don't change this!
q = q+deltaT*dq;
qArr(:,i) = q;
rArr(:,i) = r_BF_inB(q(1),q(2),q(3));
rGoalArr(:,i) = rGoal(t);

% controller:
% step 1: create a simple p controller to determine the desired foot
% point velocity
v = drGoal(t) + 10.0 * (rGoal(t) - rArr(:,i));
% step 2: perform inverse differential kinematics to calculate the
% generalized velocities
dq = pinv(J_BF_inB(q(1),q(2),q(3))) * v;

end