-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathvrrobot.m
148 lines (121 loc) · 8.08 KB
/
vrrobot.m
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
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
function []=vrrobot( robot )
% VRROBOT Create virtual world for robot (axes) animation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create wrl file of robot reference frame 0
file=sprintf([...
'#VRML V2.0 utf8 \n',...
'# Created with Robotics Symbolic Matlab Toolbox \n',...
'# Instituto Superior T?cnico - DEM \n',...
'Background { \n',...
' skyColor 0.3125 0.4336 0.6328 \n',...
' groundColor 0 0 0.63 \n',...
'} \n',...
'WorldInfo { \n',...
' title "D-H Reference Frames for Virtual Reality Robot" \n',...
' info ["Copyright 2011 DEM IST" "$Revision: 0 $" "$Date: 2011/05/31 $" "$Author: Jorge M. M. Martins $" ] \n',...
'} \n',...
'NavigationInfo { \n',...
' type "EXAMINE" \n',...
'} \n',...
'Viewpoint { \n',...
' description "View 1" \n',...
' orientation 1 0 0 1.5708 \n',...
' position 0 -3 1.1 \n',...
'} \n',...
'DEF Frame0 Transform { \n',...
' children [ \n',...
vrmlaxes('x0') ,...
vrmlaxes('y0') ,...
vrmlaxes('z0') ]);
%Close parenthesis and brackets at end of file
fileend=sprintf([']\n' '}\n']);
for i=1:size(robot,1) %add all links
if robot(i,1)==['q' num2str(i)]
d=double(robot(i,5)); %get ref value if d is a coordinate
v=double(robot(i,2));
else
d=double(robot(i,1));
v=double(robot(i,5)); %get ref value if v is a coordinate
end
a=double(robot(i,3));
alpha=double(robot(i,4));
%Add link to file
file=[file vrmllink(i,d,v,a,alpha)];
%Add closing parenthesis and brackets at end of file
fileend=sprintf([']\n' '}\n' '}\n' '}\n' fileend]);
end
file=[file fileend]; % Join file with endoffile
fid=fopen('vrrobot.wrl','w');
fprintf(fid,'%s',file);
fclose(fid);
function link = vrmllink(linknumber,d,v,a,alpha)
link=sprintf([...
' DEF Jt', num2str(linknumber), '_DH_dv Transform { \n',...
' translation 0 0 ', num2str(d), ' \n',...
' rotation 0 0 1 ', num2str(v), ' \n',...
' children DEF Jt', num2str(linknumber), '_DH_aa Transform { \n',...
' translation ', num2str(a), ' 0 0 \n',...
' rotation 1 0 0 ', num2str(alpha), ' \n',...
' children DEF Link', num2str(linknumber), ' Transform { \n',...
' children [ \n',...
vrmlaxes(['x' num2str(linknumber)]) ,...
vrmlaxes(['y' num2str(linknumber)]) ,...
vrmlaxes(['z' num2str(linknumber)]) ]);
function axes = vrmlaxes(label)
if label(1)=='x'
transrot=sprintf(['translation .1 0 0 \n',...
'rotation 0 0 -1 1.5708 \n']);
end
if label(1)=='y'
transrot=sprintf('translation 0 .1 0 \n');
end
if label(1)=='z'
transrot=sprintf(['translation 0 0 .1 \n',...
'rotation 1 0 0 1.5708 \n']);
end
axes=sprintf([...
' DEF ', label ,'_axes Transform { \n',...
transrot ,...
' children [ \n',...
' DEF ', label,'_axes_arrowbody Shape { \n',...
' geometry Cylinder { \n',...
' radius 0.005 \n',...
' height 0.2 \n',...
' } \n',...
' appearance Appearance { \n',...
' material Material { \n',...
' } \n',...
' } \n',...
' } \n',...
' DEF ', label, '_axes_arrowtip Transform { \n',...
' translation 0 .12 0 \n',...
' children Shape { \n',...
' geometry Cone { \n',...
' height .04 \n',...
' bottomRadius .015 \n',...
' } \n',...
' appearance Appearance { \n',...
' material Material { \n',...
' } \n',...
' } \n',...
' } \n',...
' } \n',...
' DEF ', label, '_axes_label Transform { \n',...
' translation 0 .15 0 \n',...
' children Billboard { \n',...
' axisOfRotation 0 0 0 \n',...
' children Shape { \n',...
' geometry DEF TEXT Text { \n',...
' length 0 \n',...
' fontStyle FontStyle { \n',...
' style "BOLDITALIC" \n',...
' size 0.1 \n',...
' family "SANS" \n',...
' } \n',...
' string "', label,'" \n',...
' } \n',...
' } \n',...
' } \n',...
' } \n',...
' ] \n',...
'} \n']);