iCub-main
icub-main
app
depth2kin
scripts
show_eyes_axes.m
Go to the documentation of this file.
1
function
show_eyes_axes
(Tl,Tr)
2
3
figure
('color','
white
');
4
view
(3);
5
grid
on
;
6
hold
on
;
7
8
Tl0
=eye(4,4);
Tl0
(1,4)=-0.034;
9
Tr0
=eye(4,4);
Tr0
(1,4)=+0.034;
10
11
show_eye
(
Tl0
*eye(4,4),
Tl0
*Tl);
12
show_eye
(
Tr0
*eye(4,4),
Tr0
*Tr);
13
14
15
function
show_eye
(To,Tn)
16
17
scale=0.005;
18
19
% old axes
20
x
=To(1:3,1);
21
y
=To(1:3,2);
22
z
=To(1:3,3);
23
p
=To(1:3,4);
24
25
quiver3
(
p
(1),
p
(2),
p
(3),
p
(1)+
x
(1),
p
(2)+
x
(2),
p
(3)+
x
(3),scale,'r--','linewidth',2.0);
26
quiver3
(
p
(1),
p
(2),
p
(3),
p
(1)+
y
(1),
p
(2)+
y
(2),
p
(3)+
y
(3),scale,'g--','linewidth',2.0);
27
quiver3
(
p
(1),
p
(2),
p
(3),
p
(1)+
z
(1),
p
(2)+
z
(2),
p
(3)+
z
(3),scale,'b--','linewidth',2.0);
28
29
% new axes
30
x
=Tn(1:3,1);
31
y
=Tn(1:3,2);
32
z
=Tn(1:3,3);
33
p
=Tn(1:3,4);
34
35
quiver3
(
p
(1),
p
(2),
p
(3),
p
(1)+
x
(1),
p
(2)+
x
(2),
p
(3)+
x
(3),scale,'r','linewidth',2.0);
36
quiver3
(
p
(1),
p
(2),
p
(3),
p
(1)+
y
(1),
p
(2)+
y
(2),
p
(3)+
y
(3),scale,'g','linewidth',2.0);
37
quiver3
(
p
(1),
p
(2),
p
(3),
p
(1)+
z
(1),
p
(2)+
z
(2),
p
(3)+
z
(3),scale,'b','linewidth',2.0);
38
39
axis
equal
;
function
function[xhat, yhat, F, H]
Definition:
compute_ekf_fast.m:1
z
z
Definition:
show_eyes_axes.m:22
Tl0
Tl0
Definition:
show_eyes_axes.m:8
show_eye
show_eye(Tl0 *eye(4, 4), Tl0 *Tl)
y
y
Definition:
show_eyes_axes.m:21
show_eyes_axes
function show_eyes_axes(Tl, Tr) figure('color'
on
grid on
Definition:
show_eyes_axes.m:5
quiver3
quiver3(p(1), p(2), p(3), p(1)+x(1), p(2)+x(2), p(3)+x(3), scale, 'r--', 'linewidth', 2.0)
white
function white
Definition:
show_eyes_axes.m:3
x
old axes x
Definition:
show_eyes_axes.m:20
p
p
Definition:
show_eyes_axes.m:23
Tr0
Tr0
Definition:
show_eyes_axes.m:9
view
view(3)
equal
axis equal
Definition:
show_eyes_axes.m:39
figure
figure(1) hold on grid on plot(time
Generated on Thu Oct 3 2024 09:32:54 for iCub-main by
1.9.1