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%% Define tasks
11%NormTask: A task variable that tries to keep the norm between two points
12%at a certain value
13%CONSTRUCTOR: NormTask(Set-based task, sigma_des, gain, weighting matrix)
14%sigma_des = 0 => The USM will move towards this point
15%sigma_des = k => The USM will keep a distance k from this point
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]; %6 base velocities + 4 joint angles
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    %tasks = {Position_f};
36end
37dim = 10; %(6 base vel + 4 joints)
38p_attract = [-15; 0; -180]; %THIS IS THE GOAL POSITION FOR THE END-EFFECTOR
39%% Update tasks
40%Extract position Jacobian
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%Get closest ostacle surface point
47p_obst_f = obstacleSurfacePoints(:,1);
48p_obst_b = obstacleSurfacePoints(:,2);
49p_obst_r = obstacleSurfacePoints(:,3);
50
51%Calculate task errors and find the task jacobian
52p_attract_f = p_I_f + (p_attract - p_I_f)/norm(p_attract - p_I_f);%Normalise to get const. vel
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%Rear tasks
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%% Calculate system velocities
65activeTasks = determineActiveTasks(tasks);
66Q = multipleTaskCLIK(tasks, activeTasks, dim);
67zeta_ref = Q(:,1);
68
69%CA for the rear
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