1function [qdot_ref] = CLIK_pathFollowing(Jf, Jr, T_I_f, T_I_r, pf_ref, pr_ref)
 2%% TASK 1 - End-effector position
 3%Jf_pos = Jf(1:3,:);  %Position Jacobian
 4%Jf_pos_joints = Jf(1:3,7:end); %Only interested in solving for joint angle velocities
 5%J1 = Jf(1:3,7:end);
 6J1 = Jf(1:3,:);
 7
 8pf_I = tform2trvec(T_I_f);
 9
10%Pseudo-Inverse
11J1_inv = J1'/(J1*J1');
12
13sigma_tilde_I = pf_ref - pf_I; %pf_ref is also in intertial coordinates, i.e pf_ref = pf_ref_I
14R_I_f = tform2rotm(T_I_f);
15sigma_tilde_f = R_I_f'*sigma_tilde_I;
16
17k1 = [1 1 1]*0.5;
18k1 = diag(k1);
19
20%Full position Jacobian
21zeta_ref = J1_inv * (zeros(3,1) + k1*sigma_tilde_f);
22qdot_ref_f = [zeros(4,1); zeta_ref(7:end)];
23
24%Only joint velocities
25% qdot_ref = J1_inv * (zeros(3,1) + k1*sigma_tilde_f);
26% qdot_ref_f = [zeros(4,1); qdot_ref];
27%% TASK 2 - Rear end position
28%Jb_pos = Jr(1:3,:);
29J2 = Jr(1:3,:);
30%J2 = Jr(1:3,7:end);
31
32pr_I = tform2trvec(T_I_r);
33
34%Pseudo-Inverse
35J2_inv = J2'/(J2*J2');
36
37sigma_tilde_I = pr_ref - pr_I;
38R_I_r = tform2rotm(T_I_r);
39sigma_tilde_r = R_I_r'*sigma_tilde_I;
40
41k1 = [1 1 1]*0.5;
42k1 = diag(k1);
43
44%Full position Jacobian
45zeta_ref = J2_inv * (zeros(3,1) - k1*sigma_tilde_r);
46qdot_ref_r = [zeta_ref(10); zeta_ref(9); zeta_ref(8); zeta_ref(7); zeros(4,1)];
47
48%Only joints
49% qdot_ref = J2_inv * (zeros(3,1) - k1*sigma_tilde_r);
50% qdot_ref_r = [qdot_ref(4); qdot_ref(3); qdot_ref(2); qdot_ref(1); zeros(4,1)];
51%% Total
52qdot_ref = qdot_ref_f + qdot_ref_r;
53end