1classdef NormTask < handle
 2   properties
 3      Active %Boolean. True if the set-based task is active
 4      Gain %CLIK gain
 5      Weights %Weights used if weighted pseudo-inverse is applied
 6      SetBased %Boolean. True if set-based task
 7      J %Task Jacobian
 8      J_inv %Inverse of task jacobian
 9      Sigma_des %Desired distance from point
10      Sigma_actual %Actual distance from point
11      Sigma_tilde %sigma_des - sigma_actual
12      Sigma_ref_dot %(sigma_des_dot - Gain*sigma_tilde) from CLIK equation
13      Sigma_des_dot %Derivative of desired value
14   end
15   methods
16       function obj = NormTask(setBased, sigma_des, gain, W)
17           if nargin > 0
18               obj.SetBased = setBased;
19               obj.Sigma_des = sigma_des;
20               obj.Gain = gain;
21               obj.Weights = W;
22               obj.Sigma_des_dot = 0;
23               
24               obj.Active = 0;
25               if setBased == 0 %Equality constraints are always active
26                   obj.Active = 1;
27               end
28           else
29               error('Enter setBased, sigma_des, gain and weights');
30           end
31       end
32       
33       function out = update(obj, Jpos, point, pos, orientation, travel_dir)
34           
35            W_inv = diag(1./obj.Weights);
36
37            diff_I = point - pos;
38            R_I_f = orientation;
39            diff_f = R_I_f'*diff_I;
40            obj.J = diff_f'/norm(diff_f)*Jpos;
41
42            %Pseudo-Inverse
43            obj.J_inv = obj.J'/(obj.J*obj.J');
44            %Weighted pseudo-inverse
45            %J1_inv = W_inv*J1'/(J1*W_inv*J1');
46
47            obj.Sigma_actual = sqrt(diff_f'*diff_f); %Actual distance to point
48            obj.Sigma_tilde = obj.Sigma_des - obj.Sigma_actual;
49            
50            obj.Sigma_ref_dot = obj.Sigma_des_dot - obj.Gain*obj.Sigma_tilde;
51            
52            if obj.SetBased == 1 %Check if the constraint is active
53                if obj.Sigma_actual < obj.Sigma_des || obj.Active == 1
54                    obj.Active = 1;
55                    p_obst = R_I_f'*(point-pos); %Vector from point on USM to point to avoid
56                    p_travel = R_I_f'*(travel_dir-pos); %Vector from point on USM to desired location
57                    if p_obst'*p_travel <= 0 && obj.Sigma_actual >= obj.Sigma_des
58                        obj.Active = 0;
59                    end
60                end
61            end
62            
63            out = 1;
64       end
65   end
66   
67end
68