GrxVrmlExporter.java
Go to the documentation of this file.
1 package com.generalrobotix.ui.util;
2 
3 import java.io.BufferedWriter;
4 import java.io.File;
5 import java.io.FileWriter;
6 import java.util.Vector;
7 import java.util.regex.*;
8 
9 import jp.go.aist.hrp.simulator.AppearanceInfo;
10 import jp.go.aist.hrp.simulator.MaterialInfo;
11 import jp.go.aist.hrp.simulator.ShapeInfo;
12 import jp.go.aist.hrp.simulator.ShapePrimitiveType;
13 import jp.go.aist.hrp.simulator.TextureInfo;
14 
15 import com.generalrobotix.ui.item.GrxLinkItem;
16 import com.generalrobotix.ui.item.GrxModelItem;
17 import com.generalrobotix.ui.item.GrxSegmentItem;
18 import com.generalrobotix.ui.item.GrxSensorItem;
19 import com.generalrobotix.ui.item.GrxShapeItem;
20 import com.generalrobotix.ui.item.GrxExtraJointItem;
21 
22 public class GrxVrmlExporter {
29  public static boolean export(GrxModelItem model, String exportPath){
30  // TODO check existence of exportPath
31  int idx = exportPath.lastIndexOf(File.separatorChar);
32  if(idx==-1)
33  idx = exportPath.lastIndexOf('/');
34  String exportDir = exportPath.substring(0, idx);
35 
36  BufferedWriter writer = null;
37  try {
38  writer = new BufferedWriter(new FileWriter(exportPath));
39 
40  writer.write("#VRML V2.0 utf8\n");
41  writer.write("\n");
42  writer.write("PROTO Joint [\n");
43  writer.write(" exposedField SFVec3f center 0 0 0\n");
44  writer.write(" exposedField MFNode children []\n");
45  writer.write(" exposedField MFFloat llimit []\n");
46  writer.write(" exposedField MFFloat lvlimit []\n");
47  writer.write(" exposedField SFRotation limitOrientation 0 0 1 0\n");
48  writer.write(" exposedField SFString name \"\"\n");
49  writer.write(" exposedField SFRotation rotation 0 0 1 0\n");
50  writer.write(" exposedField SFVec3f scale 1 1 1\n");
51  writer.write(" exposedField SFRotation scaleOrientation 0 0 1 0\n");
52  writer.write(" exposedField MFFloat stiffness [ 0 0 0 ]\n");
53  writer.write(" exposedField SFVec3f translation 0 0 0\n");
54  writer.write(" exposedField MFFloat ulimit []\n");
55  writer.write(" exposedField MFFloat uvlimit []\n");
56  writer.write(" exposedField SFString jointType \"\"\n");
57  writer.write(" exposedField SFInt32 jointId -1\n");
58  writer.write(" exposedField SFVec3f jointAxis 0 0 1\n");
59  writer.write("\n");
60  writer.write(" exposedField SFFloat gearRatio 1\n");
61  writer.write(" exposedField SFFloat rotorInertia 0\n");
62  writer.write(" exposedField SFFloat rotorResistor 0\n");
63  writer.write(" exposedField SFFloat torqueConst 1\n");
64  writer.write(" exposedField SFFloat encoderPulse 1\n");
65  writer.write("]\n");
66  writer.write("{\n");
67  writer.write(" Transform {\n");
68  writer.write(" center IS center\n");
69  writer.write(" children IS children\n");
70  writer.write(" rotation IS rotation\n");
71  writer.write(" scale IS scale\n");
72  writer.write(" scaleOrientation IS scaleOrientation\n");
73  writer.write(" translation IS translation\n");
74  writer.write(" }\n");
75  writer.write("}\n");
76  writer.write("\n");
77  writer.write("PROTO Segment [\n");
78  writer.write(" field SFVec3f bboxCenter 0 0 0\n");
79  writer.write(" field SFVec3f bboxSize -1 -1 -1\n");
80  writer.write(" exposedField SFVec3f centerOfMass 0 0 0\n");
81  writer.write(" exposedField MFNode children [ ]\n");
82  writer.write(" exposedField SFNode coord NULL\n");
83  writer.write(" exposedField MFNode displacers [ ]\n");
84  writer.write(" exposedField SFFloat mass 0 \n");
85  writer.write(" exposedField MFFloat momentsOfInertia [ 0 0 0 0 0 0 0 0 0 ]\n");
86  writer.write(" exposedField SFString name \"\"\n");
87  writer.write(" eventIn MFNode addChildren\n");
88  writer.write(" eventIn MFNode removeChildren\n");
89  writer.write("]\n");
90  writer.write("{\n");
91  writer.write(" Group {\n");
92  writer.write(" addChildren IS addChildren\n");
93  writer.write(" bboxCenter IS bboxCenter\n");
94  writer.write(" bboxSize IS bboxSize\n");
95  writer.write(" children IS children\n");
96  writer.write(" removeChildren IS removeChildren\n");
97  writer.write(" }\n");
98  writer.write("}\n");
99  writer.write("\n");
100  writer.write("PROTO Humanoid [\n");
101  writer.write(" field SFVec3f bboxCenter 0 0 0\n");
102  writer.write(" field SFVec3f bboxSize -1 -1 -1\n");
103  writer.write(" exposedField SFVec3f center 0 0 0\n");
104  writer.write(" exposedField MFNode humanoidBody [ ]\n");
105  writer.write(" exposedField MFString info [ ]\n");
106  writer.write(" exposedField MFNode joints [ ]\n");
107  writer.write(" exposedField SFString name \"\"\n");
108  writer.write(" exposedField SFRotation rotation 0 0 1 0\n");
109  writer.write(" exposedField SFVec3f scale 1 1 1\n");
110  writer.write(" exposedField SFRotation scaleOrientation 0 0 1 0\n");
111  writer.write(" exposedField MFNode segments [ ]\n");
112  writer.write(" exposedField MFNode sites [ ]\n");
113  writer.write(" exposedField SFVec3f translation 0 0 0\n");
114  writer.write(" exposedField SFString version \"1.1\"\n");
115  writer.write(" exposedField MFNode viewpoints [ ]\n");
116  writer.write("]\n");
117  writer.write("{\n");
118  writer.write(" Transform {\n");
119  writer.write(" bboxCenter IS bboxCenter\n");
120  writer.write(" bboxSize IS bboxSize\n");
121  writer.write(" center IS center\n");
122  writer.write(" rotation IS rotation\n");
123  writer.write(" scale IS scale\n");
124  writer.write(" scaleOrientation IS scaleOrientation\n");
125  writer.write(" translation IS translation\n");
126  writer.write(" children [\n");
127  writer.write(" Group {\n");
128  writer.write(" children IS viewpoints\n");
129  writer.write(" }\n");
130  writer.write(" Group {\n");
131  writer.write(" children IS humanoidBody \n");
132  writer.write(" }\n");
133  writer.write(" ]\n");
134  writer.write(" }\n");
135  writer.write("}\n");
136  writer.write("\n");
137  writer.write("PROTO VisionSensor [\n");
138  writer.write(" exposedField SFVec3f translation 0 0 0\n");
139  writer.write(" exposedField SFRotation rotation 0 0 1 0\n");
140  writer.write(" exposedField MFNode children [ ]\n");
141  writer.write(" exposedField SFFloat fieldOfView 0.785398\n");
142  writer.write(" exposedField SFString name \"\"\n");
143  writer.write(" exposedField SFFloat frontClipDistance 0.01\n");
144  writer.write(" exposedField SFFloat backClipDistance 10.0\n");
145  writer.write(" exposedField SFString type \"NONE\"\n");
146  writer.write(" exposedField SFInt32 sensorId -1\n");
147  writer.write(" exposedField SFInt32 width 320\n");
148  writer.write(" exposedField SFInt32 height 240\n");
149  writer.write(" exposedField SFFloat frameRate 30\n");
150  writer.write("]\n");
151  writer.write("{\n");
152  writer.write(" Transform {\n");
153  writer.write(" rotation IS rotation\n");
154  writer.write(" translation IS translation\n");
155  writer.write(" children IS children\n");
156  writer.write(" }\n");
157  writer.write("}\n");
158  writer.write("\n");
159  writer.write("\n");
160  writer.write("PROTO ForceSensor [ \n");
161  writer.write(" exposedField SFVec3f maxForce -1 -1 -1\n");
162  writer.write(" exposedField SFVec3f maxTorque -1 -1 -1\n");
163  writer.write(" exposedField SFVec3f translation 0 0 0\n");
164  writer.write(" exposedField SFRotation rotation 0 0 1 0\n");
165  writer.write(" exposedField MFNode children [ ]\n");
166  writer.write(" exposedField SFInt32 sensorId -1\n");
167  writer.write("]\n");
168  writer.write("{\n");
169  writer.write(" Transform {\n");
170  writer.write(" translation IS translation\n");
171  writer.write(" rotation IS rotation\n");
172  writer.write(" children IS children\n");
173  writer.write(" }\n");
174  writer.write("}\n");
175  writer.write("\n");
176  writer.write("PROTO Gyro [\n");
177  writer.write(" exposedField SFVec3f maxAngularVelocity -1 -1 -1\n");
178  writer.write(" exposedField SFVec3f translation 0 0 0\n");
179  writer.write(" exposedField SFRotation rotation 0 0 1 0\n");
180  writer.write(" exposedField MFNode children [ ]\n");
181  writer.write(" exposedField SFInt32 sensorId -1\n");
182  writer.write("]\n");
183  writer.write("{\n");
184  writer.write(" Transform {\n");
185  writer.write(" translation IS translation\n");
186  writer.write(" rotation IS rotation\n");
187  writer.write(" children IS children\n");
188  writer.write(" }\n");
189  writer.write("}\n");
190  writer.write("\n");
191  writer.write("PROTO AccelerationSensor [\n");
192  writer.write(" exposedField SFVec3f maxAcceleration -1 -1 -1\n");
193  writer.write(" exposedField SFVec3f translation 0 0 0\n");
194  writer.write(" exposedField SFRotation rotation 0 0 1 0\n");
195  writer.write(" exposedField MFNode children [ ]\n");
196  writer.write(" exposedField SFInt32 sensorId -1\n");
197  writer.write("]\n");
198  writer.write("{\n");
199  writer.write(" Transform {\n");
200  writer.write(" translation IS translation\n");
201  writer.write(" rotation IS rotation\n");
202  writer.write(" children IS children\n");
203  writer.write(" }\n");
204  writer.write("}\n");
205  writer.write("\n");
206  writer.write("PROTO RangeSensor [\n");
207  writer.write(" exposedField SFVec3f translation 0 0 0\n");
208  writer.write(" exposedField SFRotation rotation 0 0 1 0\n");
209  writer.write(" exposedField MFNode children [ ]\n");
210  writer.write(" exposedField SFInt32 sensorId -1\n");
211  writer.write(" exposedField SFFloat scanAngle 3.14159 #[rad]\n");
212  writer.write(" exposedField SFFloat scanStep 0.1 #[rad]\n");
213  writer.write(" exposedField SFFloat scanRate 10 #[Hz]\n");
214  writer.write(" exposedField SFFloat maxDistance 10\n");
215  writer.write("]\n");
216  writer.write("{\n");
217  writer.write(" Transform {\n");
218  writer.write(" translation IS translation\n");
219  writer.write(" rotation IS rotation\n");
220  writer.write(" children IS children\n");
221  writer.write(" }\n");
222  writer.write("}\n");
223  writer.write("\n");
224  writer.write("PROTO Plane [\n");
225  writer.write(" exposedField SFVec3f size 10 10 0\n");
226  writer.write("]\n");
227  writer.write("{\n");
228  writer.write(" Box {\n");
229  writer.write(" size IS size\n");
230  writer.write(" }\n");
231  writer.write("}\n");
232  writer.write("\n");
233  writer.write("PROTO ExtraJoint [\n");
234  writer.write(" exposedField SFString link1Name \"\"\n");
235  writer.write(" exposedField SFString link2Name \"\"\n");
236  writer.write(" exposedField SFVec3f link1LocalPos 0 0 0\n");
237  writer.write(" exposedField SFVec3f link2LocalPos 0 0 0\n");
238  writer.write(" exposedField SFString jointType \"xyz\"\n");
239  writer.write(" exposedField SFVec3f jointAxis 1 0 0\n");
240  writer.write("]\n");
241  writer.write("{\n");
242  writer.write("}\n");
243  writer.write("\n");
244  writer.write("NavigationInfo {\n");
245  writer.write(" avatarSize 0.5\n");
246  writer.write(" headlight TRUE\n");
247  writer.write(" type [\"EXAMINE\", \"ANY\"]\n");
248  writer.write("}\n");
249  writer.write("\n");
250  writer.write("Background {\n");
251  writer.write(" skyColor 0.4 0.6 0.4\n");
252  writer.write("}\n");
253  writer.write("\n");
254  writer.write("Viewpoint {\n");
255  writer.write(" position 3 0 0.835\n");
256  writer.write(" orientation 0.5770 0.5775 0.5775 2.0935\n");
257  writer.write("}\n");
258  writer.write("\n");
259  for(int i=0; i<model.extraJoints_.size(); i++){
260  exportExtraJoint(writer, model.extraJoints_.get(i));
261  writer.write("\n");
262  }
263  writer.write("DEF "+model.getName()+" Humanoid{\n");
264  writer.write(" humanoidBody [\n");
265  Vector<GrxLinkItem> links = exportLink(writer, model.rootLink(), " ", exportDir, model.getURL(false));
266  writer.write(" ]\n");
267  writer.write(" joints [\n");
268  for (int i=0; i<links.size(); i++){
269  writer.write(" USE "+links.get(i).getName()+",\n");
270  }
271  writer.write(" ]\n");
272  writer.write(" segments [\n");
273  for (int i=0; i<links.size(); i++){
274  GrxLinkItem link = links.get(i);
275  for (int j=0; j<link.children_.size(); j++){
276  if (link.children_.get(j) instanceof GrxSegmentItem){
277  writer.write(" USE "+link.children_.get(j).getName()+",\n");
278  }
279  }
280  }
281  writer.write(" ]\n");
282  writer.write("}\n");
283  return true;
284  } catch (Exception e) {
285  e.printStackTrace();
286  } finally {
287  try {
288  if (writer != null) {
289  writer.flush();
290  writer.close();
291  }
292  } catch (Exception e) {
293  }
294  }
295  return false;
296  }
297 
307  public static Vector<GrxLinkItem> exportLink(BufferedWriter writer, GrxLinkItem link, String indent,
308  String exportDir, String mainPath){
309  Vector<GrxLinkItem> links = new Vector<GrxLinkItem>();
310  links.add(link);
311  try{
312  writer.write(indent+"DEF "+link.getName()+" Joint {\n");
313  writer.write(indent+" jointType \""+link.jointType_+"\"\n");
314  if (link.jointId_ != -1) writer.write(indent+" jointId "+link.jointId_+"\n");
315  if (link.jointType_.equals("rotate") || link.jointType_.equals("slide")){
316  writer.write(indent+" jointAxis "+link.getProperty("jointAxis")+"\n");
317  }
318  if (!valueEquals(link.getProperty("translation"),"0.0 0.0 0.0 ")){
319  writer.write(indent+" translation "+link.getProperty("translation")+"\n");
320  }
321  if (!isZero(link.getProperty("rotation"),3)){
322  writer.write(indent+" rotation "+link.getProperty("rotation")+"\n");
323  }
324  if (!link.getProperty("ulimit").equals("")) writer.write(indent+" ulimit ["+link.getProperty("ulimit")+"]\n");
325  if (!link.getProperty("llimit").equals("")) writer.write(indent+" llimit ["+link.getProperty("llimit")+"]\n");
326  if (!link.getProperty("uvlimit").equals("")) writer.write(indent+" uvlimit ["+link.getProperty("uvlimit")+"]\n");
327  if (!link.getProperty("lvlimit").equals("")) writer.write(indent+" lvlimit ["+link.getProperty("lvlimit")+"]\n");
328  if (link.gearRatio_ != 1.0) writer.write(indent+" gearRatio "+link.gearRatio_+"\n");
329  if (link.rotorInertia_ != 0.0) writer.write(indent+" rotorInertia "+link.rotorInertia_+"\n");
330  if (link.rotorResistor_ != 0.0) writer.write(indent+" rotorResistor "+link.rotorResistor_+"\n");
331  if (link.torqueConst_ != 1.0) writer.write(indent+" torqueConst "+link.torqueConst_+"\n");
332  if (link.encoderPulse_ != 1.0) writer.write(indent+" encoderPulse "+link.encoderPulse_+"\n");
333  writer.write(indent+" children[\n");
334  for (int i=0; i<link.children_.size(); i++){
335  if (link.children_.get(i) instanceof GrxSensorItem){
336  exportSensor(writer, (GrxSensorItem)link.children_.get(i), indent+" ", exportDir, mainPath);
337  }
338  }
339  for (int i=0; i<link.children_.size(); i++){
340  if (link.children_.get(i) instanceof GrxSegmentItem){
341  exportSegment(writer, (GrxSegmentItem)link.children_.get(i), indent+" ", exportDir, mainPath);
342  }
343  }
344  for (int i=0; i<link.children_.size(); i++){
345  if (link.children_.get(i) instanceof GrxLinkItem){
346  Vector<GrxLinkItem> childLinks = exportLink(writer, (GrxLinkItem)link.children_.get(i),
347  indent+" ", exportDir, mainPath);
348  links.addAll(childLinks);
349  }
350  }
351  writer.write(indent+" ]\n");
352  writer.write(indent+"}\n");
353  }catch(Exception ex){
354  ex.printStackTrace();
355  }
356  return links;
357  }
358 
359  public static void exportSegment(BufferedWriter writer, GrxSegmentItem segment, String indent, String exportDir, String mainPath){
360  try{
361  String translation = segment.getProperty("translation");
362  String rotation = segment.getProperty("rotation");
363  boolean useTransform = false;
364  if(!valueEquals(translation, "0.0 0.0 0.0 " ) || !isZero(rotation, 3 )){
365  useTransform = true;
366  writer.write(indent+"Transform {\n");
367  writer.write(indent+" translation "+translation+"\n");
368  writer.write(indent+" rotation "+rotation+"\n");
369  writer.write(indent+" children[\n");
370  indent += " ";
371  }
372  writer.write(indent+"DEF "+segment.getName()+" Segment{\n");
373  writer.write(indent+" centerOfMass "+segment.getProperty("centerOfMass")+"\n");
374  writer.write(indent+" mass "+segment.getProperty("mass")+"\n");
375  writer.write(indent+" momentsOfInertia [ "+segment.getProperty("momentsOfInertia")+"]\n");
376  writer.write(indent+" children[\n");
377  for (int i=0; i<segment.children_.size(); i++){
378  if (segment.children_.get(i) instanceof GrxShapeItem){
379  GrxShapeItem shape = (GrxShapeItem)segment.children_.get(i);
380  exportShape(writer, shape, indent+" ", exportDir, mainPath);
381  }
382  }
383  writer.write(indent+" ]\n");
384  writer.write(indent+"}\n");
385  if(useTransform){
386  writer.write(indent+" ]\n");
387  writer.write(indent+"}\n");
388  }
389  }catch(Exception ex){
390  ex.printStackTrace();
391  }
392  }
393 
402  public static void exportSensor(BufferedWriter writer, GrxSensorItem sensor, String indent, String exportDir, String mainPath){
403  try{
404  String nodeType="unknown";
405  if (sensor.type_.equals("Force")){
406  nodeType = "ForceSensor";
407  }else if(sensor.type_.equals("RateGyro")){
408  nodeType = "Gyro";
409  }else if(sensor.type_.equals("Acceleration")){
410  nodeType = "AccelerationSensor";
411  }else if(sensor.type_.equals("Vision")){
412  nodeType = "VisionSensor";
413  }else if(sensor.type_.equals("Range")){
414  nodeType = "RangeSensor";
415  }
416  writer.write(indent+"DEF "+sensor.getName()+" "+nodeType+" {\n");
417  writer.write(indent+" sensorId "+sensor.id_+"\n");
418  if (!valueEquals(sensor.getProperty("translation"),"0.0 0.0 0.0 ")){
419  writer.write(indent+" translation "+sensor.getProperty("translation")+"\n");
420  }
421  if (!isZero(sensor.getProperty("rotation"),3)){
422  writer.write(indent+" rotation "+sensor.getProperty("rotation")+"\n");
423  }
424  // sensor specific parameters
425  if (nodeType.equals("VisionSensor")){
426  if (!valueEquals(sensor.getProperty("fieldOfView"),"0.785398")){
427  writer.write(indent+" fieldOfView "+sensor.getProperty("fieldOfView")+"\n");
428  }
429  if (!valueEquals(sensor.getProperty("frontClipDistance"),"0.01")){
430  writer.write(indent+" frontClipDistance "+sensor.getProperty("frontClipDistance")+"\n");
431  }
432  if (!valueEquals(sensor.getProperty("backClipDistance"),"10.0")){
433  writer.write(indent+" backClipDistance "+sensor.getProperty("backClipDistance")+"\n");
434  }
435  if (!valueEquals(sensor.getProperty("width"),"320")){
436  writer.write(indent+" width "+sensor.getProperty("width")+"\n");
437  }
438  if (!valueEquals(sensor.getProperty("height"),"240")){
439  writer.write(indent+" height "+sensor.getProperty("height")+"\n");
440  }
441  if (!valueEquals(sensor.getProperty("frameRate"),"30.0")){
442  writer.write(indent+" frameRate "+sensor.getProperty("frameRate")+"\n");
443  }
444  if (!sensor.getProperty("cameraType").equals("NONE")){
445  writer.write(indent+" type \""+sensor.getProperty("cameraType")+"\"\n");
446  }
447  if (!sensor.getProperty("name").equals("")){
448  writer.write(indent+" name \""+sensor.getProperty("name")+"\"\n");
449  }
450  }else if(nodeType.equals("ForceSensor")){
451  String maxf = sensor.getProperty("maxForce");
452  if (!valueEquals(maxf, "-1.0 -1.0 -1.0 ")){
453  writer.write(indent+" maxForce "+maxf+"\n");
454  }
455  String maxt = sensor.getProperty("maxTorque");
456  if (!valueEquals(maxt, "-1.0 -1.0 -1.0 ")){
457  writer.write(indent+" maxTorque "+maxt+"\n");
458  }
459  }else if(nodeType.equals("Gyro")){
460  String max = sensor.getProperty("maxAngularVelocity");
461  if (!valueEquals(max, "-1.0 -1.0 -1.0 ")){
462  writer.write(indent+" maxAngularVelocity "+max+"\n");
463  }
464  }else if(nodeType.equals("AccelerationSensor")){
465  String max = sensor.getProperty("maxAcceleration");
466  if (!valueEquals(max, "-1.0 -1.0 -1.0 ")){
467  writer.write(indent+" maxAcceleration "+max+"\n");
468  }
469  }else if(nodeType.equals("RangeSensor")){
470  String angle = sensor.getProperty("scanAngle");
471  if (!valueEquals(angle, "3.14159")){
472  writer.write(indent+" scanAngle "+angle+"\n");
473  }
474  String step = sensor.getProperty("scanStep");
475  if (!valueEquals(step, "0.1")){
476  writer.write(indent+" scanStep "+step+"\n");
477  }
478  String rate = sensor.getProperty("scanRate");
479  if (!valueEquals(rate, "10")){
480  writer.write(indent+" scanRate "+rate+"\n");
481  }
482  String distance = sensor.getProperty("maxDistance");
483  if (!valueEquals(distance, "10")){
484  writer.write(indent+" maxDistance "+distance+"\n");
485  }
486  }
487  if (sensor.children_.size() > 0){
488  writer.write(indent+" children[\n");
489  for (int i=0; i<sensor.children_.size(); i++){
490  if (sensor.children_.get(i) instanceof GrxShapeItem){
491  exportShape(writer, (GrxShapeItem)sensor.children_.get(i), indent+" ", exportDir, mainPath);
492  }
493  }
494  writer.write(indent+" ]\n");
495  }
496  writer.write(indent+"}\n");
497  }catch(Exception ex){
498  ex.printStackTrace();
499  }
500  }
501 
510  public static void exportShape(BufferedWriter writer, GrxShapeItem shape, String indent, String exportDir, String mainPath){
511  System.out.println("mainPath = "+ mainPath);
512  System.out.println("url of shape = "+shape.getURL(false));
513  try{
514  // If this model is created from scratch and this shape is primitive, shape.getUrl() == null
515  // If this shape is loaded by Inline node, mainPath != shape.getUrl()
516  // If this shape is directry written in the main file, mainPath == shape.getUrl()
517  if (!shape.isInline_){
518  writer.write(indent+"Transform {\n");
519  if (!valueEquals(shape.getProperty("translation"),"0.0 0.0 0.0 ")){
520  writer.write(indent+" translation "+shape.getProperty("translation")+"\n");
521  }
522  if (!isZero(shape.getProperty("rotation"),3)){
523  writer.write(indent+" rotation "+shape.getProperty("rotation")+"\n");
524  }
525  writer.write(indent+" children[\n");
526  writer.write(indent+" Shape{\n");
527  exportGeometry(writer, shape, indent+" ");
528  if (shape.appearances_[0] != null){
529  writer.write(indent+" appearance Appearance{\n");
530  if (shape.materials_[0] != null){
531  MaterialInfo mat = shape.materials_[0];
532  writer.write(indent+" material Material{\n");
533  writer.write(indent+" diffuseColor "
534  +mat.diffuseColor[0]+" "+mat.diffuseColor[1]+" "+mat.diffuseColor[2]+"\n");
535  writer.write(indent+" }\n");
536  }
537  if(shape.textures_[0] != null){
538  TextureInfo tex = shape.textures_[0];
539  writer.write(indent+" texture ImageTexture {\n");
540  String url = tex.url;
541  url = url.replace('\\','/');
542  writer.write(indent+" url \""+_absPath2relPath(url, exportDir)+"\"\n");
543  writer.write(indent+" }\n");
544  }
545  writer.write(indent+" }\n");
546  }
547  writer.write(indent+" }\n");
548  writer.write(indent+" ]\n");
549  writer.write(indent+"}\n");
550  }else{
551  writer.write(indent+"Transform {\n");
552  if (!valueEquals(shape.getProperty("translation"),"0.0 0.0 0.0 ")){
553  writer.write(indent+" translation "+shape.getProperty("translation")+"\n");
554  }
555  if (!isZero(shape.getProperty("rotation"),3)){
556  writer.write(indent+" rotation "+shape.getProperty("rotation")+"\n");
557  }
558  writer.write(indent+" children[\n");
559  writer.write(indent+"Inline { url \""+_absPath2relPath(shape.getURL(false), exportDir)+"\" }\n");
560  writer.write(indent+" ]\n");
561  writer.write(indent+"}\n");
562  }
563  }catch(Exception ex){
564  ex.printStackTrace();
565  }
566  }
567 
568  public static void exportExtraJoint(BufferedWriter writer, GrxExtraJointItem extraJoint){
569  try{
570  writer.write("DEF "+extraJoint.getName()+" ExtraJoint{\n");
571  writer.write(" link1Name \""+extraJoint.getProperty("link1Name")+"\"\n");
572  writer.write(" link2Name \""+extraJoint.getProperty("link2Name")+"\"\n");
573  writer.write(" link1LocalPos "+extraJoint.getProperty("link1LocalPos")+"\n");
574  writer.write(" link2LocalPos "+extraJoint.getProperty("link2LocalPos")+"\n");
575  writer.write(" jointType \""+extraJoint.getProperty("jointType")+"\"\n");
576  writer.write(" jointAxis "+extraJoint.getProperty("jointAxis")+"\n");
577  writer.write("}\n");
578  }catch(Exception ex){
579  ex.printStackTrace();
580  }
581  }
582 
589  private static String _absPath2relPath(String absPath, String baseDir){
590  Pattern localPattern = Pattern.compile("\\\\");
591  //Matcher localMatcher = localPattern.matcher(File.separator); は使わないで、すべて"/"として扱えるように前処理する
592  Matcher localMatcher = localPattern.matcher("/");
593  String localStr = localMatcher.replaceAll("\\\\\\\\");
594 
595  String [] dirs1 = absPath.split( localStr );
596  String [] dirs2 = baseDir.split( localStr );
597  String relPath = "";
598  int cnt=0;
599  //Linux MacOSの場合とWindowsの場合で処理を分岐する
600  if (System.getProperty("os.name").equals("Linux") || System.getProperty("os.name").equals("Mac OS X")) { //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$ //$NON-NLS-4$
601  while (cnt < dirs1.length && cnt < dirs2.length && dirs1[cnt].equals(dirs2[cnt])) cnt++;
602  for (int i=0; i<dirs2.length-cnt; i++) relPath += "../";
603  for (int i=cnt; i<dirs1.length; i++){
604  relPath += dirs1[i];
605  if (i != dirs1.length-1) relPath += "/";
606  }
607  }else{
608  if(dirs1[0].equals(dirs2[0])) {
609  while (cnt < dirs1.length && cnt < dirs2.length && dirs1[cnt].equals(dirs2[cnt])) cnt++;
610  for (int i=0; i<dirs2.length-cnt; i++) relPath += "../";
611  }
612  for (int i=cnt; i<dirs1.length; i++){
613  relPath += dirs1[i];
614  if (i != dirs1.length-1) relPath += "/";
615  }
616  }
617  return relPath;
618  }
619 
620  public static void exportGeometry(BufferedWriter writer, GrxShapeItem shape, String indent){
621  try{
622  ShapeInfo info = shape.shapes_[0];
623  ShapePrimitiveType ptype = info.primitiveType;
624  float [] pparams = info.primitiveParameters;
625 
626  if (ptype == ShapePrimitiveType.SP_MESH){
627  writer.write(indent+"geometry IndexedFaceSet {\n");
628  writer.write(indent+"ccw TRUE\n");
629  writer.write(indent+"coord Coordinate {\n");
630  writer.write(indent+"point [\n");
631  for(int i=0; i<info.vertices.length;)
632  writer.write(indent+info.vertices[i++]+" "+info.vertices[i++]+" "+info.vertices[i++]+",\n");
633  writer.write(indent+"]\n");
634  writer.write(indent+"}\n");
635  writer.write(indent+"coordIndex [\n");
636  for(int i=0; i<info.triangles.length;)
637  writer.write(indent+info.triangles[i++]+" "+info.triangles[i++]+" "+info.triangles[i++]+" -1,\n");
638  writer.write(indent+"]\n");
639  AppearanceInfo appinfo = shape.appearances_[0];
640  if(appinfo != null){
641  if(appinfo.normals.length != 0){
642  if(!appinfo.normalPerVertex)
643  writer.write(indent+"normalPerVertex FALSE\n");
644  writer.write(indent+"normal Normal {\n");
645  writer.write(indent+"vector [\n");
646  for(int i=0; i<appinfo.normals.length;)
647  writer.write(indent+appinfo.normals[i++]+" "+appinfo.normals[i++]+" "+appinfo.normals[i++]+",\n");
648  writer.write(indent+"]\n");
649  writer.write(indent+"}\n");
650  }
651  if(appinfo.normalIndices.length != 0){
652  writer.write(indent+"normalIndex [\n");
653  if(appinfo.normalPerVertex){
654  for(int i=0; i<appinfo.normalIndices.length;)
655  writer.write(indent+appinfo.normalIndices[i++]+" "+appinfo.normalIndices[i++]+" "+appinfo.normalIndices[i++]+" -1,\n");
656  }else{
657  for(int i=0; i<appinfo.normalIndices.length; i++){
658  if(i%3==0) writer.write(indent+" ");
659  writer.write(appinfo.normalIndices[i]+" ");
660  if(i%3==2) writer.write("\n");
661  }
662  }
663  writer.write(indent+"]\n");
664  }
665  if(!appinfo.solid)
666  writer.write(indent+"solid FALSE\n");
667  if (appinfo.creaseAngle != 0.0f)
668  writer.write(indent+"creaseAngle "+ appinfo.creaseAngle+"\n");
669  if(appinfo.colors.length != 0){
670  if(!appinfo.colorPerVertex)
671  writer.write(indent+"colorPerVertex FALSE\n");
672  writer.write(indent+"color Color {\n");
673  writer.write(indent+"color [\n");
674  for(int i=0; i<appinfo.colors.length;)
675  writer.write(indent+appinfo.colors[i++]+" "+appinfo.colors[i++]+" "+appinfo.colors[i++]+",\n");
676  writer.write(indent+"]\n");
677  writer.write(indent+"}\n");
678  }
679  if(appinfo.colorIndices.length != 0){
680  writer.write(indent+"colorIndex [\n");
681  if(appinfo.colorPerVertex){
682  for(int i=0; i<appinfo.colorIndices.length;)
683  writer.write(indent+appinfo.colorIndices[i++]+" "+appinfo.colorIndices[i++]+" "+appinfo.colorIndices[i++]+" -1,\n");
684  }else{
685  for(int i=0; i<appinfo.colorIndices.length; i++){
686  if(i%3==0) writer.write(indent+" ");
687  writer.write(appinfo.colorIndices[i]+" ");
688  if(i%3==2) writer.write("\n");
689  }
690  }
691  writer.write(indent+"]\n");
692  }
693  TextureInfo texinfo = shape.textures_[0];
694  if(texinfo != null){
695  writer.write(indent+"texCoord TextureCoordinate {\n");
696  writer.write(indent+"point [\n");
697  for(int i=0; i<appinfo.textureCoordinate.length;)
698  writer.write(indent+appinfo.textureCoordinate[i++]+" "+appinfo.textureCoordinate[i++]+",\n");
699  writer.write(indent+"]\n");
700  writer.write(indent+"}\n");
701  writer.write(indent+"texCoordIndex [\n");
702  for(int i=0; i<appinfo.textureCoordIndices.length;)
703  writer.write(indent+appinfo.textureCoordIndices[i++]+" "+appinfo.textureCoordIndices[i++]+" "+appinfo.textureCoordIndices[i++]+" -1,\n");
704  writer.write(indent+"]\n");
705  }
706  }
707  writer.write(indent+"}\n");
708  }else if (ptype == ShapePrimitiveType.SP_BOX){
709  writer.write(indent+"geometry Box{\n");
710  writer.write(indent+" size "
711  +pparams[0]+" "
712  +pparams[1]+" "
713  +pparams[2]+"\n");
714  writer.write(indent+"}\n");
715  }else if (ptype == ShapePrimitiveType.SP_CYLINDER){
716  writer.write(indent+"geometry Cylinder{\n");
717  writer.write(indent+" radius "+pparams[0]+"\n");
718  writer.write(indent+" height "+pparams[1]+"\n");
719  if (pparams[2]==0){
720  writer.write(indent+" top FALSE\n");
721  }
722  if (pparams[3]==0){
723  writer.write(indent+" bottom FALSE\n");
724  }
725  if (pparams[4]==0){
726  writer.write(indent+" side FALSE\n");
727  }
728  writer.write(indent+"}\n");
729  }else if (ptype == ShapePrimitiveType.SP_CONE){
730  writer.write(indent+"geometry Cone{\n");
731  writer.write(indent+" bottomRadius "+pparams[0]+"\n");
732  writer.write(indent+" height "+pparams[1]+"\n");
733  if (pparams[2]==0){
734  writer.write(indent+" bottom FALSE\n");
735  }
736  if (pparams[3]==0){
737  writer.write(indent+" side FALSE\n");
738  }
739  writer.write(indent+"}\n");
740  }else if (ptype == ShapePrimitiveType.SP_SPHERE){
741  writer.write(indent+"geometry Sphere{\n");
742  writer.write(indent+" radius "+pparams[0]+"\n");
743  writer.write(indent+"}\n");
744  }else if (ptype == ShapePrimitiveType.SP_PLANE){
745  writer.write(indent+"geometry Plane{\n");
746  writer.write(indent+" size "+pparams[0]+" "+pparams[1]+" "+pparams[2]+"\n");
747  writer.write(indent+"}\n");
748  }
749  }catch(Exception ex){
750  ex.printStackTrace();
751  }
752  }
753 
754  private static boolean valueEquals(String s1, String s2){
755  final double EPS = 0.00000001;
756  String[] ss1 = s1.split(" ");
757  String[] ss2 = s2.split(" ");
758  int n = ss1.length;
759  if(n!=ss2.length)
760  return false;
761  for(int i=0; i<n; i++){
762  try{
763  if( Math.abs(Double.parseDouble(ss1[i]) - Double.parseDouble(ss2[i])) > EPS )
764  return false;
765  }catch(Exception ex){
766  return false;
767  }
768  }
769  return true;
770  }
771 
772  private static boolean isZero(String s1, int i){
773  String[] ss1 = s1.split(" ");
774  if(i < ss1.length ){
775  try{
776  if( Math.abs(Double.parseDouble(ss1[i])) < 1e-10 )
777  return true;
778  else
779  return false;
780  }catch(Exception ex){
781  return false;
782  }
783  }else
784  return false;
785  }
786 }
com.generalrobotix.ui.item.GrxShapeItem.appearances_
AppearanceInfo[] appearances_
Definition: GrxShapeItem.java:86
i
png_uint_32 i
Definition: png.h:2732
com.generalrobotix.ui.item.GrxShapeItem
sensor item
Definition: GrxShapeItem.java:83
com.generalrobotix.ui.item.GrxSegmentItem
Definition: GrxSegmentItem.java:19
com.generalrobotix.ui.util.GrxVrmlExporter.isZero
static boolean isZero(String s1, int i)
Definition: GrxVrmlExporter.java:772
com.generalrobotix.ui.util.GrxVrmlExporter.exportSensor
static void exportSensor(BufferedWriter writer, GrxSensorItem sensor, String indent, String exportDir, String mainPath)
Definition: GrxVrmlExporter.java:402
com.generalrobotix.ui.util.GrxVrmlExporter.exportShape
static void exportShape(BufferedWriter writer, GrxShapeItem shape, String indent, String exportDir, String mainPath)
export shape item
Definition: GrxVrmlExporter.java:510
com.generalrobotix.ui.util.GrxVrmlExporter.valueEquals
static boolean valueEquals(String s1, String s2)
Definition: GrxVrmlExporter.java:754
com.generalrobotix.ui.item.GrxLinkItem.gearRatio_
double gearRatio_
Definition: GrxLinkItem.java:84
com.generalrobotix.ui.GrxBasePlugin.equals
boolean equals(Object obj)
check whether obj equals to this
Definition: GrxBasePlugin.java:357
com.generalrobotix.ui.item.GrxShapeItem.shapes_
ShapeInfo[] shapes_
Definition: GrxShapeItem.java:85
com.generalrobotix.ui.item.GrxExtraJointItem
item which have a transformation
Definition: GrxExtraJointItem.java:38
com.generalrobotix.ui.util.GrxVrmlExporter
Definition: GrxVrmlExporter.java:22
com.generalrobotix.ui.item.GrxShapeItem.isInline_
boolean isInline_
Definition: GrxShapeItem.java:84
com.generalrobotix.ui.item.GrxLinkItem.rotorResistor_
double rotorResistor_
Definition: GrxLinkItem.java:83
com.generalrobotix.ui.util.GrxVrmlExporter._absPath2relPath
static String _absPath2relPath(String absPath, String baseDir)
convert absolute path into relative path
Definition: GrxVrmlExporter.java:589
jp.go
autoplay.n
n
Definition: autoplay.py:12
jp
jp.go.aist
com.generalrobotix.ui.util.GrxVrmlExporter.export
static boolean export(GrxModelItem model, String exportPath)
export model item to a VRML97 file
Definition: GrxVrmlExporter.java:29
com.generalrobotix.ui.item.GrxLinkItem.torqueConst_
double torqueConst_
Definition: GrxLinkItem.java:85
com.generalrobotix.ui.item.GrxSensorItem.type_
String type_
Definition: GrxSensorItem.java:42
hrp::max
static int max(int a, int b)
Definition: MatrixSolvers.cpp:54
com.generalrobotix.ui.util.GrxVrmlExporter.exportExtraJoint
static void exportExtraJoint(BufferedWriter writer, GrxExtraJointItem extraJoint)
Definition: GrxVrmlExporter.java:568
com.generalrobotix.ui.item
Definition: GrxCollisionPairItem.java:19
EPS
#define EPS
com.generalrobotix.ui.item.GrxSensorItem.id_
int id_
Definition: GrxSensorItem.java:43
com.generalrobotix.ui.item.GrxSensorItem
sensor
Definition: GrxSensorItem.java:41
s2
png_voidp png_voidp s2
Definition: png.h:2107
jp.go.aist.hrp
com.generalrobotix.ui.item.GrxLinkItem.rotorInertia_
double rotorInertia_
Definition: GrxLinkItem.java:82
s1
png_voidp s1
Definition: png.h:2107
com.generalrobotix.ui.GrxBasePlugin.getName
final String getName()
get name
Definition: GrxBasePlugin.java:199
com.generalrobotix.ui.util.GrxVrmlExporter.exportGeometry
static void exportGeometry(BufferedWriter writer, GrxShapeItem shape, String indent)
Definition: GrxVrmlExporter.java:620
com.generalrobotix.ui.item.GrxLinkItem
Definition: GrxLinkItem.java:68
com.generalrobotix.ui.item.GrxTransformItem.children_
Vector< GrxTransformItem > children_
Definition: GrxTransformItem.java:58
com.generalrobotix.ui.item.GrxShapeItem.textures_
TextureInfo[] textures_
Definition: GrxShapeItem.java:88
com.generalrobotix
com.generalrobotix.ui.item.GrxLinkItem.jointId_
short jointId_
Definition: GrxLinkItem.java:71
com.generalrobotix.ui.item.GrxModelItem.rootLink
GrxLinkItem rootLink()
get root link
Definition: GrxModelItem.java:375
com.generalrobotix.ui.GrxBasePlugin.getURL
String getURL(boolean expand)
Definition: GrxBasePlugin.java:367
com
com.generalrobotix.ui.item.GrxModelItem
item corresponds to a robot model
Definition: GrxModelItem.java:52
com.generalrobotix.ui.item.GrxLinkItem.encoderPulse_
double encoderPulse_
Definition: GrxLinkItem.java:86
com.generalrobotix.ui.item.GrxShapeItem.materials_
MaterialInfo[] materials_
Definition: GrxShapeItem.java:87
jp.go.aist.hrp.simulator
Definition: PathConsumer.java:8
info
backing_store_ptr info
Definition: jmemsys.h:181
com.generalrobotix.ui.util.GrxVrmlExporter.exportSegment
static void exportSegment(BufferedWriter writer, GrxSegmentItem segment, String indent, String exportDir, String mainPath)
Definition: GrxVrmlExporter.java:359
com.generalrobotix.ui
com.generalrobotix.ui.util.GrxVrmlExporter.exportLink
static Vector< GrxLinkItem > exportLink(BufferedWriter writer, GrxLinkItem link, String indent, String exportDir, String mainPath)
export link information
Definition: GrxVrmlExporter.java:307
com.generalrobotix.ui.item.GrxLinkItem.jointType_
String jointType_
Definition: GrxLinkItem.java:72
com.generalrobotix.ui.item.GrxModelItem.extraJoints_
Vector< GrxExtraJointItem > extraJoints_
Definition: GrxModelItem.java:69

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:03