matlab_visual_servo/examples/continuous_time_vs/load_O_features.m at main · Vanvitelli-Robotics/matlab_visual_servo · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
% O = Object
% C = camera
% W = space/world

% features w.r.t. object frame

O_features = [ ...
    [ 0; 0;  0], ...
     [.5; 0;  0], ...
    [ 1; 0;  0], ...
     [ 0; 0; .5], ...
     [.5; 0; .5], ...
     [ 1; 0; .5], ...
     [ 0; 0;  1], ...
     [.5; 0;  1], ...
    [ 1; 0;  1] ...
    ]*0.04;
O_T_C_desired = SE3(rotx(90), .1*[.5, .5, .5]');
% return
% O_features = [ ...
%     [ 0; 0;  0], ...
%     [ 0; 0;  1], ...
%     [ 0; 1; 0], ...
%     [ 0; 1; 1], ...
%     ];
% O_features = [ ... allineate con origine T*
%     [ 0; -1;  .5], ...
%     [ 0; 0;  .5], ...
%     [ 0; 1; .5], ...
%     ];
% O_features = [ ... allineati ma non dipendenti
%     [ -1; 0;  .5], ...
%     [ 0; 0;  .75], ...
%     [ 1; 0; 1], ...
%     ...[ 0; 1;  .5+(.25/2*1)], ...
%     ];
% O_T_C_desired = SE3(rotx(90)*rotx(0), [0, 2, .5]');

%% 30

O_features = [ ...
    [[ 0; 0;  0], ...
     [.5; .5;  0], ...
    [ 1; 0;  0], ...
     [ 0; .5; .5], ...
    ... [.5; 0; .5], ...
    ... [ 1; 0; .5], ...
    ... [ 0; 1;  1], ...
    ... [.5; 0;  1], ...
    ...[ 1; 1;  1] ...
    ]*1, ...
    ]*0.04;
O_T_C_desired = SE3(rotx(90), .1*[.5, .5, .5]');

return

%% triangolo equi
ll = 0.01*3;
hh = sqrt(ll^2 - (.5*ll)^2);
O_features = [ ...
    [ -ll/2; -hh/2;  0], ...
    [ ll/2; -hh/2; 0 ], ...
    [ 0; hh/2; 0], ...
    ];
bari = sum(O_features,2)/size(O_features,2);
O_T_C_desired = SE3(rotx(0), bari' + [0 0 .05]);


%% quadrato
% ll = 1;
% O_features = [ ...
%     [ -ll/2;  0; 0], ...
%     [ +ll/2;  0; 0 ], ...
%     [ -ll/2;  ll; 0], ...
%     [ +ll/2;  ll; 0], ...
%     ... extra
%     [ -ll/2;  0; 0.3]];
% bari = sum(O_features,2)/size(O_features,2);
% O_T_C_desired = SE3(rotx(0), bari' + [0 0 1]);