1function [qdot_ref] = CLIK_pathFollowing(Jf, Jr, T_I_f, T_I_r, pf_ref, pr_ref)
2
3
4
5
6J1 = Jf(1:3,:);
7
8pf_I = tform2trvec(T_I_f);
9
10
11J1_inv = J1'/(J1*J1');
12
13sigma_tilde_I = pf_ref - pf_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
21zeta_ref = J1_inv * (zeros(3,1) + k1*sigma_tilde_f);
22qdot_ref_f = [zeros(4,1); zeta_ref(7:end)];
23
24
25
26
27
28
29J2 = Jr(1:3,:);
30
31
32pr_I = tform2trvec(T_I_r);
33
34
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
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
49
50
51
52qdot_ref = qdot_ref_f + qdot_ref_r;
53end