1function [V_b_Ib_ref, qdot_ref, activeTasks] = CLIK_collisionAvoidance(T_I_f, T_I_b, T_I_r, obstacleSurfacePoints, Jf, Jb, Jr)
2p_I_f = tform2trvec(T_I_f);
3p_I_b = tform2trvec(T_I_b);
4p_I_r = tform2trvec(T_I_r);
5
6R_I_f = tform2rotm(T_I_f);
7R_I_b = tform2rotm(T_I_b);
8R_I_r = tform2rotm(T_I_r);
9
10
11
12
13
14
15
16persistent tasks;
17persistent CA_r;
18persistent Position_r;
19if isempty(tasks)
20 k_CA = 3;
21 k_pos = 1;
22 distance_CA = 0.5;
23 distance_pos = 0;
24 W = [1 1 1 1 1 1 1 1 1 1];
25
26 CA_f = NormTask(1,distance_CA,k_CA,W);
27 CA_b = NormTask(1,distance_CA,1,W);
28 CA_r = NormTask(1,distance_CA,k_CA,W);
29
30 Position_f = NormTask(0,distance_pos,k_pos,W);
31 Position_b = NormTask(0,distance_pos,k_pos,W);
32 Position_r = NormTask(0,distance_pos,k_pos,W);
33
34 tasks = {CA_f, CA_b, Position_f};
35
36end
37dim = 10;
38p_attract = [-15; 0; -180];
39
40
41Jf_pos = Jf(1:3,:);
42Jb_pos = Jb(1:3,:);
43Jr_pos = Jr(1:3,:);
44Jr_pos_joints = Jr(1:3,7:end);
45
46
47p_obst_f = obstacleSurfacePoints(:,1);
48p_obst_b = obstacleSurfacePoints(:,2);
49p_obst_r = obstacleSurfacePoints(:,3);
50
51
52p_attract_f = p_I_f + (p_attract - p_I_f)/norm(p_attract - p_I_f);
53p_attract_b = p_I_b + (p_attract - p_I_b)/norm(p_attract - p_I_b);
54p_attract_r = p_I_r + (p_attract - p_I_r)/norm(p_attract - p_I_r);
55
56tasks{1}.update(Jf_pos, p_obst_f, p_I_f, R_I_f, p_attract_f);
57tasks{3}.update(Jf_pos, p_attract_f, p_I_f, R_I_f, p_attract_f);
58
59tasks{2}.update(Jb_pos, p_obst_b, p_I_b, R_I_b, p_attract_b);
60
61
62CA_r.update(Jr_pos_joints, p_obst_r, p_I_r, R_I_r, p_attract_r);
63Position_r.update(Jr_pos,[3;3;-180],p_I_r,R_I_r,p_attract_r);
64
65activeTasks = determineActiveTasks(tasks);
66Q = multipleTaskCLIK(tasks, activeTasks, dim);
67zeta_ref = Q(:,1);
68
69
70if CA_r.Active == 1
71 qr = CA_r.J_inv*(0 + CA_r.Gain*CA_r.Sigma_tilde);
72else
73 qr = zeros(dim,1);
74end
75
76
77V_b_Ib_ref = zeta_ref(1:6);
78qdot_ref = [qr(4); qr(3); qr(2); qr(1); zeta_ref(7:end)];
79
80end