ColladaWriter.h
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2011 University of Tokyo, General Robotix Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
22 #ifndef OPENHRP_COLLADA_WRITER_H
23 #define OPENHRP_COLLADA_WRITER_H
24 
25 #include "ColladaUtil.h"
26 #include <hrpUtil/MatrixSolvers.h>
27 
28 using namespace std;
29 using namespace ColladaUtil;
30 
31 #include <hrpCorba/ViewSimulator.hh>
32 
33 //#include <zip.h> // for saving compressed files
34 //#ifdef _WIN32
35 //#include <iowin32.h>
36 //#else
37 //#include <unistd.h>
38 //#endif
39 
41 {
43  rotation[0] = rotation[1] = rotation[2] = rotation[3] = 0;
44  translation[0] = translation[1] = translation[2] = 0;
45  }
46  std::string name;
47  std::string basename, effectorname;
48  std::list<std::string> grippernames;
49  std::list<std::string> gripperdir;
50  // manipulator coordinate system
51  DblArray4 rotation;
52  DblArray3 translation;
53 };
54 
55 class ColladaWriter : public daeErrorHandler
56 {
57  static bool ComparePair(const std::pair<int,int>& p0,const std::pair<int,int>& p1)
58  {
59  return p0.second < p1.second;
60  }
61 
62 public:
63  struct SCENE
64  {
65  domVisual_sceneRef vscene;
66  domKinematics_sceneRef kscene;
67  domPhysics_sceneRef pscene;
68  domInstance_with_extraRef viscene;
69  domInstance_kinematics_sceneRef kiscene;
70  domInstance_with_extraRef piscene;
71  };
72 
73  struct LINKOUTPUT
74  {
75  list<pair<int,std::string> > listusedlinks;
76  daeElementRef plink;
77  domNodeRef pnode;
78  };
79 
81  {
82  domPhysics_modelRef pmodel;
83  std::vector<std::string > vrigidbodysids;
84  };
85 
87  {
88  struct axis_output
89  {
90  //axis_output(const string& sid, KinBody::JointConstPtr pjoint, int iaxis) : sid(sid), pjoint(pjoint), iaxis(iaxis) {}
91  axis_output() : iaxis(0) {
92  }
93  string sid; // joint/axis
94  string nodesid;
95  int ijoint;
96  int iaxis;
97  string jointnodesid;
98  };
99  domKinematics_modelRef kmodel;
100  std::vector<axis_output> vaxissids;
101  std::vector<std::string > vlinksids;
102  std::map<std::string, int> maplinknames, mapjointnames;
103  };
104 
105  struct axis_sids
106  {
107  axis_sids(const string& axissid, const string& valuesid, const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {
108  }
109  string axissid, valuesid, jointnodesid;
110  };
111 
113  {
114  domInstance_kinematics_modelRef ikm;
115  std::vector<axis_sids> vaxissids;
116  boost::shared_ptr<kinematics_model_output> kmout;
117  std::vector<std::pair<std::string,std::string> > vkinematicsbindings; // node and kinematics model bindings
118  };
119 
121  {
122  domInstance_articulated_systemRef ias;
123  std::vector<axis_sids> vaxissids;
124  //std::vector<std::string > vlinksids;
125  //std::map<std::string, int> maplinknames;
126  std::vector<std::pair<std::string,std::string> > vkinematicsbindings;
127  };
128 
130  {
131  domInstance_physics_modelRef ipm;
132  boost::shared_ptr<physics_model_output> pmout;
133  };
134 
136  {
137  std::string xmlfilename, kinematicsgeometryhash;
138  boost::shared_ptr<kinematics_model_output> kmout;
139  boost::shared_ptr<physics_model_output> pmout;
140  };
141 
142  ColladaWriter(const std::list<ManipulatorInfo>& listmanipulators, const char* comment_str) : _dom(NULL) {
143  _listmanipulators = listmanipulators;
144  daeErrorHandler::setErrorHandler(this);
145  COLLADALOG_INFO(str(boost::format("init COLLADA writer version: %s, namespace: %s")%COLLADA_VERSION%COLLADA_NAMESPACE));
146  _collada.reset(new DAE);
147  _collada->setIOPlugin( NULL );
148  _collada->setDatabase( NULL );
149 
150  const char* documentName = "openrave_snapshot";
151  daeInt error = _collada->getDatabase()->insertDocument(documentName, &_doc ); // also creates a collada root
152  BOOST_ASSERT( error == DAE_OK && !!_doc );
153  _dom = daeSafeCast<domCOLLADA>(_doc->getDomRoot());
154 
155  //create the required asset tag
156  domAssetRef asset = daeSafeCast<domAsset>( _dom->add( COLLADA_ELEMENT_ASSET ) );
157  {
158  // facet becomes owned by locale, so no need to explicitly delete
159  boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%dT%H:%M:%s");
160  std::stringstream ss;
161  ss.imbue(std::locale(ss.getloc(), facet));
162  ss << boost::posix_time::second_clock::local_time();
163 
164  domAsset::domCreatedRef created = daeSafeCast<domAsset::domCreated>( asset->add( COLLADA_ELEMENT_CREATED ) );
165  created->setValue(ss.str().c_str());
166  domAsset::domModifiedRef modified = daeSafeCast<domAsset::domModified>( asset->add( COLLADA_ELEMENT_MODIFIED ) );
167  modified->setValue(ss.str().c_str());
168 
169  domAsset::domContributorRef contrib = daeSafeCast<domAsset::domContributor>( asset->add( COLLADA_TYPE_CONTRIBUTOR ) );
170  domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast<domAsset::domContributor::domAuthoring_tool>( contrib->add( COLLADA_ELEMENT_AUTHORING_TOOL ) );
171  authoringtool->setValue("OpenHRP3 Collada Writer");
172  domAsset::domContributor::domCommentsRef comments = daeSafeCast<domAsset::domContributor::domComments>( contrib->add( COLLADA_ELEMENT_COMMENTS ) );
173  comments->setValue(comment_str);
174 
175 
176  domAsset::domUnitRef units = daeSafeCast<domAsset::domUnit>( asset->add( COLLADA_ELEMENT_UNIT ) );
177  units->setMeter(1);
178  units->setName("meter");
179 
180  domAsset::domUp_axisRef zup = daeSafeCast<domAsset::domUp_axis>( asset->add( COLLADA_ELEMENT_UP_AXIS ) );
181  zup->setValue(UP_AXIS_Z_UP);
182  }
183 
184  _globalscene = _dom->getScene();
185  if( !_globalscene ) {
186  _globalscene = daeSafeCast<domCOLLADA::domScene>( _dom->add( COLLADA_ELEMENT_SCENE ) );
187  }
188 
189  _visualScenesLib = daeSafeCast<domLibrary_visual_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
190  _visualScenesLib->setId("vscenes");
191  _geometriesLib = daeSafeCast<domLibrary_geometries>(_dom->add(COLLADA_ELEMENT_LIBRARY_GEOMETRIES));
192  _geometriesLib->setId("geometries");
193  _effectsLib = daeSafeCast<domLibrary_effects>(_dom->add(COLLADA_ELEMENT_LIBRARY_EFFECTS));
194  _effectsLib->setId("effects");
195  _imagesLib = daeSafeCast<domLibrary_images>(_dom->add(COLLADA_ELEMENT_LIBRARY_IMAGES));
196  _materialsLib = daeSafeCast<domLibrary_materials>(_dom->add(COLLADA_ELEMENT_LIBRARY_MATERIALS));
197  _materialsLib->setId("materials");
198  _kinematicsModelsLib = daeSafeCast<domLibrary_kinematics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
199  _kinematicsModelsLib->setId("kmodels");
200  _articulatedSystemsLib = daeSafeCast<domLibrary_articulated_systems>(_dom->add(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS));
201  _articulatedSystemsLib->setId("asystems");
202  _kinematicsScenesLib = daeSafeCast<domLibrary_kinematics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
203  _kinematicsScenesLib->setId("kscenes");
204  _physicsScenesLib = daeSafeCast<domLibrary_physics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
205  _physicsScenesLib->setId("pscenes");
206  _physicsModelsLib = daeSafeCast<domLibrary_physics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_MODELS));
207  _physicsModelsLib->setId("pmodels");
208  domExtraRef pextra_library_sensors = daeSafeCast<domExtra>(_dom->add(COLLADA_ELEMENT_EXTRA));
209  pextra_library_sensors->setId("sensors");
210  pextra_library_sensors->setType("library_sensors");
211  _sensorsLib = daeSafeCast<domTechnique>(pextra_library_sensors->add(COLLADA_ELEMENT_TECHNIQUE));
212  _sensorsLib->setProfile("OpenRAVE");
213  _nextsensorid = 0;
214  domExtraRef pextra_library_actuators = daeSafeCast<domExtra>(_dom->add(COLLADA_ELEMENT_EXTRA));
215  pextra_library_actuators->setId("actuators");
216  pextra_library_actuators->setType("library_actuators");
217  _actuatorsLib = daeSafeCast<domTechnique>(pextra_library_actuators->add(COLLADA_ELEMENT_TECHNIQUE));
218  _actuatorsLib->setProfile("OpenRAVE");
219  _nextactuatorid = 0;
220  }
221  virtual ~ColladaWriter() {
222  _collada.reset();
223  DAE::cleanup();
224  }
225 
226 
228  virtual void Save(const string& filename)
229  {
230  bool bcompress = filename.size() >= 4 && filename[filename.size()-4] == '.' && ::tolower(filename[filename.size()-3]) == 'z' && ::tolower(filename[filename.size()-2]) == 'a' && ::tolower(filename[filename.size()-1]) == 'e';
231  if( !bcompress ) {
232  if(!_collada->writeTo(_doc->getDocumentURI()->getURI(), filename.c_str()) ) {
233  throw ModelLoader::ModelLoaderException(str(boost::format("failed to save collada file to %s")%filename).c_str());
234  }
235  return;
236  }
237  COLLADALOG_WARN("cannot save as compressed collada file\n");
238  }
239 
240  virtual bool Write(BodyInfo_impl* bodyInfo) { //, ShapeSetInfo_impl* shapeSetInfo) {
241  _CreateScene();
242  boost::shared_ptr<instance_articulated_system_output> iasout = _WriteRobot(bodyInfo);
243  if( !iasout ) {
244  return false;
245  }
246  _WriteBindingsInstance_kinematics_scene(_scene.kiscene,bodyInfo,iasout->vaxissids,iasout->vkinematicsbindings);
247  return true;
248  }
249 
251  virtual boost::shared_ptr<instance_articulated_system_output> _WriteRobot(BodyInfo_impl* bodyInfo)
252  {
253  COLLADALOG_VERBOSE(str(boost::format("writing robot as instance_articulated_system (%d) %s\n")%_GetRobotId(bodyInfo)%bodyInfo->name()));
254  string asid = str(boost::format("robot%d")%_GetRobotId(bodyInfo));
255  string askid = str(boost::format("%s_kinematics")%asid);
256  string asmid = str(boost::format("%s_motion")%asid);
257  string iassid = str(boost::format("%s_inst")%asmid);
258 
259  domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(_scene.kscene->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
260  ias->setSid(iassid.c_str());
261  ias->setUrl((string("#")+asmid).c_str());
262  ias->setName(bodyInfo->name());
263 
264  boost::shared_ptr<instance_articulated_system_output> iasout(new instance_articulated_system_output());
265  iasout->ias = ias;
266 
267  // motion info
268  domArticulated_systemRef articulated_system_motion = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
269  articulated_system_motion->setId(asmid.c_str());
270  domMotionRef motion = daeSafeCast<domMotion>(articulated_system_motion->add(COLLADA_ELEMENT_MOTION));
271  domMotion_techniqueRef mt = daeSafeCast<domMotion_technique>(motion->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
272  domInstance_articulated_systemRef ias_motion = daeSafeCast<domInstance_articulated_system>(motion->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
273  ias_motion->setUrl(str(boost::format("#%s")%askid).c_str());
274 
275  // kinematics info
276  domArticulated_systemRef articulated_system_kinematics = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
277  articulated_system_kinematics->setId(askid.c_str());
278  domKinematicsRef kinematics = daeSafeCast<domKinematics>(articulated_system_kinematics->add(COLLADA_ELEMENT_KINEMATICS));
279  domKinematics_techniqueRef kt = daeSafeCast<domKinematics_technique>(kinematics->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
280 
281  boost::shared_ptr<instance_kinematics_model_output> ikmout = _WriteInstance_kinematics_model(bodyInfo,kinematics,askid);
282  LinkInfoSequence_var links = bodyInfo->links();
283  for(size_t idof = 0; idof < ikmout->vaxissids.size(); ++idof) {
284  string axis_infosid = str(boost::format("axis_info_inst%d")%idof);
285  LinkInfo& pjoint = links[ikmout->kmout->vaxissids.at(idof).ijoint];
286  string jointType(CORBA::String_var(pjoint.jointType));
287 
288  // Kinematics axis info
289  domKinematics_axis_infoRef kai = daeSafeCast<domKinematics_axis_info>(kt->add(COLLADA_ELEMENT_AXIS_INFO));
290  kai->setAxis(str(boost::format("%s/%s")%ikmout->kmout->kmodel->getID()%ikmout->kmout->vaxissids.at(idof).sid).c_str());
291  kai->setSid(axis_infosid.c_str());
292  domCommon_bool_or_paramRef active = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_ACTIVE));
293  daeSafeCast<domCommon_bool_or_param::domBool>(active->add(COLLADA_ELEMENT_BOOL))->setValue(jointType != "fixed");
294  domCommon_bool_or_paramRef locked = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_LOCKED));
295  daeSafeCast<domCommon_bool_or_param::domBool>(locked->add(COLLADA_ELEMENT_BOOL))->setValue(false);
296 
297  dReal fmult = jointType == "rotate" ? (180.0/M_PI) : 1.0;
298  vector<dReal> vmin, vmax;
299  if( jointType == "fixed" ) {
300  vmin.push_back(0);
301  vmax.push_back(0);
302  }
303  else {
304  if( pjoint.llimit.length() > 0 ) {
305  vmin.push_back(fmult*pjoint.llimit[0]);
306  }
307  if( pjoint.ulimit.length() > 0 ) {
308  vmax.push_back(fmult*pjoint.ulimit[0]);
309  }
310  }
311 
312  if( vmin.size() > 0 || vmax.size() > 0 ) {
313  domKinematics_limitsRef plimits = daeSafeCast<domKinematics_limits>(kai->add(COLLADA_ELEMENT_LIMITS));
314  if( vmin.size() > 0 ) {
315  daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MIN)->add(COLLADA_ELEMENT_FLOAT))->setValue(vmin[0]);
316  }
317  if( vmax.size() > 0 ) {
318  daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MAX)->add(COLLADA_ELEMENT_FLOAT))->setValue(vmax[0]);
319  }
320  }
321 
322  // Motion axis info
323  domMotion_axis_infoRef mai = daeSafeCast<domMotion_axis_info>(mt->add(COLLADA_ELEMENT_AXIS_INFO));
324  mai->setAxis(str(boost::format("%s/%s")%askid%axis_infosid).c_str());
325  if( pjoint.uvlimit.length() > 0 ) {
326  domCommon_float_or_paramRef speed = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_SPEED));
327  daeSafeCast<domCommon_float_or_param::domFloat>(speed->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint.uvlimit[0]);
328  }
329  if( pjoint.climit.length() > 0 ) {
330  domCommon_float_or_paramRef accel = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_ACCELERATION));
331  daeSafeCast<domCommon_float_or_param::domFloat>(accel->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint.climit[0] * pjoint.torqueConst * pjoint.gearRatio);
332  }
333  }
334 
335  // write the bindings
336  string asmsym = str(boost::format("%s_%s")%asmid%ikmout->ikm->getSid());
337  string assym = str(boost::format("%s_%s")%_scene.kscene->getID()%ikmout->ikm->getSid());
338  for(std::vector<std::pair<std::string,std::string> >::iterator it = ikmout->vkinematicsbindings.begin(); it != ikmout->vkinematicsbindings.end(); ++it) {
339  domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
340  abm->setSid(asmsym.c_str());
341  daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%it->first).c_str());
342  domKinematics_newparamRef ab = daeSafeCast<domKinematics_newparam>(ias->add(COLLADA_ELEMENT_NEWPARAM));
343  ab->setSid(assym.c_str());
344  daeSafeCast<domKinematics_newparam::domSIDREF>(ab->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%asmid%asmsym).c_str());
345  iasout->vkinematicsbindings.push_back(make_pair(string(ab->getSid()), it->second));
346  }
347  for(size_t idof = 0; idof < ikmout->vaxissids.size(); ++idof) {
348  const axis_sids& kas = ikmout->vaxissids.at(idof);
349  domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
350  abm->setSid(str(boost::format("%s_%s")%asmid%kas.axissid).c_str());
351  daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.axissid).c_str());
352  domKinematics_newparamRef ab = daeSafeCast<domKinematics_newparam>(ias->add(COLLADA_ELEMENT_NEWPARAM));
353  ab->setSid(str(boost::format("%s_%s")%assym%kas.axissid).c_str());
354  daeSafeCast<domKinematics_newparam::domSIDREF>(ab->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s_%s")%asmid%asmid%kas.axissid).c_str());
355  string valuesid;
356  if( kas.valuesid.size() > 0 ) {
357  domKinematics_newparamRef abmvalue = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
358  abmvalue->setSid(str(boost::format("%s_%s")%asmid%kas.valuesid).c_str());
359  daeSafeCast<domKinematics_newparam::domSIDREF>(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.valuesid).c_str());
360  domKinematics_newparamRef abvalue = daeSafeCast<domKinematics_newparam>(ias->add(COLLADA_ELEMENT_NEWPARAM));
361  valuesid = str(boost::format("%s_%s")%assym%kas.valuesid);
362  abvalue->setSid(valuesid.c_str());
363  daeSafeCast<domKinematics_newparam::domSIDREF>(abvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s_%s")%asmid%asmid%kas.valuesid).c_str());
364  }
365  iasout->vaxissids.push_back(axis_sids(string(ab->getSid()),valuesid,kas.jointnodesid));
366  }
367 
368  boost::shared_ptr<kinematics_model_output> kmout = _GetKinematics_model(bodyInfo);
369  string kmodelid = kmout->kmodel->getID(); kmodelid += "/";
370 
371  // write manipulators
372  for(std::list<ManipulatorInfo>::const_iterator itmanip = _listmanipulators.begin(); itmanip != _listmanipulators.end(); ++itmanip) {
373  if (kmout->maplinknames.find(itmanip->basename) == kmout->maplinknames.end()){
374  std::cerr << "can't find a link named " << itmanip->basename << ", manipulator " << itmanip->name << " is not defined" << std::endl;
375  continue;
376  }
377  if (kmout->maplinknames.find(itmanip->effectorname) == kmout->maplinknames.end()){
378  std::cerr << "can't find a link named " << itmanip->effectorname << ", manipulator " << itmanip->name << " is not defined" << std::endl;
379  continue;
380  }
381  domExtraRef pextra = daeSafeCast<domExtra>(articulated_system_motion->add(COLLADA_ELEMENT_EXTRA));
382  pextra->setName(itmanip->name.c_str());
383  pextra->setType("manipulator");
384  domTechniqueRef ptec = daeSafeCast<domTechnique>(pextra->add(COLLADA_ELEMENT_TECHNIQUE));
385  ptec->setProfile("OpenRAVE");
386  daeElementRef frame_origin = ptec->add("frame_origin");
387  frame_origin->setAttribute("link",(kmodelid+_GetLinkSid(kmout->maplinknames[itmanip->basename])).c_str());
388  daeElementRef frame_tip = ptec->add("frame_tip");
389  frame_tip->setAttribute("link",(kmodelid+_GetLinkSid(kmout->maplinknames[itmanip->effectorname])).c_str());
390  _WriteTransformation(frame_tip,itmanip->rotation, itmanip->translation);
391  BOOST_ASSERT(itmanip->gripperdir.size() == itmanip->grippernames.size());
392  std::list<std::string>::const_iterator itgripperdir = itmanip->gripperdir.begin();
393  std::list<std::string>::const_iterator itgrippername = itmanip->grippernames.begin();
394  while(itgrippername != itmanip->grippernames.end() ) {
395  daeElementRef gripper_joint = ptec->add("gripper_joint");
396  gripper_joint->setAttribute("joint", str(boost::format("%sjointsid%d")%kmodelid%kmout->mapjointnames[*itgrippername]).c_str());
397  daeElementRef closing_direction = gripper_joint->add("closing_direction");
398  closing_direction->setAttribute("axis","./axis0");
399  closing_direction->add("float")->setCharData(*itgripperdir); // might be -1.0
400  ++itgrippername;
401  ++itgripperdir;
402  }
403  }
404 
405  boost::shared_ptr<instance_physics_model_output> ipmout = _WriteInstance_physics_model(bodyInfo,_scene.pscene,_scene.pscene->getID());
406 
407  // interface type
408  // {
409  // domExtraRef pextra = daeSafeCast<domExtra>(articulated_system_motion->add(COLLADA_ELEMENT_EXTRA));
410  // pextra->setType("interface_type");
411  // domTechniqueRef ptec = daeSafeCast<domTechnique>(pextra->add(COLLADA_ELEMENT_TECHNIQUE));
412  // ptec->setProfile("OpenRAVE");
413  // ptec->add("interface")->setCharData(probot->GetXMLId());
414  // }
415 
416  // sensors
417  for(size_t ilink = 0; ilink < links->length(); ++ilink) {
418  LinkInfo& linkInfo = links[ilink];
419  for(size_t isensor = 0; isensor < linkInfo.sensors.length(); ++isensor) {
420  SensorInfo& sensor = linkInfo.sensors[isensor];
421  daeElementRef domsensor = WriteSensor(sensor, bodyInfo->name());
422 
423  domExtraRef extra_attach_sensor = daeSafeCast<domExtra>(articulated_system_motion->add(COLLADA_ELEMENT_EXTRA));
424  extra_attach_sensor->setName(sensor.name);
425  extra_attach_sensor->setType("attach_sensor");
426  domTechniqueRef attach_sensor = daeSafeCast<domTechnique>(extra_attach_sensor->add(COLLADA_ELEMENT_TECHNIQUE));
427  attach_sensor->setProfile("OpenRAVE");
428 
429  string strurl = str(boost::format("#%s")%domsensor->getID());
430  daeElementRef isensor0 = attach_sensor->add("instance_sensor");
431  isensor0->setAttribute("url",strurl.c_str());
432 
433  daeElementRef frame_origin = attach_sensor->add("frame_origin");
434  frame_origin->setAttribute("link",(kmodelid+_GetLinkSid(kmout->maplinknames[string(linkInfo.segments[0].name)])).c_str());
435  if( string(domsensor->getAttribute("type")).find("camera") != string::npos ) {
436  // rotate camera coord system by 180 on x-axis since camera direction is toward -z
437  DblArray4 rotation; rotation[0] = 1; rotation[1] = 0; rotation[2] = 0; rotation[3] = M_PI;
438  _SetRotate(daeSafeCast<domRotate>(frame_origin->add(COLLADA_ELEMENT_ROTATE,0)), rotation);
439  }
440  _WriteTransformation(frame_origin,sensor.rotation, sensor.translation);
441  }
442  }
443 
444  // actuators, create in the order specified by jointId!
445  std::vector< pair<int, int> > vjointids(links->length());
446  for(size_t ilink = 0; ilink < links->length(); ++ilink) {
447  vjointids[ilink].first = ilink;
448  vjointids[ilink].second = links[ilink].jointId;
449  }
450  sort(vjointids.begin(),vjointids.end(),ComparePair);
451  for(size_t ipair = 0; ipair < vjointids.size(); ++ipair) {
452  size_t ilink = vjointids[ipair].first;
453  LinkInfo& linkInfo = links[ilink];
454  daeElementRef domactuator = WriteActuator(linkInfo, bodyInfo->name());
455 
456  domExtraRef extra_attach_actuator = daeSafeCast<domExtra>(articulated_system_motion->add(COLLADA_ELEMENT_EXTRA));
457  extra_attach_actuator->setName(linkInfo.name);
458  extra_attach_actuator->setType("attach_actuator");
459  domTechniqueRef attach_actuator = daeSafeCast<domTechnique>(extra_attach_actuator->add(COLLADA_ELEMENT_TECHNIQUE));
460  attach_actuator->setProfile("OpenRAVE");
461 
462  string strurl = str(boost::format("#%s")%domactuator->getID());
463  daeElementRef iactuator = attach_actuator->add("instance_actuator");
464  iactuator->setAttribute("url",strurl.c_str());
465  attach_actuator->add("bind_actuator")->setAttribute("joint",str(boost::format("%sjointsid%d")%kmodelid%kmout->mapjointnames[string(linkInfo.name)]).c_str());
466  }
467 
468  return iasout;
469  }
470 
472  virtual boost::shared_ptr<instance_kinematics_model_output> _WriteInstance_kinematics_model(BodyInfo_impl* bodyInfo, daeElementRef parent, const string& sidscope)
473  {
474  COLLADALOG_VERBOSE(str(boost::format("writing instance_kinematics_model (%d) %s\n")%_GetRobotId(bodyInfo)%bodyInfo->name()));
475  boost::shared_ptr<kinematics_model_output> kmout = WriteKinematics_model(bodyInfo);
476 
477  boost::shared_ptr<instance_kinematics_model_output> ikmout(new instance_kinematics_model_output());
478  ikmout->kmout = kmout;
479  ikmout->ikm = daeSafeCast<domInstance_kinematics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
480 
481  string symscope, refscope;
482  if( sidscope.size() > 0 ) {
483  symscope = sidscope+string("_");
484  refscope = sidscope+string("/");
485  }
486  string ikmsid = str(boost::format("%s_inst")%kmout->kmodel->getID());
487  ikmout->ikm->setUrl(str(boost::format("#%s")%kmout->kmodel->getID()).c_str());
488  ikmout->ikm->setSid(ikmsid.c_str());
489 
490  domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
491  kbind->setSid((symscope+ikmsid).c_str());
492  daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid).c_str());
493  ikmout->vkinematicsbindings.push_back(make_pair(string(kbind->getSid()), str(boost::format("visual%d/node0")%_GetRobotId(bodyInfo))));
494  LinkInfoSequence_var links = bodyInfo->links();
495  ikmout->vaxissids.reserve(kmout->vaxissids.size());
496  int i = 0;
497  for(std::vector<kinematics_model_output::axis_output>::iterator it = kmout->vaxissids.begin(); it != kmout->vaxissids.end(); ++it) {
498  domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
499  string ref = it->sid;
500  size_t index = ref.find("/");
501  while(index != string::npos) {
502  ref[index] = '.';
503  index = ref.find("/",index+1);
504  }
505  string sid = symscope+ikmsid+"_"+ref;
506  kbind->setSid(sid.c_str());
507  daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid+"/"+it->sid).c_str());
508  domKinematics_newparamRef pvalueparam = daeSafeCast<domKinematics_newparam>(ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
509  pvalueparam->setSid((sid+string("_value")).c_str());
510  daeSafeCast<domKinematics_newparam::domFloat>(pvalueparam->add(COLLADA_ELEMENT_FLOAT))->setValue(links[it->ijoint].jointValue);
511  ikmout->vaxissids.push_back(axis_sids(sid,pvalueparam->getSid(),kmout->vaxissids.at(i).jointnodesid));
512  ++i;
513  }
514 
515  return ikmout;
516  }
517 
518  virtual boost::shared_ptr<instance_physics_model_output> _WriteInstance_physics_model(BodyInfo_impl* bodyInfo, daeElementRef parent, const string& sidscope)
519  {
520  boost::shared_ptr<physics_model_output> pmout = WritePhysics_model(bodyInfo);
521  boost::shared_ptr<instance_physics_model_output> ipmout(new instance_physics_model_output());
522  ipmout->pmout = pmout;
523  ipmout->ipm = daeSafeCast<domInstance_physics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_PHYSICS_MODEL));
524  ipmout->ipm->setParent(xsAnyURI(*ipmout->ipm,string("#")+_GetNodeId(bodyInfo)));
525  string symscope, refscope;
526  if( sidscope.size() > 0 ) {
527  symscope = sidscope+string("_");
528  refscope = sidscope+string("/");
529  }
530  string ipmsid = str(boost::format("%s_inst")%pmout->pmodel->getID());
531  ipmout->ipm->setUrl(str(boost::format("#%s")%pmout->pmodel->getID()).c_str());
532  ipmout->ipm->setSid(ipmsid.c_str());
533  for(size_t i = 0; i < pmout->vrigidbodysids.size(); ++i) {
534  domInstance_rigid_bodyRef pirb = daeSafeCast<domInstance_rigid_body>(ipmout->ipm->add(COLLADA_ELEMENT_INSTANCE_RIGID_BODY));
535  pirb->setBody(pmout->vrigidbodysids[i].c_str());
536  pirb->setTarget(xsAnyURI(*pirb,str(boost::format("#%s")%_GetNodeId(bodyInfo,i))));
537  }
538 
539  if ( pmout->vrigidbodysids.size() >= 0) {
540  LinkInfoSequence_var links = bodyInfo->links();
541  if ( links->length() > 0 ) {
542  LinkInfo& linkInfo = links[0];
543  string jointType(CORBA::String_var(linkInfo.jointType));
544  if ( jointType == "fixed" ) {
545  domInstance_rigid_constraintRef pirc = daeSafeCast<domInstance_rigid_constraint>(ipmout->ipm->add(COLLADA_ELEMENT_INSTANCE_RIGID_CONSTRAINT));
546  pirc->setConstraint("rigid_constraint0");
547  }
548  }
549  }
550  return ipmout;
551  }
552 
553  virtual boost::shared_ptr<kinematics_model_output> WriteKinematics_model(BodyInfo_impl* bodyInfo) {
554  boost::shared_ptr<kinematics_model_output> kmout = _GetKinematics_model(bodyInfo);
555  if( !!kmout ) {
556  return kmout;
557  }
558 
559  domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(_kinematicsModelsLib->add(COLLADA_ELEMENT_KINEMATICS_MODEL));
560  string kmodelid = str(boost::format("kmodel%d")%_GetRobotId(bodyInfo));
561  kmodel->setId(kmodelid.c_str());
562  kmodel->setName(bodyInfo->name());
563 
564  domKinematics_model_techniqueRef ktec = daeSafeCast<domKinematics_model_technique>(kmodel->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
565 
566  // Create root node for the visual scene
567  domNodeRef pnoderoot = daeSafeCast<domNode>(_scene.vscene->add(COLLADA_ELEMENT_NODE));
568  string bodyid = _GetNodeId(bodyInfo);
569  pnoderoot->setId(bodyid.c_str());
570  pnoderoot->setSid(bodyid.c_str());
571  pnoderoot->setName(bodyInfo->name());
572 
573  LinkInfoSequence_var links = bodyInfo->links();
574  vector<domJointRef> vdomjoints(links->length());
575  kmout.reset(new kinematics_model_output());
576  kmout->kmodel = kmodel;
577  kmout->vaxissids.resize(0);
578  kmout->vlinksids.resize(links->length());
579 
580  // add root link
581  kmout->mapjointnames[std::string(links[0].name)] = 1000;
582  for(size_t ilink = 0; ilink < vdomjoints.size(); ++ilink) {
583  LinkInfo& linkInfo = links[ilink];
584  if (!linkInfo.segments.length()){
585  std::cerr << "Collada Warning: segment node for " << ilink << "th joint is not defined" << std::endl;
586  }else{
587  kmout->maplinknames[std::string(linkInfo.segments[0].name)] = ilink;
588  }
589  string jointType(CORBA::String_var(linkInfo.jointType));
590  daeString colladaelement;
591  int dof = 1;
592  dReal fmult = 1;
593  vector<dReal> lmin, lmax;
594  if( jointType == "fixed" ) {
595  colladaelement = COLLADA_ELEMENT_REVOLUTE;
596  lmin.push_back(0);
597  lmax.push_back(0);
598  }
599  else {
600  if( jointType == "rotate" ) {
601  colladaelement = COLLADA_ELEMENT_REVOLUTE;
602  fmult = 180.0f/M_PI;
603  }
604  else if( jointType == "slide" ) {
605  colladaelement = COLLADA_ELEMENT_PRISMATIC;
606  }
607  else {
608  COLLADALOG_INFO(str(boost::format("joint type %s not supported")%jointType));
609  continue;
610  }
611  if( linkInfo.llimit.length() > 0 ) {
612  lmin.push_back(fmult*linkInfo.llimit[0]);
613  }
614  if( linkInfo.ulimit.length() > 0 ) {
615  lmax.push_back(fmult*linkInfo.ulimit[0]);
616  }
617  }
618 
619  if ( linkInfo.jointId >= 0 ) {
620  kmout->mapjointnames[std::string(linkInfo.name)] = linkInfo.jointId;
621  } else {
622  kmout->mapjointnames[std::string(linkInfo.name)] = 1000+ilink;
623  }
624 
625  domJointRef pdomjoint = daeSafeCast<domJoint>(ktec->add(COLLADA_ELEMENT_JOINT));
626  string jointsid = str(boost::format("jointsid%d")%kmout->mapjointnames[std::string(linkInfo.name)]);
627  pdomjoint->setSid( jointsid.c_str() );
628  //pdomjoint->setName(str(boost::format("joint%d")%linkInfo.jointId).c_str());
629  pdomjoint->setName(linkInfo.name); // changed
630  vector<domAxis_constraintRef> vaxes(dof);
631  for(int ia = 0; ia < dof; ++ia) {
632  vaxes[ia] = daeSafeCast<domAxis_constraint>(pdomjoint->add(colladaelement));
633  string axisid = str(boost::format("axis%d")%ia);
634  vaxes[ia]->setSid(axisid.c_str());
636  axissid.ijoint = ilink;
637  axissid.sid = jointsid+string("/")+axisid;
638  axissid.iaxis = ia;
639  axissid.jointnodesid = str(boost::format("%s/%s")%bodyid%_GetJointNodeSid(ilink,ia));
640  kmout->vaxissids.push_back(axissid);
641  domAxisRef paxis = daeSafeCast<domAxis>(vaxes.at(ia)->add(COLLADA_ELEMENT_AXIS));
642  paxis->getValue().setCount(3);
643  paxis->getValue()[0] = linkInfo.jointAxis[0];
644  paxis->getValue()[1] = linkInfo.jointAxis[1];
645  paxis->getValue()[2] = linkInfo.jointAxis[2];
646  if( lmin.size() > 0 || lmax.size() > 0 ) {
647  domJoint_limitsRef plimits = daeSafeCast<domJoint_limits>(vaxes[ia]->add(COLLADA_TYPE_LIMITS));
648  if( ia < (int)lmin.size() ) {
649  daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MIN))->getValue() = lmin.at(ia);
650  }
651  if( ia < (int)lmax.size() ) {
652  daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MAX))->getValue() = lmax.at(ia);
653  }
654  }
655  }
656  vdomjoints.at(ilink) = pdomjoint;
657  }
658 
659  list<int> listunusedlinks;
660  for(unsigned int i = 0; i < links->length(); ++i) {
661  listunusedlinks.push_back(i);
662  }
663 
664  while(listunusedlinks.size()>0) {
665  int ilink = listunusedlinks.front();
666  LINKOUTPUT childinfo = _WriteLink(bodyInfo, ilink, ktec, pnoderoot, kmodel->getID(), kmout->mapjointnames);
667  _WriteTransformation(childinfo.plink, links[ilink].rotation, links[ilink].translation);
668  _WriteTransformation(childinfo.pnode, links[ilink].rotation, links[ilink].translation);
669  for(list<pair<int,std::string> >::iterator itused = childinfo.listusedlinks.begin(); itused != childinfo.listusedlinks.end(); ++itused) {
670  kmout->vlinksids.at(itused->first) = itused->second;
671  listunusedlinks.remove(itused->first);
672  }
673  }
674 
675  // // interface type
676  // {
677  // domExtraRef pextra = daeSafeCast<domExtra>(kmout->kmodel->add(COLLADA_ELEMENT_EXTRA));
678  // pextra->setType("interface_type");
679  // domTechniqueRef ptec = daeSafeCast<domTechnique>(pextra->add(COLLADA_ELEMENT_TECHNIQUE));
680  // ptec->setProfile("OpenRAVE");
681  // ptec->add("interface")->setCharData(pbody->GetXMLId());
682  // }
683 
684  // collision data
685  // {
686  // domExtraRef pextra = daeSafeCast<domExtra>(kmout->kmodel->add(COLLADA_ELEMENT_EXTRA));
687  // pextra->setType("collision");
688  // domTechniqueRef ptec = daeSafeCast<domTechnique>(pextra->add(COLLADA_ELEMENT_TECHNIQUE));
689  // ptec->setProfile("OpenRAVE");
690  // FOREACHC(itadjacent,pbody->_vForcedAdjacentLinks) {
691  // KinBody::LinkPtr plink0 = pbody->GetLink(itadjacent->first);
692  // KinBody::LinkPtr plink1 = pbody->GetLink(itadjacent->second);
693  // if( !!plink0 && !!plink1 ) {
694  // daeElementRef pignore = ptec->add("ignore_link_pair");
695  // pignore->setAttribute("link0",(kmodelid + string("/") + kmout->vlinksids.at(plink0->GetIndex())).c_str());
696  // pignore->setAttribute("link1",(kmodelid + string("/") + kmout->vlinksids.at(plink1->GetIndex())).c_str());
697  // }
698  // }
699  // }
700 
701  _AddKinematics_model(bodyInfo,kmout);
702  return kmout;
703  }
704 
705  virtual boost::shared_ptr<physics_model_output> WritePhysics_model(BodyInfo_impl* bodyInfo) {
706  boost::shared_ptr<physics_model_output> pmout = _GetPhysics_model(bodyInfo);
707  if( !!pmout ) {
708  return pmout;
709  }
710  pmout.reset(new physics_model_output());
711  pmout->pmodel = daeSafeCast<domPhysics_model>(_physicsModelsLib->add(COLLADA_ELEMENT_PHYSICS_MODEL));
712  string pmodelid = str(boost::format("pmodel%d")%_GetRobotId(bodyInfo));
713  pmout->pmodel->setId(pmodelid.c_str());
714  pmout->pmodel->setName(bodyInfo->name());
715  LinkInfoSequence_var links = bodyInfo->links();
716  for(unsigned int ilink = 0; ilink < links->length(); ++ilink) {
717  LinkInfo& link = links[ilink];
718  domRigid_bodyRef rigid_body = daeSafeCast<domRigid_body>(pmout->pmodel->add(COLLADA_ELEMENT_RIGID_BODY));
719  string rigidsid = str(boost::format("rigid%d")%ilink);
720  pmout->vrigidbodysids.push_back(rigidsid);
721  rigid_body->setId(rigidsid.c_str());
722  rigid_body->setSid(rigidsid.c_str());
723  rigid_body->setName(link.segments[0].name);
724  domRigid_body::domTechnique_commonRef ptec = daeSafeCast<domRigid_body::domTechnique_common>(rigid_body->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
725  domTargetable_floatRef mass = daeSafeCast<domTargetable_float>(ptec->add(COLLADA_ELEMENT_MASS));
726  mass->setValue(link.mass);
727  dmatrix inertia, evec;
728  dvector eval;
729  inertia.resize(3,3);
730  inertia(0,0) = link.inertia[0]; inertia(0,1) = link.inertia[1]; inertia(0,2) = link.inertia[2];
731  inertia(1,0) = link.inertia[3]; inertia(1,1) = link.inertia[4]; inertia(1,2) = link.inertia[5];
732  inertia(2,0) = link.inertia[6]; inertia(2,1) = link.inertia[7]; inertia(2,2) = link.inertia[8];
733  evec.resize(3,3);
734  eval.resize(3);
735  hrp::calcEigenVectors(inertia,evec,eval);
736  if (det(evec) < 0.0) {/* fix for right-handed coordinates */
737  // hrp::calcEigenVectors return row majored matrix??
738  evec(2,0) *= -1.0; evec(2,1) *= -1.0; evec(2,2) *= -1.0;
739  }
740  DblArray12 tinertiaframe;
741  for(int j = 0; j < 3; ++j) {
742  // hrp::calcEigenVectors return row majored matrix??
743  tinertiaframe[4*0+j] = evec(j, 0);
744  tinertiaframe[4*1+j] = evec(j, 1);
745  tinertiaframe[4*2+j] = evec(j, 2);
746  }
747  DblArray4 quat, rotation;
748  DblArray3 translation;
749  QuatFromMatrix(quat,tinertiaframe);
750  AxisAngleFromQuat(rotation,quat);
751  domTargetable_float3Ref pdominertia = daeSafeCast<domTargetable_float3>(ptec->add(COLLADA_ELEMENT_INERTIA));
752  pdominertia->getValue().setCount(3);
753  pdominertia->getValue()[0] = eval[0]; pdominertia->getValue()[1] = eval[1]; pdominertia->getValue()[2] = eval[2];
754  daeElementRef mass_frame = ptec->add(COLLADA_ELEMENT_MASS_FRAME);
755  _WriteTransformation(mass_frame, rotation, link.centerOfMass);
756  // add all the parents
757  int icurlink = ilink;
758  while(icurlink > 0) {
759  _WriteTransformation(mass_frame, links[icurlink].rotation, links[icurlink].translation);
760  icurlink = links[icurlink].parentIndex;
761  }
762  //daeSafeCast<domRigid_body::domTechnique_common::domDynamic>(ptec->add(COLLADA_ELEMENT_DYNAMIC))->setValue(xsBoolean(dynamic));
763  // create a shape for every geometry
764  for(unsigned int igeom = 0; igeom < link.shapeIndices.length(); ++igeom) {
765  const TransformedShapeIndex& tsi = link.shapeIndices[igeom];
766  DblArray12 transformMatrix;
767  if( tsi.inlinedShapeTransformMatrixIndex >= 0 ) {
768  PoseMult(transformMatrix, link.inlinedShapeTransformMatrices[tsi.inlinedShapeTransformMatrixIndex],tsi.transformMatrix);
769  }
770  else {
771  for(int i = 0; i < 12; ++i) {
772  transformMatrix[i] = tsi.transformMatrix[i];
773  }
774  }
775  domRigid_body::domTechnique_common::domShapeRef pdomshape = daeSafeCast<domRigid_body::domTechnique_common::domShape>(ptec->add(COLLADA_ELEMENT_SHAPE));
776  // there is a weird bug here where _WriteTranformation will fail to create rotate/translate elements in instance_geometry is created first... (is this part of the spec?)
777  QuatFromMatrix(quat,transformMatrix);
778  AxisAngleFromQuat(rotation,quat);
779  translation[0] = transformMatrix[4*0+3]; translation[1] = transformMatrix[4*1+3]; translation[2] = transformMatrix[4*2+3];
780  _WriteTransformation(pdomshape,rotation,translation);
781  icurlink = ilink;
782  while(icurlink >= 0) {
783  _WriteTransformation(pdomshape, links[icurlink].rotation, links[icurlink].translation);
784  icurlink = links[icurlink].parentIndex;
785  }
786  domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pdomshape->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
787  pinstgeom->setUrl(xsAnyURI(*pinstgeom,string("#")+_GetGeometryId(bodyInfo, ilink,igeom)));
788  }
789  }
790  if ( links->length() > 0 && std::string(CORBA::String_var(links[0].jointType)) == std::string("fixed") ) {
791  domRigid_constraintRef rigid_constraint = daeSafeCast<domRigid_constraint>(pmout->pmodel->add(COLLADA_ELEMENT_RIGID_CONSTRAINT));
792  rigid_constraint->setSid("rigid_constraint0");
793  rigid_constraint->setName(links[0].segments[0].name);
794  domRigid_constraint::domAttachmentRef patt = daeSafeCast<domRigid_constraint::domAttachment>(rigid_constraint->add(COLLADA_TYPE_ATTACHMENT));
795  patt->setRigid_body("#rigid0");
796  domRigid_constraint::domRef_attachmentRef prefatt = daeSafeCast<domRigid_constraint::domRef_attachment>(rigid_constraint->add(COLLADA_TYPE_REF_ATTACHMENT));
797  prefatt->setRigid_body("#visual1");
798  }
799 
800  return pmout;
801  }
802 
806  virtual domGeometryRef WriteGeometry(BodyInfo_impl* bodyInfo, const ShapeInfo& shapeInfo, const DblArray12& transformMatrix, const string& parentid)
807  {
808  const FloatSequence& vertices = shapeInfo.vertices;
809  const LongSequence& triangles = shapeInfo.triangles;
810  string effid = parentid+string("_eff");
811  string matid = parentid+string("_mat");
812  string texid = parentid+string("_tex");
813 
814  AppearanceInfo& appearanceInfo = (*bodyInfo->appearances())[shapeInfo.appearanceIndex];
815  const FloatSequence& normals = appearanceInfo.normals;
816  const FloatSequence& textures = appearanceInfo.textureCoordinate;
817  domEffectRef pdomeff;
818  if ( appearanceInfo.materialIndex < 0 ) {
819  MaterialInfo matInfo;
820  pdomeff = WriteEffect(matInfo);
821  } else {
822  pdomeff = WriteEffect((*bodyInfo->materials())[appearanceInfo.materialIndex]);
823  }
824  pdomeff->setId(effid.c_str());
825 
826  if ( appearanceInfo.textureIndex >= 0 ) {
827  TextureInfo texture = (*bodyInfo->textures())[appearanceInfo.textureIndex];
828 
829  domProfile_commonRef pprofile = daeSafeCast<domProfile_common>(pdomeff->getDescendant(daeElement::matchType(domProfile_common::ID())));
830 
831  domFx_common_newparamRef pparam = daeSafeCast<domFx_common_newparam>(pprofile->add(COLLADA_ELEMENT_NEWPARAM));
832  pparam->setSid("file1-sampler");
833  domFx_sampler2DRef psampler = daeSafeCast<domFx_sampler2D>(pparam->add(COLLADA_ELEMENT_SAMPLER2D));
834  daeElementRef psurface = pparam->add("surface");
835  daeSafeCast<domInstance_image>(psampler->add("instance_image"))->setUrl(string("#"+texid).c_str());
836  psampler->add("minfilter")->setCharData("LINEAR_MIPMAP_LINEAR");
837  psampler->add("magfilter")->setCharData("LINEAR");
838 
839  domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(pdomeff->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
840  {
841  domFx_common_color_or_textureRef pdiffuse = daeSafeCast<domFx_common_color_or_texture>(pphong->getDescendant(daeElement::matchType(domFx_common_color_or_texture::ID())));
842  pphong->removeFromParent(pphong->getDiffuse());
843  }
844 
845  domFx_common_color_or_textureRef pdiffuse = daeSafeCast<domFx_common_color_or_texture>(pphong->add(COLLADA_ELEMENT_DIFFUSE));
846  domFx_common_color_or_texture::domTextureRef pdiffusetexture = daeSafeCast<domFx_common_color_or_texture::domTexture>(pdiffuse->add(COLLADA_ELEMENT_TEXTURE));
847  pdiffusetexture->setAttribute("texture", "file1-sampler");
848  pdiffusetexture->setAttribute("texcoord", "TEX0");
849 
850  domImageRef pdomtex;
851  pdomtex = WriteTexture((*bodyInfo->textures())[appearanceInfo.textureIndex]);
852  pdomtex->setId(texid.c_str());
853  }
854 
855  domMaterialRef pdommat = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
856  pdommat->setId(matid.c_str());
857  domInstance_effectRef pdominsteff = daeSafeCast<domInstance_effect>(pdommat->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
858  pdominsteff->setUrl((string("#")+effid).c_str());
859 
860  //check shapeInfo.primitiveType: SP_MESH, SP_BOX, SP_CYLINDER, SP_CONE, SP_SPHERE
861  if( shapeInfo.primitiveType != SP_MESH ) {
862  COLLADALOG_WARN("shape index is not SP_MESH type, could result in inaccuracies");
863  }
864  domGeometryRef pdomgeom = daeSafeCast<domGeometry>(_geometriesLib->add(COLLADA_ELEMENT_GEOMETRY));
865  {
866  pdomgeom->setId(parentid.c_str());
867  domMeshRef pdommesh = daeSafeCast<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH));
868  {
869  domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
870  {
871  pvertsource->setId((parentid+string("_positions")).c_str());
872 
873  domFloat_arrayRef parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
874  parray->setId((parentid+string("_positions-array")).c_str());
875  parray->setCount(vertices.length());
876  parray->setDigits(6); // 6 decimal places
877  parray->getValue().setCount(vertices.length());
878 
879  for(size_t ind = 0; ind < vertices.length(); ind += 3) {
880  DblArray3 v, vnew;
881  v[0] = vertices[ind]; v[1] = vertices[ind+1]; v[2] = vertices[ind+2];
882  PoseMultVector(vnew, transformMatrix, v);
883  parray->getValue()[ind+0] = vnew[0];
884  parray->getValue()[ind+1] = vnew[1];
885  parray->getValue()[ind+2] = vnew[2];
886  }
887 
888  domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
889  domAccessorRef pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
890  pacc->setCount(vertices.length()/3);
891  pacc->setSource(xsAnyURI(*pacc, string("#")+parentid+string("_positions-array")));
892  pacc->setStride(3);
893 
894  domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
895  px->setName("X"); px->setType("float");
896  domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
897  py->setName("Y"); py->setType("float");
898  domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
899  pz->setName("Z"); pz->setType("float");
900  }
901 
902  domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->add(COLLADA_ELEMENT_VERTICES));
903  {
904  pverts->setId("vertices");
905  domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->add(COLLADA_ELEMENT_INPUT));
906  pvertinput->setSemantic("POSITION");
907  pvertinput->setSource(domUrifragment(*pvertsource, string("#")+parentid+string("_positions")));
908  }
909 
910  domSourceRef pnormsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
911  if ( normals.length() > 0 && appearanceInfo.normalIndices.length() > 0 )
912  {
913  pnormsource->setId((parentid+string("_normals")).c_str());
914 
915  domFloat_arrayRef parray = daeSafeCast<domFloat_array>(pnormsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
916  parray->setId((parentid+string("_normals-array")).c_str());
917  parray->setCount(normals.length());
918  parray->setDigits(6); // 6 decimal places
919  parray->getValue().setCount(normals.length());
920 
921  for(size_t ind = 0; ind < normals.length(); ++ind) {
922  parray->getValue()[ind] = normals[ind];
923  }
924 
925  domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pnormsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
926  domAccessorRef pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
927  pacc->setCount(normals.length()/3);
928  pacc->setSource(xsAnyURI(*pacc, string("#")+parentid+string("_normals-array")));
929  pacc->setStride(3);
930 
931  domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
932  px->setName("X"); px->setType("float");
933  domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
934  py->setName("Y"); py->setType("float");
935  domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
936  pz->setName("Z"); pz->setType("float");
937  }
938 
939  domSourceRef ptexsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
940  if ( textures.length() > 0 && appearanceInfo.textureCoordIndices.length() >= 0 )
941  {
942  ptexsource->setId((parentid+string("_texcoords")).c_str());
943 
944  domFloat_arrayRef parray = daeSafeCast<domFloat_array>(ptexsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
945  parray->setId((parentid+string("_texcoords-array")).c_str());
946  parray->setCount(textures.length());
947  parray->setDigits(6); // 6 decimal places
948  parray->getValue().setCount(textures.length());
949 
950  for(size_t ind = 0; ind < textures.length(); ++ind) {
951  parray->getValue()[ind] = textures[ind];
952  }
953 
954  domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(ptexsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
955  domAccessorRef pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
956  pacc->setCount(vertices.length()/2);
957  pacc->setSource(xsAnyURI(*pacc, string("#")+parentid+string("_textures-array")));
958  pacc->setStride(2);
959 
960  domParamRef ps = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
961  ps->setName("S"); ps->setType("float");
962  domParamRef pt = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
963  pt->setName("T"); pt->setType("float");
964  }
965 
966  domTrianglesRef ptris = daeSafeCast<domTriangles>(pdommesh->add(COLLADA_ELEMENT_TRIANGLES));
967  {
968  int offset = 0;
969  ptris->setCount(triangles.length()/3);
970  ptris->setMaterial("mat0");
971 
972  domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
973  pvertoffset->setSemantic("VERTEX");
974  pvertoffset->setOffset(offset++);
975  pvertoffset->setSource(domUrifragment(*pverts, string("#")+parentid+string("/vertices")));
976 
977  domPRef pindices = daeSafeCast<domP>(ptris->add(COLLADA_ELEMENT_P));
978 
979  if ( normals.length() > 0 && appearanceInfo.normalIndices.length() > 0 ){
980  domInput_local_offsetRef pnormoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
981  pnormoffset->setSemantic("NORMAL");
982  pnormoffset->setOffset(offset++);
983  pnormoffset->setSource(domUrifragment(*pverts, string("#")+parentid+string("_normals")));
984 
985  }
986  if ( textures.length() > 0 && appearanceInfo.textureCoordIndices.length() > 0 ){
987  domInput_local_offsetRef ptexoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
988  ptexoffset->setSemantic("TEXCOORD");
989  ptexoffset->setOffset(offset++);
990  ptexoffset->setSource(domUrifragment(*pverts, string("#")+parentid+string("_texcoords")));
991  }
992  pindices->getValue().setCount(triangles.length()*offset);
993 
994  for(size_t ind = 0; ind < triangles.length(); ++ind) {
995  int i = ind * offset;
996  pindices->getValue()[i++] = triangles[ind];
997  if ( normals.length() > 0 && appearanceInfo.normalIndices.length() > 0 ){
998  if ( appearanceInfo.normalPerVertex == 1 ) {
999  pindices->getValue()[i++] = appearanceInfo.normalIndices[ind];
1000  } else {
1001  pindices->getValue()[i++] = appearanceInfo.normalIndices[triangles[ind]/3];
1002  }
1003  }
1004  if (textures.length() > 0 && appearanceInfo.textureCoordIndices.length() > 0 ){
1005  pindices->getValue()[i++] = appearanceInfo.textureCoordIndices[ind];
1006  }
1007  }
1008  }
1009  }
1010  }
1011 
1012  return pdomgeom;
1013  }
1014 
1018  virtual domEffectRef WriteEffect(const MaterialInfo& material)
1019  {
1020  domEffectRef pdomeff = daeSafeCast<domEffect>(_effectsLib->add(COLLADA_ELEMENT_EFFECT));
1021 
1022  domProfile_commonRef pprofile = daeSafeCast<domProfile_common>(pdomeff->add(COLLADA_ELEMENT_PROFILE_COMMON));
1023 
1024  domProfile_common::domTechniqueRef ptec = daeSafeCast<domProfile_common::domTechnique>(pprofile->add(COLLADA_ELEMENT_TECHNIQUE));
1025 
1026  domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(ptec->add(COLLADA_ELEMENT_PHONG));
1027 
1028  domFx_common_color_or_textureRef pambient = daeSafeCast<domFx_common_color_or_texture>(pphong->add(COLLADA_ELEMENT_AMBIENT));
1029  domFx_common_color_or_texture::domColorRef pambientcolor = daeSafeCast<domFx_common_color_or_texture::domColor>(pambient->add(COLLADA_ELEMENT_COLOR));
1030  pambientcolor->getValue().setCount(4);
1031  pambientcolor->getValue()[0] = material.diffuseColor[0];
1032  pambientcolor->getValue()[1] = material.diffuseColor[1];
1033  pambientcolor->getValue()[2] = material.diffuseColor[2];
1034  pambientcolor->getValue()[3] = 1;
1035 
1036  domFx_common_color_or_textureRef pdiffuse = daeSafeCast<domFx_common_color_or_texture>(pphong->add(COLLADA_ELEMENT_DIFFUSE));
1037  domFx_common_color_or_texture::domColorRef pdiffusecolor = daeSafeCast<domFx_common_color_or_texture::domColor>(pdiffuse->add(COLLADA_ELEMENT_COLOR));
1038  pdiffusecolor->getValue().setCount(4);
1039  pdiffusecolor->getValue()[0] = material.diffuseColor[0];
1040  pdiffusecolor->getValue()[1] = material.diffuseColor[1];
1041  pdiffusecolor->getValue()[2] = material.diffuseColor[2];
1042  pdiffusecolor->getValue()[3] = 1;
1043 
1044  domFx_common_color_or_textureRef pspecular = daeSafeCast<domFx_common_color_or_texture>(pphong->add(COLLADA_ELEMENT_SPECULAR));
1045  domFx_common_color_or_texture::domColorRef pspecularcolor = daeSafeCast<domFx_common_color_or_texture::domColor>(pspecular->add(COLLADA_ELEMENT_COLOR));
1046  pspecularcolor->getValue().setCount(4);
1047  pspecularcolor->getValue()[0] = material.specularColor[0];
1048  pspecularcolor->getValue()[1] = material.specularColor[1];
1049  pspecularcolor->getValue()[2] = material.specularColor[2];
1050  pspecularcolor->getValue()[3] = 1;
1051 
1052  domFx_common_color_or_textureRef pemission = daeSafeCast<domFx_common_color_or_texture>(pphong->add(COLLADA_ELEMENT_EMISSION));
1053  domFx_common_color_or_texture::domColorRef pemissioncolor = daeSafeCast<domFx_common_color_or_texture::domColor>(pemission->add(COLLADA_ELEMENT_COLOR));
1054  pemissioncolor->getValue().setCount(4);
1055  pemissioncolor->getValue()[0] = material.emissiveColor[0];
1056  pemissioncolor->getValue()[1] = material.emissiveColor[1];
1057  pemissioncolor->getValue()[2] = material.emissiveColor[2];
1058  pemissioncolor->getValue()[3] = 1;
1059 
1060  //domFx_common_color_or_textureRef ptransparency = daeSafeCast<domFx_common_color_or_texture>(pphong->add(COLLADA_ELEMENT_TRANSPARENCY));
1061  //ptransparency->setAttribute("opage","A_ZERO"); // 0 is opaque
1062  return pdomeff;
1063  }
1064 
1066  virtual domImageRef WriteTexture(const TextureInfo& texture)
1067  {
1068  domImageRef pdomimg = daeSafeCast<domImage>(_imagesLib->add(COLLADA_ELEMENT_IMAGE));
1069  pdomimg->setName(texture.url);
1070  domImage::domInit_fromRef pdominitfrom = daeSafeCast<domImage::domInit_from>(pdomimg->add(COLLADA_ELEMENT_INIT_FROM));
1071  if ( ! pdominitfrom->setCharData(string(texture.url)) ) {
1072  domImage_source::domRefRef pdomref = daeSafeCast<domImage_source::domRef>(pdominitfrom->add(COLLADA_ELEMENT_REF));
1073  pdomref->setValue(texture.url);
1074  }
1075  return pdomimg;
1076  }
1077 
1078  virtual daeElementRef WriteSensor(const SensorInfo& sensor, const string& parentid)
1079  {
1080  daeElementRef domsensor = _sensorsLib->add("sensor");
1081  _nextsensorid++; domsensor->setAttribute("id",str(boost::format("sensor%d")%_nextsensorid).c_str());
1082 
1083  string vrmltype = tolowerstring(string(sensor.type));
1084  domsensor->setAttribute("sid",str(boost::format("%d")%sensor.id).c_str());
1085  if( vrmltype == "force" ) {
1086  domsensor->setAttribute("type","base_force6d");
1087  domsensor->add("load_range_force")->setCharData(str(boost::format("%f %f %f")%sensor.specValues[0]%sensor.specValues[1]%sensor.specValues[2]));
1088  domsensor->add("load_range_torque")->setCharData(str(boost::format("%f %f %f")%sensor.specValues[3]%sensor.specValues[4]%sensor.specValues[5]));
1089  }
1090  else if( vrmltype == "rategyro") {
1091  domsensor->setAttribute("type","base_imu");
1092  domsensor->add("max_angular_velocity")->setCharData(str(boost::format("%f %f %f")%sensor.specValues[0]%sensor.specValues[1]%sensor.specValues[2]));
1093  }
1094  else if( vrmltype == "acceleration" ) {
1095  domsensor->setAttribute("type","base_imu");
1096  domsensor->add("max_acceleration")->setCharData(str(boost::format("%f %f %f")%sensor.specValues[0]%sensor.specValues[1]%sensor.specValues[2]));
1097  }
1098  else if( vrmltype == "vision" ) {
1099  domsensor->setAttribute("type","base_pinhole_camera");
1100  // frontClipDistance, backClipDistance, fieldOfView, type, width, height, frameRate
1101  if( sensor.specValues.length() != 7 ) {
1102  COLLADALOG_WARN(str(boost::format("vision sensor has wrong number of values! %d!=7")%sensor.specValues.length()));
1103  }
1104  domsensor->add("focal_length")->setCharData(str(boost::format("%f")%sensor.specValues[0]));
1105  double fieldOfView = sensor.specValues[2], width = sensor.specValues[4], height = sensor.specValues[5];
1106  stringstream sintrinsic; sintrinsic << std::setprecision(15);
1107  double fx = 0.5/(tanf(fieldOfView*0.5));
1108  sintrinsic << fx*width << " 0 " << 0.5*width << " 0 " << fx*height << " " << 0.5*height;
1109  domsensor->add("intrinsic")->setCharData(sintrinsic.str());
1110  stringstream simage_dimensions; simage_dimensions << (int)width << " " << (int)height << " ";
1111  string format = "uint8";
1112  Camera::CameraType cameratype = Camera::CameraType((int)sensor.specValues[3]);
1113  switch(cameratype) {
1114  case Camera::NONE:
1115  COLLADALOG_WARN("no camera type specified!");
1116  break;
1117  case Camera::COLOR:
1118  simage_dimensions << 3;
1119  break;
1120  case Camera::MONO:
1121  simage_dimensions << 1;
1122  break;
1123  case Camera::DEPTH:
1124  simage_dimensions << 1;
1125  format = "float32";
1126  break;
1127  case Camera::COLOR_DEPTH:
1128  format = "float32";
1129  simage_dimensions << 4;
1130  break;
1131  case Camera::MONO_DEPTH:
1132  format = "float32";
1133  simage_dimensions << 2;
1134  break;
1135  }
1136  domsensor->add("format")->setCharData(format);
1137  domsensor->add("image_dimensions")->setCharData(simage_dimensions.str());
1138  domsensor->add("measurement_time")->setCharData(str(boost::format("%f")%(1.0/sensor.specValues[6])));
1139  }
1140  else if( vrmltype == "range" ) {
1141  domsensor->setAttribute("type","base_laser1d");
1142  domsensor->add("angle_range")->setCharData(str(boost::format("%f")%(sensor.specValues[0])));
1143  domsensor->add("angle_increment")->setCharData(str(boost::format("%f")%(sensor.specValues[1])));
1144  domsensor->add("measurement_time")->setCharData(str(boost::format("%f")%(1.0/sensor.specValues[2])));
1145  domsensor->add("distance_range")->setCharData(str(boost::format("%f")%(sensor.specValues[3])));
1146  }
1147  return domsensor;
1148  }
1149 
1150  virtual daeElementRef WriteActuator(const LinkInfo& plink, const string& parentid)
1151  {
1152  daeElementRef domactuator = _actuatorsLib->add("actuator");
1153  _nextactuatorid++; domactuator->setAttribute("id",str(boost::format("actuator%d")%_nextactuatorid).c_str());
1154  domactuator->setAttribute("type","electric_motor");
1155  domactuator->add("assigned_power_rating")->setCharData("1.0");
1156  double max_speed = plink.uvlimit.length()/2*M_PI > 0 ? plink.uvlimit[0] : 0;
1157  domactuator->add("max_speed")->setCharData(str(boost::format("%f")%max_speed));
1158  domactuator->add("no_load_speed")->setCharData(str(boost::format("%f")%max_speed));
1159  double max_torque = std::abs((plink.climit.length() > 0 ? plink.climit[0] : 0)*plink.gearRatio*plink.torqueConst);
1160  domactuator->add("nominal_torque")->setCharData(str(boost::format("%f")%max_torque));
1161  domactuator->add("nominal_voltage")->setCharData("0");
1162  domactuator->add("rotor_inertia")->setCharData(str(boost::format("%f")%(plink.rotorInertia)));
1163  domactuator->add("speed_constant")->setCharData("0");
1164  domactuator->add("speed_torque_gradient")->setCharData("0");
1165  domactuator->add("starting_current")->setCharData("0");
1166  domactuator->add("terminal_resistance")->setCharData(str(boost::format("%f")%(plink.rotorResistor)));
1167  domactuator->add("torque_constant")->setCharData(str(boost::format("%f")%(plink.torqueConst)));
1168  domactuator->add("gear_ratio")->setCharData(str(boost::format("%f")%(plink.gearRatio)));
1169  domactuator->add("encoder_pulse")->setCharData(str(boost::format("%f")%(plink.encoderPulse)));
1170 
1171  return domactuator;
1172  }
1173 
1174 private:
1175 
1177  virtual void _CreateScene()
1178  {
1179  // Create visual scene
1180  _scene.vscene = daeSafeCast<domVisual_scene>(_visualScenesLib->add(COLLADA_ELEMENT_VISUAL_SCENE));
1181  _scene.vscene->setId("vscene");
1182  _scene.vscene->setName("OpenRAVE Visual Scene");
1183 
1184  // Create kinematics scene
1185  _scene.kscene = daeSafeCast<domKinematics_scene>(_kinematicsScenesLib->add(COLLADA_ELEMENT_KINEMATICS_SCENE));
1186  _scene.kscene->setId("kscene");
1187  _scene.kscene->setName("OpenRAVE Kinematics Scene");
1188 
1189  // Create physic scene
1190  _scene.pscene = daeSafeCast<domPhysics_scene>(_physicsScenesLib->add(COLLADA_ELEMENT_PHYSICS_SCENE));
1191  _scene.pscene->setId("pscene");
1192  _scene.pscene->setName("OpenRAVE Physics Scene");
1193 
1194  // Create instance visual scene
1195  _scene.viscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE ));
1196  _scene.viscene->setUrl( (string("#") + string(_scene.vscene->getID())).c_str() );
1197 
1198  // Create instance kinematics scene
1199  _scene.kiscene = daeSafeCast<domInstance_kinematics_scene>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE ));
1200  _scene.kiscene->setUrl( (string("#") + string(_scene.kscene->getID())).c_str() );
1201 
1202  // Create instance physics scene
1203  _scene.piscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE ));
1204  _scene.piscene->setUrl( (string("#") + string(_scene.pscene->getID())).c_str() );
1205  }
1206 
1215  virtual LINKOUTPUT _WriteLink(BodyInfo_impl* bodyInfo, int ilink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri, std::map<std::string, int> mapjointnames)
1216  {
1217  LinkInfo& plink = (*bodyInfo->links())[ilink];
1218  LINKOUTPUT out;
1219  string linksid = _GetLinkSid(ilink);
1220  domLinkRef pdomlink = daeSafeCast<domLink>(pkinparent->add(COLLADA_ELEMENT_LINK));
1221  pdomlink->setName(plink.segments[0].name);
1222  pdomlink->setSid(linksid.c_str());
1223 
1224  domNodeRef pnode = daeSafeCast<domNode>(pnodeparent->add(COLLADA_ELEMENT_NODE));
1225  std::string nodeid = _GetNodeId(bodyInfo,ilink);
1226  pnode->setId( nodeid.c_str() );
1227  string nodesid = str(boost::format("node%d")%ilink);
1228  pnode->setSid(nodesid.c_str());
1229  pnode->setName(plink.segments[0].name);
1230  ShapeInfoSequence* curShapeInfoSeq = bodyInfo->shapes();
1231 
1232  for(unsigned int igeom = 0; igeom < plink.shapeIndices.length(); ++igeom) {
1233  string geomid = _GetGeometryId(bodyInfo, ilink,igeom);
1234  const TransformedShapeIndex& tsi = plink.shapeIndices[igeom];
1235  DblArray12 transformMatrix;
1236  //this matrix is already multiplied in tsi.transformMatrix plink.inlinedShapeTransformMatrices[tsi.inlinedShapeTransformMatrixIndex]
1237  for(int i = 0; i < 12; ++i) {
1238  transformMatrix[i] = tsi.transformMatrix[i];
1239  }
1240  domGeometryRef pdomgeom = WriteGeometry(bodyInfo,(*curShapeInfoSeq)[tsi.shapeIndex], transformMatrix, geomid);
1241  domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
1242  pinstgeom->setUrl((string("#")+geomid).c_str());
1243 
1244  domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
1245  domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1246  domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
1247  pinstmat->setTarget(xsAnyURI(*pinstmat, string("#")+geomid+string("_mat")));
1248  pinstmat->setSymbol("mat0");
1249  }
1250 
1251  // sensor shape
1252  int igeomid = plink.shapeIndices.length();
1253  for(size_t isensor = 0; isensor < plink.sensors.length(); ++isensor) {
1254  SensorInfo& sensor = plink.sensors[isensor];
1255  // shape
1256  for(unsigned int igeom = 0; igeom < sensor.shapeIndices.length(); ++igeom) {
1257  string geomid = _GetGeometryId(bodyInfo, ilink, igeomid++);
1258 
1259  const TransformedShapeIndex& tsi = sensor.shapeIndices[igeom];
1260  DblArray12 sensorMatrix, transformMatrix,sensorgeomMatrix;
1261  PoseFromAxisAngleTranslation(sensorMatrix,sensor.rotation, sensor.translation);
1262  for(int i = 0; i < 12; ++i) {
1263  sensorgeomMatrix[i] = tsi.transformMatrix[i];
1264  }
1265  PoseMult(transformMatrix, sensorMatrix, sensorgeomMatrix);
1266 
1267  domGeometryRef pdomgeom = WriteGeometry(bodyInfo,(*bodyInfo->shapes())[tsi.shapeIndex], transformMatrix, geomid);
1268  domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
1269  pinstgeom->setUrl((string("#")+geomid).c_str());
1270 
1271  domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
1272  domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
1273  domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
1274  pinstmat->setTarget(xsAnyURI(*pinstmat, string("#")+geomid+string("_mat")));
1275  pinstmat->setSymbol("mat0");
1276  }
1277  }
1278 
1279  // go through all children
1280  for(unsigned int _ichild = 0; _ichild < plink.childIndices.length(); ++_ichild) {
1281  int ichild = plink.childIndices[_ichild];
1282  LinkInfo& childlink = (*bodyInfo->links())[ichild];
1283  domLink::domAttachment_fullRef pattfull = daeSafeCast<domLink::domAttachment_full>(pdomlink->add(COLLADA_TYPE_ATTACHMENT_FULL));
1284  // fixjoint does not have jointID
1285  string jointid = str(boost::format("%s/jointsid%d")%strModelUri%mapjointnames[string(childlink.name)]);
1286  pattfull->setJoint(jointid.c_str());
1287 
1288  LINKOUTPUT childinfo = _WriteLink(bodyInfo, ichild, pattfull, pnode, strModelUri, mapjointnames);
1289  out.listusedlinks.insert(out.listusedlinks.end(),childinfo.listusedlinks.begin(),childinfo.listusedlinks.end());
1290 
1291  _WriteTransformation(pattfull, childlink.rotation, childlink.translation);
1292  //_WriteTransformation(childinfo.plink, pjoint->GetInternalHierarchyRightTransform());
1293  //_WriteTransformation(childinfo.pnode, pjoint->GetInternalHierarchyRightTransform());
1294 
1295  string jointnodesid = _GetJointNodeSid(ichild,0);
1296  string jointType(CORBA::String_var(childlink.jointType));
1297  if( jointType == "rotate" || jointType == "fixed" ) {
1298  domRotateRef protate = daeSafeCast<domRotate>(childinfo.pnode->add(COLLADA_ELEMENT_ROTATE,0));
1299  protate->setSid(jointnodesid.c_str());
1300  protate->getValue().setCount(4);
1301  protate->getValue()[0] = childlink.jointAxis[0];
1302  protate->getValue()[1] = childlink.jointAxis[1];
1303  protate->getValue()[2] = childlink.jointAxis[2];
1304  protate->getValue()[3] = 0;
1305  }
1306  else if( jointType == "slide" ) {
1307  domTranslateRef ptrans = daeSafeCast<domTranslate>(childinfo.pnode->add(COLLADA_ELEMENT_TRANSLATE,0));
1308  ptrans->setSid(jointnodesid.c_str());
1309  ptrans->getValue().setCount(3);
1310  ptrans->getValue()[0] = 0;
1311  ptrans->getValue()[1] = 0;
1312  ptrans->getValue()[2] = 0;
1313  }
1314  else {
1315  COLLADALOG_WARN(str(boost::format("unsupported joint type specified %s")%jointType));
1316  }
1317  _WriteTransformation(childinfo.pnode, childlink.rotation, childlink.translation);
1318  }
1319 
1320  out.listusedlinks.push_back(make_pair(ilink,linksid));
1321  out.plink = pdomlink;
1322  out.pnode = pnode;
1323  return out;
1324  }
1325 
1326  void _SetRotate(domTargetable_float4Ref prot, const DblArray4& rotation)
1327  {
1328  prot->getValue().setCount(4);
1329  prot->getValue()[0] = rotation[0];
1330  prot->getValue()[1] = rotation[1];
1331  prot->getValue()[2] = rotation[2];
1332  prot->getValue()[3] = rotation[3]*(180.0/M_PI);
1333  }
1334 
1338  void _WriteTransformation(daeElementRef pelt, const DblArray4& rotation, const DblArray3& translation)
1339  {
1340  _SetRotate(daeSafeCast<domRotate>(pelt->add(COLLADA_ELEMENT_ROTATE,0)), rotation);
1341  _SetVector3(daeSafeCast<domTranslate>(pelt->add(COLLADA_ELEMENT_TRANSLATE,0))->getValue(),translation);
1342  }
1343 
1344  // binding in instance_kinematics_scene
1345  void _WriteBindingsInstance_kinematics_scene(domInstance_kinematics_sceneRef ikscene, BodyInfo_impl* bodyInfo, const std::vector<axis_sids>& vaxissids, const std::vector<std::pair<std::string,std::string> >& vkinematicsbindings)
1346  {
1347  for(std::vector<std::pair<std::string,std::string> >::const_iterator it = vkinematicsbindings.begin(); it != vkinematicsbindings.end(); ++it) {
1348  domBind_kinematics_modelRef pmodelbind = daeSafeCast<domBind_kinematics_model>(ikscene->add(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL));
1349  pmodelbind->setNode(it->second.c_str());
1350  daeSafeCast<domCommon_param>(pmodelbind->add(COLLADA_ELEMENT_PARAM))->setValue(it->first.c_str());
1351  }
1352  for(std::vector<axis_sids>::const_iterator it = vaxissids.begin(); it != vaxissids.end(); ++it) {
1353  domBind_joint_axisRef pjointbind = daeSafeCast<domBind_joint_axis>(ikscene->add(COLLADA_ELEMENT_BIND_JOINT_AXIS));
1354  pjointbind->setTarget(it->jointnodesid.c_str());
1355  daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_AXIS)->add(COLLADA_TYPE_PARAM))->setValue(it->axissid.c_str());
1356  daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_VALUE)->add(COLLADA_TYPE_PARAM))->setValue(it->valuesid.c_str());
1357  }
1358  }
1359 
1361  template <typename T> static void _SetVector3(T& t, const DblArray3& v) {
1362  t.setCount(3);
1363  t[0] = v[0];
1364  t[1] = v[1];
1365  t[2] = v[2];
1366  }
1367 
1368  virtual void _AddKinematics_model(BodyInfo_impl* bodyInfo, boost::shared_ptr<kinematics_model_output> kmout) {
1369  string xmlfilename;
1370  if( !!bodyInfo->url() ) {
1371  xmlfilename = bodyInfo->url();
1372  }
1373  for(std::list<kinbody_models>::iterator it = _listkinbodies.begin(); it != _listkinbodies.end(); ++it) {
1374  if( it->xmlfilename == xmlfilename ) {
1375  BOOST_ASSERT(!it->kmout);
1376  it->kmout = kmout;
1377  return;
1378  }
1379  }
1380  kinbody_models cache;
1381  cache.xmlfilename = xmlfilename;
1382  cache.kinematicsgeometryhash = "";
1383  cache.kmout = kmout;
1384  _listkinbodies.push_back(cache);
1385  }
1386 
1387  virtual boost::shared_ptr<kinematics_model_output> _GetKinematics_model(BodyInfo_impl* bodyInfo) {
1388  for(std::list<kinbody_models>::iterator it = _listkinbodies.begin(); it != _listkinbodies.end(); ++it) {
1389  if( !bodyInfo->url() || it->xmlfilename == bodyInfo->url() ) {
1390  return it->kmout;
1391  }
1392  }
1393  return boost::shared_ptr<kinematics_model_output>();
1394  }
1395 
1396  virtual void _AddPhysics_model(BodyInfo_impl* bodyInfo, boost::shared_ptr<physics_model_output> pmout) {
1397  string xmlfilename;
1398  if( !!bodyInfo->url() ) {
1399  xmlfilename = bodyInfo->url();
1400  }
1401  for(std::list<kinbody_models>::iterator it = _listkinbodies.begin(); it != _listkinbodies.end(); ++it) {
1402  if( it->xmlfilename == xmlfilename ) {
1403  BOOST_ASSERT(!it->pmout);
1404  it->pmout = pmout;
1405  return;
1406  }
1407  }
1408  kinbody_models cache;
1409  cache.xmlfilename = xmlfilename;
1410  cache.kinematicsgeometryhash = "";
1411  cache.pmout = pmout;
1412  _listkinbodies.push_back(cache);
1413  }
1414 
1415  virtual boost::shared_ptr<physics_model_output> _GetPhysics_model(BodyInfo_impl* bodyInfo) {
1416  for(std::list<kinbody_models>::iterator it = _listkinbodies.begin(); it != _listkinbodies.end(); ++it) {
1417  if( !bodyInfo->url() || it->xmlfilename == bodyInfo->url() ) {
1418  return it->pmout;
1419  }
1420  }
1421  return boost::shared_ptr<physics_model_output>();
1422  }
1423 
1424  virtual int _GetRobotId(BodyInfo_impl* bodyInfo) {
1425  return 1;
1426  }
1427  virtual std::string _GetNodeId(BodyInfo_impl* bodyInfo) {
1428  return str(boost::format("visual%d")%_GetRobotId(bodyInfo));
1429  }
1430  virtual std::string _GetNodeId(BodyInfo_impl* bodyInfo, int ilink) {
1431  return str(boost::format("v%d.node%d")%_GetRobotId(bodyInfo)%ilink);
1432  }
1433 
1434  virtual std::string _GetLinkSid(int ilink) {
1435  return str(boost::format("link%d")%ilink);
1436  }
1437 
1438  virtual std::string _GetGeometryId(BodyInfo_impl* bodyInfo, int ilink, int igeom) {
1439  return str(boost::format("g%d_%d_geom%d")%_GetRobotId(bodyInfo)%ilink%igeom);
1440  }
1441  virtual std::string _GetJointNodeSid(int ijoint, int iaxis) {
1442  return str(boost::format("node_joint%d_axis%d")%ijoint%iaxis);
1443  }
1444 
1445  virtual void handleError( daeString msg )
1446  {
1447  cerr << "COLLADA error: " << msg << endl;
1448  }
1449 
1450  virtual void handleWarning( daeString msg )
1451  {
1452  cout << "COLLADA warning: " << msg << endl;
1453  }
1454 
1455  boost::shared_ptr<DAE> _collada;
1456  domCOLLADA* _dom;
1457  daeDocument* _doc;
1458  domCOLLADA::domSceneRef _globalscene;
1459  domLibrary_visual_scenesRef _visualScenesLib;
1460  domLibrary_kinematics_scenesRef _kinematicsScenesLib;
1461  domLibrary_kinematics_modelsRef _kinematicsModelsLib;
1462  domLibrary_articulated_systemsRef _articulatedSystemsLib;
1463  domLibrary_physics_scenesRef _physicsScenesLib;
1464  domLibrary_physics_modelsRef _physicsModelsLib;
1465  domLibrary_materialsRef _materialsLib;
1466  domLibrary_effectsRef _effectsLib;
1467  domLibrary_imagesRef _imagesLib;
1468  domLibrary_geometriesRef _geometriesLib;
1469  domTechniqueRef _sensorsLib;
1470  domTechniqueRef _actuatorsLib;
1471  int _nextsensorid, _nextactuatorid;
1473  std::list<kinbody_models> _listkinbodies;
1474  std::list<ManipulatorInfo> _listmanipulators;
1475 };
1476 
1477 #endif
ColladaWriter::SCENE
Definition: ColladaWriter.h:63
ColladaWriter::_collada
boost::shared_ptr< DAE > _collada
Definition: ColladaWriter.h:1455
ShapeSetInfo_impl::appearances
virtual AppearanceInfoSequence * appearances()
Definition: ShapeSetInfo_impl.cpp:93
ColladaWriter::_SetVector3
static void _SetVector3(T &t, const DblArray3 &v)
Set vector of three elements.
Definition: ColladaWriter.h:1361
ColladaWriter::physics_model_output
Definition: ColladaWriter.h:80
basename
std::string basename(const std::string name)
Definition: projectGenerator.cpp:33
ColladaUtil::COLLADALOG_INFO
void COLLADALOG_INFO(const std::string &s)
Definition: ColladaUtil.h:87
width
png_infop png_uint_32 * width
Definition: png.h:2306
i
png_uint_32 i
Definition: png.h:2732
ColladaWriter::_sensorsLib
domTechniqueRef _sensorsLib
custom sensors library
Definition: ColladaWriter.h:1469
ColladaWriter::_visualScenesLib
domLibrary_visual_scenesRef _visualScenesLib
Definition: ColladaWriter.h:1459
ManipulatorInfo::grippernames
std::list< std::string > grippernames
Definition: ColladaWriter.h:48
ColladaUtil::dReal
double dReal
Definition: ColladaUtil.h:78
ColladaWriter::axis_sids::jointnodesid
string jointnodesid
Definition: ColladaWriter.h:109
ColladaUtil::PoseMult
void PoseMult(OpenHRP::DblArray12 &mres, const T &m0, const OpenHRP::DblArray12 &m1)
Definition: ColladaUtil.h:133
ColladaWriter::kinematics_model_output::axis_output::ijoint
int ijoint
Definition: ColladaWriter.h:95
ColladaWriter::_GetNodeId
virtual std::string _GetNodeId(BodyInfo_impl *bodyInfo, int ilink)
Definition: ColladaWriter.h:1430
ColladaWriter::WriteEffect
virtual domEffectRef WriteEffect(const MaterialInfo &material)
Definition: ColladaWriter.h:1018
ColladaWriter::axis_sids
Definition: ColladaWriter.h:105
ManipulatorInfo::gripperdir
std::list< std::string > gripperdir
Definition: ColladaWriter.h:49
hrp::calcEigenVectors
int calcEigenVectors(const dmatrix &_a, dmatrix &_evec, dvector &_eval)
Definition: MatrixSolvers.cpp:308
hrp::dvector
Eigen::VectorXd dvector
Definition: EigenTypes.h:14
ShapeSetInfo_impl::shapes
virtual ShapeInfoSequence * shapes()
Definition: ShapeSetInfo_impl.cpp:87
ColladaWriter::_WriteInstance_physics_model
virtual boost::shared_ptr< instance_physics_model_output > _WriteInstance_physics_model(BodyInfo_impl *bodyInfo, daeElementRef parent, const string &sidscope)
Definition: ColladaWriter.h:518
ColladaWriter::SCENE::kiscene
domInstance_kinematics_sceneRef kiscene
Definition: ColladaWriter.h:69
ColladaWriter::_SetRotate
void _SetRotate(domTargetable_float4Ref prot, const DblArray4 &rotation)
Definition: ColladaWriter.h:1326
ColladaWriter::_WriteTransformation
void _WriteTransformation(daeElementRef pelt, const DblArray4 &rotation, const DblArray3 &translation)
Write transformation.
Definition: ColladaWriter.h:1338
ColladaWriter::kinematics_model_output
Definition: ColladaWriter.h:86
ColladaWriter::_articulatedSystemsLib
domLibrary_articulated_systemsRef _articulatedSystemsLib
Definition: ColladaWriter.h:1462
ColladaWriter::instance_articulated_system_output::vaxissids
std::vector< axis_sids > vaxissids
Definition: ColladaWriter.h:123
ColladaWriter::kinematics_model_output::axis_output::jointnodesid
string jointnodesid
Definition: ColladaWriter.h:97
ColladaWriter::kinematics_model_output::vaxissids
std::vector< axis_output > vaxissids
no ordering
Definition: ColladaWriter.h:100
ColladaUtil::AxisAngleFromQuat
void AxisAngleFromQuat(OpenHRP::DblArray4 &rotation, const OpenHRP::DblArray4 &quat)
Definition: ColladaUtil.h:197
ColladaWriter::instance_kinematics_model_output::ikm
domInstance_kinematics_modelRef ikm
Definition: ColladaWriter.h:114
ColladaWriter::handleError
virtual void handleError(daeString msg)
Definition: ColladaWriter.h:1445
M_PI
#define M_PI
Definition: hrplib/hrpModel/Sensor.cpp:14
ColladaWriter::_listkinbodies
std::list< kinbody_models > _listkinbodies
Definition: ColladaWriter.h:1473
ColladaWriter::kinbody_models::kinematicsgeometryhash
std::string kinematicsgeometryhash
Definition: ColladaWriter.h:137
ColladaWriter::kinematics_model_output::maplinknames
std::map< std::string, int > maplinknames
Definition: ColladaWriter.h:102
ColladaWriter::SCENE::viscene
domInstance_with_extraRef viscene
Definition: ColladaWriter.h:68
BodyInfo_impl::name
virtual char * name()
Definition: BodyInfo_impl.cpp:62
ColladaWriter::WriteSensor
virtual daeElementRef WriteSensor(const SensorInfo &sensor, const string &parentid)
Definition: ColladaWriter.h:1078
ColladaWriter::_materialsLib
domLibrary_materialsRef _materialsLib
Definition: ColladaWriter.h:1465
ColladaWriter::axis_sids::axissid
string axissid
Definition: ColladaWriter.h:109
ColladaWriter::physics_model_output::pmodel
domPhysics_modelRef pmodel
Definition: ColladaWriter.h:82
ColladaWriter::_geometriesLib
domLibrary_geometriesRef _geometriesLib
Definition: ColladaWriter.h:1468
ColladaUtil::tolowerstring
std::string tolowerstring(const std::string &s)
Definition: ColladaUtil.h:296
ColladaWriter::SCENE::piscene
domInstance_with_extraRef piscene
Definition: ColladaWriter.h:70
MatrixSolvers.h
ColladaWriter::_GetJointNodeSid
virtual std::string _GetJointNodeSid(int ijoint, int iaxis)
Definition: ColladaWriter.h:1441
ColladaWriter::kinematics_model_output::axis_output::iaxis
int iaxis
Definition: ColladaWriter.h:96
ShapeSetInfo_impl::textures
virtual TextureInfoSequence * textures()
Definition: ShapeSetInfo_impl.cpp:105
ColladaUtil::COLLADALOG_VERBOSE
void COLLADALOG_VERBOSE(const std::string &s)
Definition: ColladaUtil.h:81
ColladaWriter::LINKOUTPUT::plink
daeElementRef plink
Definition: ColladaWriter.h:76
ColladaWriter::SCENE::pscene
domPhysics_sceneRef pscene
Definition: ColladaWriter.h:67
ColladaWriter::_WriteLink
virtual LINKOUTPUT _WriteLink(BodyInfo_impl *bodyInfo, int ilink, daeElementRef pkinparent, domNodeRef pnodeparent, const string &strModelUri, std::map< std::string, int > mapjointnames)
Write link of a kinematic body.
Definition: ColladaWriter.h:1215
ColladaWriter::LINKOUTPUT
Definition: ColladaWriter.h:73
ManipulatorInfo::name
std::string name
Definition: ColladaWriter.h:46
ColladaWriter::_kinematicsScenesLib
domLibrary_kinematics_scenesRef _kinematicsScenesLib
Definition: ColladaWriter.h:1460
ColladaWriter::instance_physics_model_output::ipm
domInstance_physics_modelRef ipm
Definition: ColladaWriter.h:131
ColladaWriter::_physicsScenesLib
domLibrary_physics_scenesRef _physicsScenesLib
Definition: ColladaWriter.h:1463
ColladaWriter::instance_articulated_system_output::vkinematicsbindings
std::vector< std::pair< std::string, std::string > > vkinematicsbindings
Definition: ColladaWriter.h:126
ColladaWriter::kinbody_models
Definition: ColladaWriter.h:135
filename
char * filename
Definition: cdjpeg.h:133
ColladaWriter::WriteTexture
virtual domImageRef WriteTexture(const TextureInfo &texture)
Write texture.
Definition: ColladaWriter.h:1066
ColladaWriter::_nextsensorid
int _nextsensorid
Definition: ColladaWriter.h:1471
ColladaWriter::axis_sids::valuesid
string valuesid
Definition: ColladaWriter.h:109
units
png_infop png_charp png_int_32 png_int_32 int int png_charp * units
Definition: png.h:2330
ColladaWriter::_doc
daeDocument * _doc
Definition: ColladaWriter.h:1457
int
typedef int
Definition: png.h:1111
ColladaWriter::Write
virtual bool Write(BodyInfo_impl *bodyInfo)
Definition: ColladaWriter.h:240
hrp::dmatrix
Eigen::MatrixXd dmatrix
Definition: EigenTypes.h:13
ColladaWriter::WriteActuator
virtual daeElementRef WriteActuator(const LinkInfo &plink, const string &parentid)
Definition: ColladaWriter.h:1150
ColladaWriter::_WriteRobot
virtual boost::shared_ptr< instance_articulated_system_output > _WriteRobot(BodyInfo_impl *bodyInfo)
Write kinematic body in a given scene.
Definition: ColladaWriter.h:251
ColladaWriter::LINKOUTPUT::listusedlinks
list< pair< int, std::string > > listusedlinks
Definition: ColladaWriter.h:75
ColladaWriter::_GetRobotId
virtual int _GetRobotId(BodyInfo_impl *bodyInfo)
Definition: ColladaWriter.h:1424
ColladaWriter::_listmanipulators
std::list< ManipulatorInfo > _listmanipulators
Definition: ColladaWriter.h:1474
ColladaWriter::instance_articulated_system_output
Definition: ColladaWriter.h:120
ColladaWriter::instance_kinematics_model_output
Definition: ColladaWriter.h:112
ColladaWriter::_AddKinematics_model
virtual void _AddKinematics_model(BodyInfo_impl *bodyInfo, boost::shared_ptr< kinematics_model_output > kmout)
Definition: ColladaWriter.h:1368
ColladaWriter::_globalscene
domCOLLADA::domSceneRef _globalscene
Definition: ColladaWriter.h:1458
ColladaWriter::kinematics_model_output::vlinksids
std::vector< std::string > vlinksids
same ordering as the link indices
Definition: ColladaWriter.h:101
ManipulatorInfo::effectorname
std::string effectorname
Definition: ColladaWriter.h:47
ColladaWriter::kinbody_models::xmlfilename
std::string xmlfilename
Definition: ColladaWriter.h:137
ColladaWriter::_WriteInstance_kinematics_model
virtual boost::shared_ptr< instance_kinematics_model_output > _WriteInstance_kinematics_model(BodyInfo_impl *bodyInfo, daeElementRef parent, const string &sidscope)
Write kinematic body in a given scene.
Definition: ColladaWriter.h:472
ColladaWriter::_kinematicsModelsLib
domLibrary_kinematics_modelsRef _kinematicsModelsLib
Definition: ColladaWriter.h:1461
ColladaWriter::_GetLinkSid
virtual std::string _GetLinkSid(int ilink)
Definition: ColladaWriter.h:1434
name
png_infop png_charpp name
Definition: png.h:2379
ColladaWriter::instance_physics_model_output
Definition: ColladaWriter.h:129
ColladaUtil::PoseMultVector
void PoseMultVector(OpenHRP::DblArray3 &vnew, const OpenHRP::DblArray12 &m, const OpenHRP::DblArray3 &v)
Definition: ColladaUtil.h:101
ColladaWriter::kinbody_models::kmout
boost::shared_ptr< kinematics_model_output > kmout
Definition: ColladaWriter.h:138
ColladaWriter::WriteGeometry
virtual domGeometryRef WriteGeometry(BodyInfo_impl *bodyInfo, const ShapeInfo &shapeInfo, const DblArray12 &transformMatrix, const string &parentid)
Write geometry properties.
Definition: ColladaWriter.h:806
ColladaWriter::kinematics_model_output::axis_output::sid
string sid
Definition: ColladaWriter.h:93
ColladaWriter::_dom
domCOLLADA * _dom
Definition: ColladaWriter.h:1456
ColladaWriter::ComparePair
static bool ComparePair(const std::pair< int, int > &p0, const std::pair< int, int > &p1)
Definition: ColladaWriter.h:57
ColladaWriter::kinbody_models::pmout
boost::shared_ptr< physics_model_output > pmout
Definition: ColladaWriter.h:139
BodyInfo_impl::links
virtual LinkInfoSequence * links()
Definition: BodyInfo_impl.cpp:80
ColladaWriter::_CreateScene
virtual void _CreateScene()
save all the loaded scene models and their current state.
Definition: ColladaWriter.h:1177
ColladaWriter::kinematics_model_output::axis_output
Definition: ColladaWriter.h:88
ColladaWriter::SCENE::kscene
domKinematics_sceneRef kscene
Definition: ColladaWriter.h:66
ManipulatorInfo
Definition: ColladaWriter.h:40
ColladaUtil::COLLADALOG_WARN
void COLLADALOG_WARN(const std::string &s)
Definition: ColladaUtil.h:90
ColladaUtil.h
ManipulatorInfo::rotation
DblArray4 rotation
Definition: ColladaWriter.h:51
ColladaUtil::QuatFromMatrix
void QuatFromMatrix(OpenHRP::DblArray4 &quat, const T &mat)
Definition: ColladaUtil.h:151
ManipulatorInfo::translation
DblArray3 translation
Definition: ColladaWriter.h:52
ColladaWriter::instance_kinematics_model_output::vaxissids
std::vector< axis_sids > vaxissids
Definition: ColladaWriter.h:115
ColladaUtil
Definition: ColladaUtil.h:76
ColladaWriter::kinematics_model_output::axis_output::axis_output
axis_output()
Definition: ColladaWriter.h:91
ColladaWriter::Save
virtual void Save(const string &filename)
Write down a COLLADA file.
Definition: ColladaWriter.h:228
ColladaWriter::kinematics_model_output::kmodel
domKinematics_modelRef kmodel
Definition: ColladaWriter.h:99
ColladaWriter::_actuatorsLib
domTechniqueRef _actuatorsLib
custom actuators library
Definition: ColladaWriter.h:1470
ColladaUtil::PoseFromAxisAngleTranslation
void PoseFromAxisAngleTranslation(OpenHRP::DblArray12 &pose, const OpenHRP::DblArray4 &rotation, const OpenHRP::DblArray3 &translation)
Definition: ColladaUtil.h:265
error
void error(char *msg) const
Definition: minigzip.c:87
BodyInfo_impl
Definition: BodyInfo_impl.h:29
ColladaWriter::instance_articulated_system_output::ias
domInstance_articulated_systemRef ias
Definition: ColladaWriter.h:122
hrp::det
double det(const dmatrix &_a)
Definition: MatrixSolvers.cpp:368
ColladaWriter::axis_sids::axis_sids
axis_sids(const string &axissid, const string &valuesid, const string &jointnodesid)
Definition: ColladaWriter.h:107
ColladaWriter::_physicsModelsLib
domLibrary_physics_modelsRef _physicsModelsLib
Definition: ColladaWriter.h:1464
ColladaWriter::_GetGeometryId
virtual std::string _GetGeometryId(BodyInfo_impl *bodyInfo, int ilink, int igeom)
Definition: ColladaWriter.h:1438
ColladaWriter::LINKOUTPUT::pnode
domNodeRef pnode
Definition: ColladaWriter.h:77
ColladaWriter::instance_physics_model_output::pmout
boost::shared_ptr< physics_model_output > pmout
Definition: ColladaWriter.h:132
ColladaWriter::physics_model_output::vrigidbodysids
std::vector< std::string > vrigidbodysids
same ordering as the physics indices
Definition: ColladaWriter.h:83
ColladaWriter::WritePhysics_model
virtual boost::shared_ptr< physics_model_output > WritePhysics_model(BodyInfo_impl *bodyInfo)
Definition: ColladaWriter.h:705
BodyInfo_impl::url
virtual char * url()
Definition: BodyInfo_impl.cpp:68
ColladaWriter::_GetPhysics_model
virtual boost::shared_ptr< physics_model_output > _GetPhysics_model(BodyInfo_impl *bodyInfo)
Definition: ColladaWriter.h:1415
height
png_infop png_uint_32 png_uint_32 * height
Definition: png.h:2306
ColladaWriter::ColladaWriter
ColladaWriter(const std::list< ManipulatorInfo > &listmanipulators, const char *comment_str)
Definition: ColladaWriter.h:142
ColladaWriter::handleWarning
virtual void handleWarning(daeString msg)
Definition: ColladaWriter.h:1450
ColladaWriter::instance_kinematics_model_output::vkinematicsbindings
std::vector< std::pair< std::string, std::string > > vkinematicsbindings
Definition: ColladaWriter.h:117
ManipulatorInfo::ManipulatorInfo
ManipulatorInfo()
Definition: ColladaWriter.h:42
ColladaWriter::instance_kinematics_model_output::kmout
boost::shared_ptr< kinematics_model_output > kmout
Definition: ColladaWriter.h:116
ColladaWriter::_GetNodeId
virtual std::string _GetNodeId(BodyInfo_impl *bodyInfo)
Definition: ColladaWriter.h:1427
ShapeSetInfo_impl::materials
virtual MaterialInfoSequence * materials()
Definition: ShapeSetInfo_impl.cpp:99
ColladaWriter
Definition: ColladaWriter.h:55
ColladaWriter::_WriteBindingsInstance_kinematics_scene
void _WriteBindingsInstance_kinematics_scene(domInstance_kinematics_sceneRef ikscene, BodyInfo_impl *bodyInfo, const std::vector< axis_sids > &vaxissids, const std::vector< std::pair< std::string, std::string > > &vkinematicsbindings)
Definition: ColladaWriter.h:1345
ColladaWriter::kinematics_model_output::axis_output::nodesid
string nodesid
Definition: ColladaWriter.h:94
ColladaWriter::WriteKinematics_model
virtual boost::shared_ptr< kinematics_model_output > WriteKinematics_model(BodyInfo_impl *bodyInfo)
Definition: ColladaWriter.h:553
ColladaWriter::_GetKinematics_model
virtual boost::shared_ptr< kinematics_model_output > _GetKinematics_model(BodyInfo_impl *bodyInfo)
Definition: ColladaWriter.h:1387
ColladaWriter::_imagesLib
domLibrary_imagesRef _imagesLib
Definition: ColladaWriter.h:1467
ColladaWriter::_effectsLib
domLibrary_effectsRef _effectsLib
Definition: ColladaWriter.h:1466
ColladaWriter::SCENE::vscene
domVisual_sceneRef vscene
Definition: ColladaWriter.h:65
ColladaWriter::_scene
SCENE _scene
Definition: ColladaWriter.h:1472
ColladaWriter::_AddPhysics_model
virtual void _AddPhysics_model(BodyInfo_impl *bodyInfo, boost::shared_ptr< physics_model_output > pmout)
Definition: ColladaWriter.h:1396
ColladaWriter::~ColladaWriter
virtual ~ColladaWriter()
Definition: ColladaWriter.h:221


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:02