1classdef NormTask < handle
2 properties
3 Active
4 Gain
5 Weights
6 SetBased
7 J
8 J_inv
9 Sigma_des
10 Sigma_actual
11 Sigma_tilde
12 Sigma_ref_dot
13 Sigma_des_dot
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
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
43 obj.J_inv = obj.J'/(obj.J*obj.J');
44
45
46
47 obj.Sigma_actual = sqrt(diff_f'*diff_f);
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
53 if obj.Sigma_actual < obj.Sigma_des || obj.Active == 1
54 obj.Active = 1;
55 p_obst = R_I_f'*(point-pos);
56 p_travel = R_I_f'*(travel_dir-pos);
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