BodyInfoCollada_impl.cpp
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.
21 #include "ColladaUtil.h"
22 #include "BodyInfoCollada_impl.h"
23 #include "hrpUtil/UrlUtil.h"
24 #include "hrpCorba/ViewSimulator.hh"
25 
26 #include <sys/stat.h> // for checkInlineFileUpdateTime
27 
28 #include <boost/typeof/typeof.hpp>
29 #define FOREACH(it, v) for(BOOST_TYPEOF((v).begin()) it = (v).begin(); it != (v).end(); (it)++)
30 
31 using namespace std;
32 using namespace ColladaUtil;
33 
34 namespace {
35  typedef map<string, string> SensorTypeMap;
36  SensorTypeMap sensorTypeMap;
37 }
38 
39 void operator<<(std::ostream& os, const OpenHRP::DblArray12& ttemp) {
40  OpenHRP::DblArray4 quat;
41  QuatFromMatrix(quat, ttemp);
42  os << ttemp[3] << " " << ttemp[7] << " " << ttemp[11] << " / ";
43  os << quat[0] << " " << quat[1] << " " << quat[2] << " " << quat[3];
44 }
45 
46 #define printArrayWARN(name, ary) \
47  COLLADALOG_WARN(str(boost::format(name": %f\t%f\t%f\t%f")%ary[0]%ary[1]%ary[2]%ary[3])); \
48  COLLADALOG_WARN(str(boost::format(name": %f\t%f\t%f\t%f")%ary[4]%ary[5]%ary[6]%ary[7])); \
49  COLLADALOG_WARN(str(boost::format(name": %f\t%f\t%f\t%f")%ary[8]%ary[9]%ary[10]%ary[11]));
50 #define printArrayDEBUG(name, ary) \
51  COLLADALOG_DEBUG(str(boost::format(name": %f\t%f\t%f\t%f")%ary[0]%ary[1]%ary[2]%ary[3])); \
52  COLLADALOG_DEBUG(str(boost::format(name": %f\t%f\t%f\t%f")%ary[4]%ary[5]%ary[6]%ary[7])); \
53  COLLADALOG_DEBUG(str(boost::format(name": %f\t%f\t%f\t%f")%ary[8]%ary[9]%ary[10]%ary[11]));
54 #define printArray printArrayDEBUG
55 class ColladaReader : public daeErrorHandler
59 {
61  {
62  public:
63  JointAxisBinding(daeElementRef pvisualtrans, domAxis_constraintRef pkinematicaxis, domCommon_float_or_paramRef jointvalue, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info) : pvisualtrans(pvisualtrans), pkinematicaxis(pkinematicaxis), jointvalue(jointvalue), kinematics_axis_info(kinematics_axis_info), motion_axis_info(motion_axis_info) {
64  COLLADA_ASSERT( !!pkinematicaxis );
65  daeElement* pae = pvisualtrans->getParentElement();
66  while (!!pae) {
67  visualnode = daeSafeCast<domNode> (pae);
68  if (!!visualnode) {
69  break;
70  }
71  pae = pae->getParentElement();
72  }
73 
74  if (!visualnode) {
75  COLLADALOG_WARN(str(boost::format("couldn't find parent node of element id %s, sid %s")%pkinematicaxis->getID()%pkinematicaxis->getSid()));
76  }
77  }
78 
79  daeElementRef pvisualtrans;
80  domAxis_constraintRef pkinematicaxis;
81  domCommon_float_or_paramRef jointvalue;
82  domNodeRef visualnode;
83  domKinematics_axis_infoRef kinematics_axis_info;
84  domMotion_axis_infoRef motion_axis_info;
85  };
86 
88  {
89  public:
90  domNodeRef node;
91  domLinkRef domlink;
92  domInstance_rigid_bodyRef irigidbody;
93  domRigid_bodyRef rigidbody;
94  domNodeRef nodephysicsoffset;
95  };
96 
98  {
99  public:
100  domNodeRef node;
101  domInstance_rigid_constraintRef irigidconstraint;
102  domRigid_constraintRef rigidconstraint;
103  };
104 
107  {
108  public:
109  std::list< std::pair<domNodeRef,domInstance_kinematics_modelRef> > listKinematicsVisualBindings;
110  std::list<JointAxisBinding> listAxisBindings;
111  std::list<LinkBinding> listLinkBindings;
112  std::list<ConstraintBinding> listConstraintBindings;
113 
114  bool AddAxisInfo(const domInstance_kinematics_model_Array& arr, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info)
115  {
116  if( !kinematics_axis_info ) {
117  return false;
118  }
119  for(size_t ik = 0; ik < arr.getCount(); ++ik) {
120  daeElement* pelt = daeSidRef(kinematics_axis_info->getAxis(), arr[ik]->getUrl().getElement()).resolve().elt;
121  if( !!pelt ) {
122  // look for the correct placement
123  bool bfound = false;
124  for(std::list<JointAxisBinding>::iterator itbinding = listAxisBindings.begin(); itbinding != listAxisBindings.end(); ++itbinding) {
125  if( itbinding->pkinematicaxis.cast() == pelt ) {
126  itbinding->kinematics_axis_info = kinematics_axis_info;
127  if( !!motion_axis_info ) {
128  itbinding->motion_axis_info = motion_axis_info;
129  }
130  bfound = true;
131  break;
132  }
133  }
134  if( !bfound ) {
135  COLLADALOG_WARN(str(boost::format("could not find binding for axis: %s, %s")%kinematics_axis_info->getAxis()%pelt->getAttribute("sid")));
136  return false;
137  }
138  return true;
139  }
140  }
141  COLLADALOG_WARN(str(boost::format("could not find kinematics axis target: %s")%kinematics_axis_info->getAxis()));
142  return false;
143  }
144  };
145 
146  public:
147  ColladaReader() : _dom(NULL), _nGlobalSensorId(0), _nGlobalActuatorId(0), _nGlobalManipulatorId(0), _nGlobalIndex(0) {
148  daeErrorHandler::setErrorHandler(this);
149  _bOpeningZAE = false;
150  }
151  virtual ~ColladaReader() {
152  _dae.reset();
153  //DAE::cleanup();
154  }
155 
156  bool InitFromURL(const string& url)
157  {
158  string filename( deleteURLScheme( url ) );
159 
160  // URL文字列の' \' 区切り子を'/' に置き換え Windows ファイルパス対応
161  string url2;
162  url2 = filename;
163  for(size_t i = 0; i < url2.size(); ++i) {
164  if( url2[i] == '\\' ) {
165  url2[i] = '/';
166  }
167  }
168  filename = url2;
169 
170  COLLADALOG_VERBOSE(str(boost::format("init COLLADA reader version: %s, namespace: %s, filename: %s")%COLLADA_VERSION%COLLADA_NAMESPACE%filename));
171 
172  _dae.reset(new DAE);
173  _bOpeningZAE = filename.find(".zae") == filename.size()-4;
174  _dom = daeSafeCast<domCOLLADA>(_dae->open(filename));
175  _bOpeningZAE = false;
176  if (!_dom) {
177  return false;
178  }
179  _filename=filename;
180  return _Init();
181  }
182 
183  bool InitFromData(const string& pdata)
184  {
185  COLLADALOG_DEBUG(str(boost::format("init COLLADA reader version: %s, namespace: %s")%COLLADA_VERSION%COLLADA_NAMESPACE));
186  _dae.reset(new DAE);
187  _dom = daeSafeCast<domCOLLADA>(_dae->openFromMemory(".",pdata.c_str()));
188  if (!_dom) {
189  return false;
190  }
191  return _Init();
192  }
193 
194  bool _Init()
195  {
196  _fGlobalScale = 1;
197  if( !!_dom->getAsset() ) {
198  if( !!_dom->getAsset()->getUnit() ) {
199  _fGlobalScale = _dom->getAsset()->getUnit()->getMeter();
200  }
201  }
202  return true;
203  }
204 
206  {
207  _mapJointUnits.clear();
208  _mapJointIds.clear();
209  _mapLinkNames.clear();
210  _veclinks.clear();
211  _veclinknames.clear();
212  }
213 
216  {
217  std::list< pair<domInstance_kinematics_modelRef, boost::shared_ptr<KinematicsSceneBindings> > > listPossibleBodies;
218  domCOLLADA::domSceneRef allscene = _dom->getScene();
219  if( !allscene ) {
220  return false;
221  }
222 
223  // fill asset info into robot->info_
224  //"Step 2 of the Project S"
225  //"Author : Hajime Saito, General Robotix, Inc."
226  //"Date : 2005.11.01"
227  //"Modified: 2007.09.29"
228  COLLADALOG_WARN("fill asset info");
229  _ResetRobotCache();
230 
231  // parse each instance kinematics scene, prioritize robots
232  for (size_t iscene = 0; iscene < allscene->getInstance_kinematics_scene_array().getCount(); iscene++) {
233  domInstance_kinematics_sceneRef kiscene = allscene->getInstance_kinematics_scene_array()[iscene];
234  domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
235  if (!kscene) {
236  continue;
237  }
238  boost::shared_ptr<KinematicsSceneBindings> bindings(new KinematicsSceneBindings());
239  _ExtractKinematicsVisualBindings(allscene->getInstance_visual_scene(),kiscene,*bindings);
240  _ExtractPhysicsBindings(allscene,*bindings);
241  for(size_t ias = 0; ias < kscene->getInstance_articulated_system_array().getCount(); ++ias) {
242  if( ExtractArticulatedSystem(probot, kscene->getInstance_articulated_system_array()[ias], *bindings) && !!probot ) {
243  PostProcess(probot);
244  return true;
245  }
246  }
247  for(size_t ikmodel = 0; ikmodel < kscene->getInstance_kinematics_model_array().getCount(); ++ikmodel) {
248  listPossibleBodies.push_back(make_pair(kscene->getInstance_kinematics_model_array()[ikmodel], bindings));
249  }
250  }
251 
252  for(std::list< pair<domInstance_kinematics_modelRef, boost::shared_ptr<KinematicsSceneBindings> > >::iterator it = listPossibleBodies.begin(); it != listPossibleBodies.end(); ++it) {
253  if( ExtractKinematicsModel(probot, it->first, *it->second) ) {
254  PostProcess(probot);
255  return true;
256  }
257  }
258 
259 
260  // add left-over visual objects
261  if (!!allscene->getInstance_visual_scene()) {
262  domVisual_sceneRef viscene = daeSafeCast<domVisual_scene>(allscene->getInstance_visual_scene()->getUrl().getElement().cast());
263  for (size_t node = 0; node < viscene->getNode_array().getCount(); node++) {
264  boost::shared_ptr<KinematicsSceneBindings> bindings(new KinematicsSceneBindings());
265  if ( ExtractKinematicsModel(probot, viscene->getNode_array()[node],*bindings,std::vector<std::string>()) ) {
266  PostProcess(probot);
267  return true;
268  }
269  }
270  }
271 
272  return false;
273  }
274 
276  {
277  probot->links_.length(_veclinks.size());
278  probot->linkShapeIndices_.length(_veclinks.size());
279  for(size_t i = 0; i < _veclinks.size(); ++i) {
280  probot->links_[i] = *_veclinks[i];
281  probot->linkShapeIndices_[i] = probot->links_[i].shapeIndices;
282 
283  //
284  // segments
285  int numSegment = 1;
286  probot->links_[i].segments.length(numSegment);
287  for ( int s = 0; s < numSegment; ++s ) {
288  SegmentInfo_var segmentInfo(new SegmentInfo());
289  Matrix44 T;
290  hrp::calcRodrigues(T, Vector3(0,0,1), 0.0);
291  int p = 0;
292  for(int row=0; row < 3; ++row){
293  for(int col=0; col < 4; ++col){
294  segmentInfo->transformMatrix[p++] = T(row, col);
295  }
296  }
297  segmentInfo->name = _veclinknames[i].c_str();
298  segmentInfo->shapeIndices.length(probot->links_[i].shapeIndices.length());
299  for(unsigned int j = 0; j < probot->links_[i].shapeIndices.length(); j++ ) {
300  segmentInfo->shapeIndices[j] = j;
301  }
302  segmentInfo->mass = probot->links_[i].mass;
303  segmentInfo->centerOfMass[0] = probot->links_[i].centerOfMass[0];
304  segmentInfo->centerOfMass[1] = probot->links_[i].centerOfMass[1];
305  segmentInfo->centerOfMass[2] = probot->links_[i].centerOfMass[2];
306  for(int j = 0; j < 9 ; j++ ) {
307  segmentInfo->inertia[j] = probot->links_[i].inertia[j];
308  }
309  probot->links_[i].segments[s] = segmentInfo;
310  }
311  }
312  //applyTriangleMeshShaper(modelNodeSet.humanoidNode());
313 
314  // build coldetModels
315  probot->linkColdetModels.resize(_veclinks.size());
316  for(size_t linkIndex = 0; linkIndex < _veclinks.size(); ++linkIndex) {
317  ColdetModelPtr coldetModel(new ColdetModel());
318  coldetModel->setName(std::string(probot->links_[linkIndex].name));
319  int vertexIndex = 0;
320  int triangleIndex = 0;
321 
322  Matrix44 E(Matrix44::Identity());
323  const TransformedShapeIndexSequence& shapeIndices = probot->linkShapeIndices_[linkIndex];
324  probot->setColdetModel(coldetModel, shapeIndices, E, vertexIndex, triangleIndex);
325 
326  Matrix44 T(Matrix44::Identity());
327  const SensorInfoSequence& sensors = probot->links_[linkIndex].sensors;
328  for (unsigned int i=0; i<sensors.length(); i++){
329  const SensorInfo& sensor = sensors[i];
330  calcRodrigues(T, Vector3(sensor.rotation[0], sensor.rotation[1],
331  sensor.rotation[2]), sensor.rotation[3]);
332  T(0,3) = sensor.translation[0];
333  T(1,3) = sensor.translation[1];
334  T(2,3) = sensor.translation[2];
335  const TransformedShapeIndexSequence& sensorShapeIndices = sensor.shapeIndices;
336  probot->setColdetModel(coldetModel, sensorShapeIndices, T, vertexIndex, triangleIndex);
337  }
338  if(triangleIndex>0) {
339  coldetModel->build();
340  }
341  probot->linkColdetModels[linkIndex] = coldetModel;
342  probot->links_[linkIndex].AABBmaxDepth = coldetModel->getAABBTreeDepth();
343  probot->links_[linkIndex].AABBmaxNum = coldetModel->getAABBmaxNum();
344  }
345  }
346 
349  bool ExtractArticulatedSystem(BodyInfoCollada_impl*& probot, domInstance_articulated_systemRef ias, KinematicsSceneBindings& bindings)
350  {
351  if( !ias ) {
352  return false;
353  }
354  //COLLADALOG_DEBUG(str(boost::format("instance articulated system sid %s")%ias->getSid()));
355  domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system> (ias->getUrl().getElement().cast());
356  if( !articulated_system ) {
357  return false;
358  }
359  if( !probot ) {
360  boost::shared_ptr<std::string> pinterface_type = _ExtractInterfaceType(ias->getExtra_array());
361  if( !pinterface_type ) {
362  pinterface_type = _ExtractInterfaceType(articulated_system->getExtra_array());
363  }
364  if( !!pinterface_type ) {
365  COLLADALOG_WARN(str(boost::format("need to create a robot with type %s")%*(pinterface_type)));
366  }
367  _ResetRobotCache();
368  return false;
369  }
370  if( probot->url_.size() == 0 ) {
371  probot->url_ = _filename;
372  }
373  if( probot->name_.size() == 0 && !!ias->getName() ) {
374  probot->name_ = CORBA::string_dup(ias->getName());
375  }
376  if( probot->name_.size() == 0 && !!ias->getSid()) {
377  probot->name_ = CORBA::string_dup(ias->getSid());
378  }
379  if( probot->name_.size() == 0 && !!articulated_system->getName() ) {
380  probot->name_ = CORBA::string_dup(articulated_system->getName());
381  }
382  if( probot->name_.size() == 0 && !!articulated_system->getId()) {
383  probot->name_ = CORBA::string_dup(articulated_system->getId());
384  }
385 
386  if( !!articulated_system->getMotion() ) {
387  domInstance_articulated_systemRef ias_new = articulated_system->getMotion()->getInstance_articulated_system();
388  if( !!articulated_system->getMotion()->getTechnique_common() ) {
389  for(size_t i = 0; i < articulated_system->getMotion()->getTechnique_common()->getAxis_info_array().getCount(); ++i) {
390  domMotion_axis_infoRef motion_axis_info = articulated_system->getMotion()->getTechnique_common()->getAxis_info_array()[i];
391  // this should point to a kinematics axis_info
392  domKinematics_axis_infoRef kinematics_axis_info = daeSafeCast<domKinematics_axis_info>(daeSidRef(motion_axis_info->getAxis(), ias_new->getUrl().getElement()).resolve().elt);
393  if( !!kinematics_axis_info ) {
394  // find the parent kinematics and go through all its instance kinematics models
395  daeElement* pparent = kinematics_axis_info->getParent();
396  while(!!pparent && pparent->typeID() != domKinematics::ID()) {
397  pparent = pparent->getParent();
398  }
399  COLLADA_ASSERT(!!pparent);
400  bindings.AddAxisInfo(daeSafeCast<domKinematics>(pparent)->getInstance_kinematics_model_array(), kinematics_axis_info, motion_axis_info);
401  }
402  else {
403  COLLADALOG_WARN(str(boost::format("failed to find kinematics axis %s")%motion_axis_info->getAxis()));
404  }
405  }
406  }
407  if( !ExtractArticulatedSystem(probot,ias_new,bindings) ) {
408  return false;
409  }
410  }
411  else {
412  if( !articulated_system->getKinematics() ) {
413  COLLADALOG_WARN(str(boost::format("collada <kinematics> tag empty? instance_articulated_system=%s")%ias->getID()));
414  return true;
415  }
416 
417  if( !!articulated_system->getKinematics()->getTechnique_common() ) {
418  for(size_t i = 0; i < articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array().getCount(); ++i) {
419  bindings.AddAxisInfo(articulated_system->getKinematics()->getInstance_kinematics_model_array(), articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array()[i], NULL);
420  }
421  }
422 
423  // parse the kinematics information
424  if (!probot) {
425  // create generic robot?
426  return false;
427  }
428 
429  for(size_t ik = 0; ik < articulated_system->getKinematics()->getInstance_kinematics_model_array().getCount(); ++ik) {
430  ExtractKinematicsModel(probot,articulated_system->getKinematics()->getInstance_kinematics_model_array()[ik],bindings);
431  }
432  }
433 
434  ExtractRobotManipulators(probot, articulated_system);
435  ExtractRobotAttachedSensors(probot, articulated_system);
436  ExtractRobotAttachedActuators(probot, articulated_system);
437  return true;
438  }
439 
440  bool ExtractKinematicsModel(BodyInfoCollada_impl* pkinbody, domInstance_kinematics_modelRef ikm, KinematicsSceneBindings& bindings)
441  {
442  if( !ikm ) {
443  return false;
444  }
445  COLLADALOG_DEBUG(str(boost::format("instance kinematics model sid %s")%ikm->getSid()));
446  domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model> (ikm->getUrl().getElement().cast());
447  if (!kmodel) {
448  COLLADALOG_WARN(str(boost::format("%s does not reference valid kinematics")%ikm->getSid()));
449  return false;
450  }
451  domPhysics_modelRef pmodel;
452  if( !pkinbody ) {
453  boost::shared_ptr<std::string> pinterface_type = _ExtractInterfaceType(ikm->getExtra_array());
454  if( !pinterface_type ) {
455  pinterface_type = _ExtractInterfaceType(kmodel->getExtra_array());
456  }
457  if( !!pinterface_type ) {
458  COLLADALOG_WARN(str(boost::format("need to create a robot with type %s")%*pinterface_type));
459  }
460  _ResetRobotCache();
461  return false;
462  }
463  if( pkinbody->url_.size() == 0 ) {
464  pkinbody->url_ = _filename;
465  }
466 
467  // find matching visual node
468  domNodeRef pvisualnode;
469  for(std::list< std::pair<domNodeRef,domInstance_kinematics_modelRef> >::iterator it = bindings.listKinematicsVisualBindings.begin(); it != bindings.listKinematicsVisualBindings.end(); ++it) {
470  if( it->second == ikm ) {
471  pvisualnode = it->first;
472  break;
473  }
474  }
475  if( !pvisualnode ) {
476  COLLADALOG_WARN(str(boost::format("failed to find visual node for instance kinematics model %s")%ikm->getSid()));
477  return false;
478  }
479 
480  if( pkinbody->name_.size() == 0 && !!ikm->getName() ) {
481  pkinbody->name_ = CORBA::string_dup(ikm->getName());
482  }
483  if( pkinbody->name_.size() == 0 && !!ikm->getID() ) {
484  pkinbody->name_ = CORBA::string_dup(ikm->getID());
485  }
486 
487  if (!ExtractKinematicsModel(pkinbody, kmodel, pvisualnode, pmodel, bindings)) {
488  COLLADALOG_WARN(str(boost::format("failed to load kinbody from kinematics model %s")%kmodel->getID()));
489  return false;
490  }
491  return true;
492  }
493 
495  bool ExtractKinematicsModel(BodyInfoCollada_impl* pkinbody, domNodeRef pdomnode, const KinematicsSceneBindings& bindings, const std::vector<std::string>& vprocessednodes)
496  {
497  if( !!pdomnode->getID() && find(vprocessednodes.begin(),vprocessednodes.end(),pdomnode->getID()) != vprocessednodes.end() ) {
498  COLLADALOG_WARN(str(boost::format("could not create kinematics type pnode getid %s")%pdomnode->getID()));
499  return false;
500  }
501  _ResetRobotCache();
502  string name = !pdomnode->getName() ? "" : _ConvertToValidName(pdomnode->getName());
503  if( name.size() == 0 ) {
504  name = _ConvertToValidName(pdomnode->getID());
505  }
506  boost::shared_ptr<LinkInfo> plink(new LinkInfo());
507  _veclinks.push_back(plink);
508  plink->jointId = -1;
509  plink->jointType = CORBA::string_dup("fixed");
510  plink->parentIndex = -1;
511  plink->name = CORBA::string_dup(name.c_str());
512  DblArray12 tlink; PoseIdentity(tlink);
513 
514  // Gets the geometry
515  bool bhasgeometry = ExtractGeometry(pkinbody,plink,tlink,pdomnode,bindings.listAxisBindings,vprocessednodes);
516  if( !bhasgeometry ) {
517  COLLADALOG_WARN(str(boost::format("could not extract geometry %s")%name));
518  return false;
519  }
520 
521  COLLADALOG_INFO(str(boost::format("Loading non-kinematics node '%s'")%name));
522  pkinbody->name_ = name;
523  return pkinbody;
524  }
525 
527  bool ExtractKinematicsModel(BodyInfoCollada_impl* pkinbody, domKinematics_modelRef kmodel, domNodeRef pnode, domPhysics_modelRef pmodel, const KinematicsSceneBindings bindings)
528  {
529  vector<domJointRef> vdomjoints;
530  if (!pkinbody) {
531  _ResetRobotCache();
532  }
533  if( pkinbody->name_.size() == 0 && !!kmodel->getName() ) {
534  pkinbody->name_ = CORBA::string_dup(kmodel->getName());
535  }
536  if( pkinbody->name_.size() == 0 && !!kmodel->getID() ) {
537  pkinbody->name_ = CORBA::string_dup(kmodel->getID());
538  }
539  COLLADALOG_DEBUG(str(boost::format("kinematics model: %s")%pkinbody->name_));
540  if( !!pnode ) {
541  COLLADALOG_DEBUG(str(boost::format("node name: %s")%pnode->getId()));
542  }
543  if( !kmodel->getID() ) {
544  COLLADALOG_DEBUG(str(boost::format("kinematics model: %s has no id attribute!")%pkinbody->name_));
545  }
546 
547  // Process joint of the kinbody
548  domKinematics_model_techniqueRef ktec = kmodel->getTechnique_common();
549 
550  // Store joints
551  for (size_t ijoint = 0; ijoint < ktec->getJoint_array().getCount(); ++ijoint) {
552  vdomjoints.push_back(ktec->getJoint_array()[ijoint]);
553  }
554 
555  // Store instances of joints
556  for (size_t ijoint = 0; ijoint < ktec->getInstance_joint_array().getCount(); ++ijoint) {
557  domJointRef pelt = daeSafeCast<domJoint> (ktec->getInstance_joint_array()[ijoint]->getUrl().getElement());
558  if (!pelt) {
559  COLLADALOG_WARN("failed to get joint from instance");
560  }
561  else {
562  vdomjoints.push_back(pelt);
563  }
564  }
565 
566  COLLADALOG_VERBOSE(str(boost::format("Number of root links in the kmodel %d")%ktec->getLink_array().getCount()));
567  DblArray12 identity;
568  PoseIdentity(identity);
569  for (size_t ilink = 0; ilink < ktec->getLink_array().getCount(); ++ilink) {
570  domLinkRef pdomlink = ktec->getLink_array()[ilink];
571  //
572  _ExtractFullTransform(rootOrigin, pdomlink);
573  printArray("rootOrigin", rootOrigin);
574  domNodeRef pvisualnode;
575  FOREACH(it, bindings.listKinematicsVisualBindings) {
576  if(strcmp(it->first->getName() ,pdomlink->getName()) == 0) {
577  pvisualnode = it->first;
578  break;
579  }
580  }
581  if (!!pvisualnode) {
582  getNodeParentTransform(visualRootOrigin, pvisualnode);
583  printArray("visualRootOrigin", visualRootOrigin);
584  }
585  //
586  DblArray12 identity;
587  PoseIdentity(identity);
588  int linkindex = ExtractLink(pkinbody, pdomlink, ilink == 0 ? pnode : domNodeRef(),
589  identity, identity, vdomjoints, bindings);
590 
591  boost::shared_ptr<LinkInfo> plink = _veclinks.at(linkindex);
592  AxisAngleTranslationFromPose(plink->rotation,plink->translation,rootOrigin);
593  }
594 
595  for (size_t iform = 0; iform < ktec->getFormula_array().getCount(); ++iform) {
596  domFormulaRef pf = ktec->getFormula_array()[iform];
597  if (!pf->getTarget()) {
598  COLLADALOG_WARN("formula target not valid");
599  continue;
600  }
601 
602  // find the target joint
603  boost::shared_ptr<LinkInfo> pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf,pkinbody).first;
604  if (!pjoint) {
605  continue;
606  }
607 
608  int iaxis = 0;
609  dReal ftargetunit = 1;
610  if(_mapJointUnits.find(pjoint) != _mapJointUnits.end() ) {
611  ftargetunit = _mapJointUnits[pjoint].at(iaxis);
612  }
613 
614  daeTArray<daeElementRef> children;
615  pf->getTechnique_common()->getChildren(children);
616 
617  domTechniqueRef popenravetec = _ExtractOpenRAVEProfile(pf->getTechnique_array());
618  if( !!popenravetec ) {
619  for(size_t ic = 0; ic < popenravetec->getContents().getCount(); ++ic) {
620  daeElementRef pequation = popenravetec->getContents()[ic];
621  if( pequation->getElementName() == string("equation") ) {
622  if( !pequation->hasAttribute("type") ) {
623  COLLADALOG_WARN("equaiton needs 'type' attribute, ignoring");
624  continue;
625  }
626  if( children.getCount() != 1 ) {
627  COLLADALOG_WARN("equaiton needs exactly one child");
628  continue;
629  }
630  std::string equationtype = pequation->getAttribute("type");
631  boost::shared_ptr<LinkInfo> pjointtarget;
632  if( pequation->hasAttribute("target") ) {
633  pjointtarget = _getJointFromRef(pequation->getAttribute("target").c_str(),pf,pkinbody).first;
634  }
635  try {
636  std::string eq = _ExtractMathML(pf,pkinbody,children[0]);
637  if( ftargetunit != 1 ) {
638  eq = str(boost::format("%f*(%s)")%ftargetunit%eq);
639  }
640  if( equationtype == "position" ) {
641  COLLADALOG_WARN(str(boost::format("cannot set joint %s position equation: %s!")%pjoint->name%eq));
642  }
643  else if( equationtype == "first_partial" ) {
644  if( !pjointtarget ) {
645  COLLADALOG_WARN(str(boost::format("first_partial equation '%s' needs a target attribute! ignoring...")%eq));
646  continue;
647  }
648  COLLADALOG_WARN(str(boost::format("cannot set joint %s first partial equation: d %s=%s!")%pjoint->name%pjointtarget->name%eq));
649  }
650  else if( equationtype == "second_partial" ) {
651  if( !pjointtarget ) {
652  COLLADALOG_WARN(str(boost::format("second_partial equation '%s' needs a target attribute! ignoring...")%eq));
653  continue;
654  }
655  COLLADALOG_WARN(str(boost::format("cannot set joint %s second partial equation: d^2 %s = %s!")%pjoint->name%pjointtarget->name%eq));
656  }
657  else {
658  COLLADALOG_WARN(str(boost::format("unknown equation type %s")%equationtype));
659  }
660  }
661  catch(const ModelLoader::ModelLoaderException& ex) {
662  COLLADALOG_WARN(str(boost::format("failed to parse formula %s for target %s")%equationtype%pjoint->name));
663  }
664  }
665  }
666  }
667  else if (!!pf->getTechnique_common()) {
668  try {
669  for(size_t ic = 0; ic < children.getCount(); ++ic) {
670  string eq = _ExtractMathML(pf,pkinbody,children[ic]);
671  if( ftargetunit != 1 ) {
672  eq = str(boost::format("%f*(%s)")%ftargetunit%eq);
673  }
674  if( eq.size() > 0 ) {
675  COLLADALOG_WARN(str(boost::format("cannot set joint %s position equation: %s!")%pjoint->name%eq));
676  break;
677  }
678  }
679  }
680  catch(const ModelLoader::ModelLoaderException& ex) {
681  COLLADALOG_WARN(str(boost::format("failed to parse formula for target %s: %s")%pjoint->name%ex.description));
682  }
683  }
684  }
685 
686  // read the collision data
687  for(size_t ie = 0; ie < kmodel->getExtra_array().getCount(); ++ie) {
688  domExtraRef pextra = kmodel->getExtra_array()[ie];
689  if( strcmp(pextra->getType(), "collision") == 0 ) {
690  domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
691  if( !!tec ) {
692  for(size_t ic = 0; ic < tec->getContents().getCount(); ++ic) {
693  daeElementRef pelt = tec->getContents()[ic];
694  if( pelt->getElementName() == string("ignore_link_pair") ) {
695  domLinkRef pdomlink0 = daeSafeCast<domLink>(daeSidRef(pelt->getAttribute("link0"), kmodel).resolve().elt);
696  domLinkRef pdomlink1 = daeSafeCast<domLink>(daeSidRef(pelt->getAttribute("link1"), kmodel).resolve().elt);
697  if( !pdomlink0 || !pdomlink1 ) {
698  COLLADALOG_WARN(str(boost::format("failed to reference <ignore_link_pair> links: %s %s")%pelt->getAttribute("link0")%pelt->getAttribute("link1")));
699  continue;
700  }
701  COLLADALOG_INFO(str(boost::format("need to specifying ignore link pair %s:%s")%_ExtractLinkName(pdomlink0)%_ExtractLinkName(pdomlink1)));
702  }
703  else if( pelt->getElementName() == string("bind_instance_geometry") ) {
704  COLLADALOG_WARN("currently do not support bind_instance_geometry");
705  }
706  }
707  }
708  }
709  }
710  return true;
711  }
712 
713  boost::shared_ptr<LinkInfo> GetLink(const std::string& name)
714  {
715  for(std::map<std::string,boost::shared_ptr<LinkInfo> >::iterator it = _mapJointIds.begin(); it != _mapJointIds.end(); ++it) {
716  string linkname(CORBA::String_var(it->second->name));
717  if( linkname == name ) {
718  return it->second;
719  }
720  }
721  return boost::shared_ptr<LinkInfo>();
722  }
723 
725  int ExtractLink(BodyInfoCollada_impl* pkinbody, const domLinkRef pdomlink,const domNodeRef pdomnode, const DblArray12& tParentWorldLink, const DblArray12& tParentLink, const std::vector<domJointRef>& vdomjoints, const KinematicsSceneBindings bindings) {
726  const std::list<JointAxisBinding>& listAxisBindings = bindings.listAxisBindings;
727 
728  // Set link name with the name of the COLLADA's Link
729  std::string linkname;
730  if( !!pdomlink ) {
731  linkname = _ExtractLinkName(pdomlink);
732  if( linkname.size() == 0 ) {
733  COLLADALOG_WARN("<link> has no name or id, falling back to <node>!");
734  }
735  }
736  if( linkname.size() == 0 ) {
737  if( !!pdomnode ) {
738  if (!!pdomnode->getName()) {
739  linkname = _ConvertToValidName(pdomnode->getName());
740  }
741  if( linkname.size() == 0 && !!pdomnode->getID()) {
742  linkname = _ConvertToValidName(pdomnode->getID());
743  }
744  }
745  }
746 
747  boost::shared_ptr<LinkInfo> plink(new LinkInfo());
748  plink->parentIndex = -1;
749  plink->jointId = -1;
750  plink->mass = 1;
751  plink->centerOfMass[0] = plink->centerOfMass[1] = plink->centerOfMass[2] = 0;
752  plink->inertia[0] = plink->inertia[4] = plink->inertia[8] = 1;
753  plink->jointValue = 0;
754  _mapLinkNames[linkname] = plink;
755  plink->name = CORBA::string_dup(linkname.c_str());
756  plink->jointType = CORBA::string_dup("free");
757  int ilinkindex = (int)_veclinks.size();
758  _veclinks.push_back(plink);
759  _veclinknames.push_back(linkname);
760 
761  if( !!pdomnode ) {
762  COLLADALOG_VERBOSE(str(boost::format("Node Id %s and Name %s")%pdomnode->getId()%pdomnode->getName()));
763  }
764 
765  // physics
766  domInstance_rigid_bodyRef irigidbody;
767  domRigid_bodyRef rigidbody;
768  domInstance_rigid_constraintRef irigidconstraint;
769  FOREACH(itlinkbinding, bindings.listLinkBindings) {
770  if( !!pdomnode->getID() && !!itlinkbinding->node->getID() && strcmp(pdomnode->getID(),itlinkbinding->node->getID()) == 0 ) {
771  irigidbody = itlinkbinding->irigidbody;
772  rigidbody = itlinkbinding->rigidbody;
773  }
774  }
775  FOREACH(itconstraintbinding, bindings.listConstraintBindings) {
776  if( !!pdomnode->getID() && !!itconstraintbinding->rigidconstraint->getName() && strcmp(linkname.c_str(),itconstraintbinding->rigidconstraint->getName()) == 0 ) {
777  plink->jointType = CORBA::string_dup("fixed");
778  }
779  }
780 
781  if (!pdomlink) {
782  ExtractGeometry(pkinbody,plink,tParentLink,pdomnode,listAxisBindings,std::vector<std::string>());
783  }
784  else {
785  COLLADALOG_DEBUG(str(boost::format("Attachment link elements: %d")%pdomlink->getAttachment_full_array().getCount()));
786  // use the kinematics coordinate system for each link
787  DblArray12 tlink;
788  _ExtractFullTransform(tlink, pdomlink);
789  {
790  DblArray12 tgeomlink;
791  PoseMult(tgeomlink, tParentWorldLink, tlink);
792  COLLADALOG_DEBUG(str(boost::format("geom_link:%s")%linkname.c_str()));
793  printArray("geom", tgeomlink);
794  ExtractGeometry(pkinbody,plink,tgeomlink,pdomnode,listAxisBindings,std::vector<std::string>());
795  }
796  COLLADALOG_DEBUG(str(boost::format("After ExtractGeometry Attachment link elements: %d")%pdomlink->getAttachment_full_array().getCount()));
797 
798  if( !!rigidbody && !!rigidbody->getTechnique_common() ) {
799  domRigid_body::domTechnique_commonRef rigiddata = rigidbody->getTechnique_common();
800  if( !!rigiddata->getMass() ) {
801  plink->mass = rigiddata->getMass()->getValue();
802  }
803  if( !!rigiddata->getInertia() ) {
804  plink->inertia[0] = rigiddata->getInertia()->getValue()[0];
805  plink->inertia[4] = rigiddata->getInertia()->getValue()[1];
806  plink->inertia[8] = rigiddata->getInertia()->getValue()[2];
807  }
808  if( !!rigiddata->getMass_frame() ) {
809  DblArray12 atemp1,atemp2,atemp3,tlocalmass;
810  printArray("tlink", tlink);
811  printArray("plink", tParentWorldLink);
812  PoseMult(atemp1, tParentWorldLink, tlink);
813  PoseInverse(atemp2, rootOrigin);
814  PoseMult(atemp3, atemp2, atemp1);
815  PoseInverse(atemp1, atemp3);
816  _ExtractFullTransform(atemp2, rigiddata->getMass_frame());
817  PoseMult(tlocalmass, atemp1, atemp2);
818  printArray("i_org", tlocalmass);
819 
820  plink->centerOfMass[0] = tlocalmass[3];
821  plink->centerOfMass[1] = tlocalmass[7];
822  plink->centerOfMass[2] = tlocalmass[11];
823  if( !!rigiddata->getInertia() ) {
824  DblArray12 temp_i, temp_a, m_inertia, m_result;
825  for (int i = 0; i < 12; i++) { m_inertia[i] = 0.0; }
826  m_inertia[4*0 + 0] = plink->inertia[0];
827  m_inertia[4*1 + 1] = plink->inertia[4];
828  m_inertia[4*2 + 2] = plink->inertia[8];
829  tlocalmass[3] = 0; tlocalmass[7] = 0; tlocalmass[11] = 0;
830  PoseInverse(temp_i, tlocalmass);
831  PoseMult(temp_a, tlocalmass, m_inertia);
832  PoseMult(m_result, temp_a, temp_i);
833 
834  for(int i = 0; i < 3; i++) {
835  for(int j = 0; j < 3; j++) {
836  plink->inertia[3*i + j] = m_result[4*i + j];
837  }
838  }
839  }
840  }
841  }
842 
843  // Process all atached links
844  for (size_t iatt = 0; iatt < pdomlink->getAttachment_full_array().getCount(); ++iatt) {
845  domLink::domAttachment_fullRef pattfull = pdomlink->getAttachment_full_array()[iatt];
846 
847  // get link kinematics transformation
848  DblArray12 tatt;
849  _ExtractFullTransform(tatt,pattfull);
850 
851  // Transform applied to the joint
852  // the joint anchor is actually tatt.trans! However, in openhrp3 the link and joint coordinate systems are the same!
853  // this means we need to change the coordinate system of the joint and all the attached geometry
854  //dReal anchor[3] = {tatt[3],tatt[7],tatt[11]};
855  //tatt[3] = 0; tatt[7] = 0; tatt[11] = 0;
856  //COLLADALOG_INFO(str(boost::format("tatt: %f %f %f")%anchor[0]%anchor[1]%anchor[2]));
857 
858  // process attached links
859  daeElement* peltjoint = daeSidRef(pattfull->getJoint(), pattfull).resolve().elt;
860  if( !peltjoint ) {
861  COLLADALOG_WARN(str(boost::format("could not find attached joint %s!")%pattfull->getJoint()));
862  continue;
863  }
864  string jointid;
865  if( string(pattfull->getJoint()).find("./") == 0 ) {
866  jointid = str(boost::format("%s/%s")%_ExtractParentId(pattfull)%&pattfull->getJoint()[1]);
867  }
868  else {
869  jointid = pattfull->getJoint();
870  }
871 
872  domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
873  if (!pdomjoint) {
874  domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
875  if (!!pdomijoint) {
876  pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
877  }
878  else {
879  COLLADALOG_WARN(str(boost::format("could not cast element <%s> to <joint>!")%peltjoint->getElementName()));
880  continue;
881  }
882  }
883 
884  // get direct child link
885  if (!pattfull->getLink()) {
886  COLLADALOG_WARN(str(boost::format("joint %s needs to be attached to a valid link")%jointid));
887  continue;
888  }
889 
890  // find the correct joint in the bindings
891  daeTArray<domAxis_constraintRef> vdomaxes = pdomjoint->getChildrenByType<domAxis_constraint>();
892  domNodeRef pchildnode;
893 
894  // see if joint has a binding to a visual node
895  for(std::list<JointAxisBinding>::const_iterator itaxisbinding = listAxisBindings.begin(); itaxisbinding != listAxisBindings.end(); ++itaxisbinding) {
896  for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
897  // If the binding for the joint axis is found, retrieve the info
898  if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
899  pchildnode = itaxisbinding->visualnode;
900  break;
901  }
902  }
903  if( !!pchildnode ) {
904  break;
905  }
906  }
907  if (!pchildnode) {
908  COLLADALOG_DEBUG(str(boost::format("joint %s has no visual binding")%jointid));
909  }
910 
911 
912  DblArray12 tnewparent;
913  {
914  DblArray12 ttemp;
915  PoseMult(ttemp, tParentWorldLink, tlink);
916  PoseMult(tnewparent, ttemp, tatt);
917  }
918  int ijointindex = ExtractLink(pkinbody, pattfull->getLink(), pchildnode,
919  tnewparent,
920  tatt, vdomjoints, bindings);
921  boost::shared_ptr<LinkInfo> pjoint = _veclinks.at(ijointindex);
922  int cindex = plink->childIndices.length();
923  plink->childIndices.length(cindex+1);
924  plink->childIndices[cindex] = ijointindex;
925  pjoint->parentIndex = ilinkindex;
926 
927  if ( ilinkindex == 0 ){ // due to ExtractKinematicsModel calls ExtractLink with identity
928  memcpy(tnewparent, tatt, sizeof(tatt));
929  } else {
930  DblArray12 ttemp;
931  _ExtractFullTransform(ttemp, pattfull->getLink());
932  PoseMult(tnewparent, tatt, ttemp);
933  }
934  AxisAngleTranslationFromPose(pjoint->rotation,pjoint->translation,tnewparent);
935 
936  bool bActive = true; // if not active, put into the passive list
937 
938  if( vdomaxes.getCount() > 1 ) {
939  COLLADALOG_WARN(str(boost::format("joint %s has %d degrees of freedom, only 1 DOF is supported")%pjoint->name%vdomaxes.getCount()));
940  }
941  else if( vdomaxes.getCount() == 0 ) {
942  continue;
943  }
944 
945  size_t ic = 0;
946  std::vector<dReal> vaxisunits(1,dReal(1));
947  for(std::list<JointAxisBinding>::const_iterator itaxisbinding = listAxisBindings.begin(); itaxisbinding != listAxisBindings.end(); ++itaxisbinding) {
948  if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
949  if( !!itaxisbinding->kinematics_axis_info ) {
950  if( !!itaxisbinding->kinematics_axis_info->getActive() ) {
951  // what if different axes have different active profiles?
952  bActive = resolveBool(itaxisbinding->kinematics_axis_info->getActive(),itaxisbinding->kinematics_axis_info);
953  }
954  }
955  break;
956  }
957  }
958  domAxis_constraintRef pdomaxis = vdomaxes[ic];
959  bool bIsRevolute = false;
960  if( strcmp(pdomaxis->getElementName(), "revolute") == 0 ) {
961  pjoint->jointType = CORBA::string_dup("rotate");
962  bIsRevolute = true;
963  }
964  else if( strcmp(pdomaxis->getElementName(), "prismatic") == 0 ) {
965  pjoint->jointType = CORBA::string_dup("slide");
966  vaxisunits[ic] = _GetUnitScale(pdomaxis,_fGlobalScale);
967  }
968  else {
969  COLLADALOG_WARN(str(boost::format("unsupported joint type: %s")%pdomaxis->getElementName()));
970  }
971 
972  _mapJointUnits[pjoint] = vaxisunits;
973  string jointname;
974  if( !!pdomjoint->getName() ) {
975  jointname = _ConvertToValidName(pdomjoint->getName());
976  }
977  else {
978  jointname = str(boost::format("dummy%d")%pjoint->jointId);
979  }
980  pjoint->name = CORBA::string_dup(jointname.c_str());
981 
982  if( _mapJointIds.find(jointid) != _mapJointIds.end() ) {
983  COLLADALOG_WARN(str(boost::format("jointid '%s' is duplicated!")%jointid));
984  }
985 
986  int jointsid;
987  if ( sscanf(jointid.c_str(), "kmodel1/jointsid%d", &jointsid) ) {
988  pjoint->jointId = jointsid;
989  } else {
990  pjoint->jointId = ijointindex - 1;
991  }
992  _mapJointIds[jointid] = pjoint;
993  COLLADALOG_DEBUG(str(boost::format("joint %s (%d)")%pjoint->name%pjoint->jointId));
994 
995  domKinematics_axis_infoRef kinematics_axis_info;
996  domMotion_axis_infoRef motion_axis_info;
997  for(std::list<JointAxisBinding>::const_iterator itaxisbinding = listAxisBindings.begin(); itaxisbinding != listAxisBindings.end(); ++itaxisbinding) {
998  if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
999  kinematics_axis_info = itaxisbinding->kinematics_axis_info;
1000  motion_axis_info = itaxisbinding->motion_axis_info;
1001  break;
1002  }
1003  }
1004 
1005  // Axes and Anchor assignment.
1006  dReal len2 = 0;
1007  for(int i = 0; i < 3; ++i) {
1008  len2 += pdomaxis->getAxis()->getValue()[i] * pdomaxis->getAxis()->getValue()[i];
1009  }
1010  if( len2 > 0 ) {
1011  len2 = 1/len2;
1012  DblArray12 trans_joint_to_child;
1013  _ExtractFullTransform(trans_joint_to_child, pattfull->getLink());
1014  DblArray12 patt;
1015  PoseInverse(patt, trans_joint_to_child);
1016  len2 = 1/len2;
1017  double ax = pdomaxis->getAxis()->getValue()[0]*len2;
1018  double ay = pdomaxis->getAxis()->getValue()[1]*len2;
1019  double az = pdomaxis->getAxis()->getValue()[2]*len2;
1020  pjoint->jointAxis[0] = patt[0] * ax + patt[1] * ay + patt[2] * az;
1021  pjoint->jointAxis[1] = patt[4] * ax + patt[5] * ay + patt[6] * az;
1022  pjoint->jointAxis[2] = patt[8] * ax + patt[9] * ay + patt[10] * az;
1023  COLLADALOG_DEBUG(str(boost::format("axis: %f %f %f -> %f %f %f")%ax%ay%az%pjoint->jointAxis[0]%pjoint->jointAxis[1]%pjoint->jointAxis[2]));
1024  }
1025  else {
1026  pjoint->jointAxis[0] = 0;
1027  pjoint->jointAxis[1] = 0;
1028  pjoint->jointAxis[2] = 1;
1029  }
1030  // Rotate axis from the parent offset
1031  //PoseRotateVector(pjoint->jointAxis,tatt,pjoint->jointAxis);
1032  COLLADALOG_WARN(str(boost::format("joint %s has axis: %f %f %f")%jointname%pjoint->jointAxis[0]%pjoint->jointAxis[1]%pjoint->jointAxis[2]));
1033 
1034  if( !motion_axis_info ) {
1035  COLLADALOG_WARN(str(boost::format("No motion axis info for joint %s")%pjoint->name));
1036  }
1037 
1038  // Sets the Speed and the Acceleration of the joint
1039  if (!!motion_axis_info) {
1040  if (!!motion_axis_info->getSpeed()) {
1041  pjoint->lvlimit.length(1);
1042  pjoint->uvlimit.length(1);
1043  pjoint->uvlimit[0] = resolveFloat(motion_axis_info->getSpeed(),motion_axis_info);
1044  pjoint->lvlimit[0] = -pjoint->uvlimit[0];
1045  }
1046  if (!!motion_axis_info->getAcceleration()) {
1047  COLLADALOG_DEBUG("robot has max acceleration info");
1048  }
1049  }
1050 
1051  bool joint_locked = false; // if locked, joint angle is static
1052  bool kinematics_limits = false;
1053 
1054  if (!!kinematics_axis_info) {
1055  if (!!kinematics_axis_info->getLocked()) {
1056  joint_locked = resolveBool(kinematics_axis_info->getLocked(),kinematics_axis_info);
1057  }
1058 
1059  if (joint_locked) { // If joint is locked set limits to the static value.
1060  COLLADALOG_WARN("lock joint!!");
1061  pjoint->llimit.length(1);
1062  pjoint->ulimit.length(1);
1063  pjoint->llimit[ic] = 0;
1064  pjoint->ulimit[ic] = 0;
1065  }
1066  else if (kinematics_axis_info->getLimits()) { // If there are articulated system kinematics limits
1067  kinematics_limits = true;
1068  pjoint->llimit.length(1);
1069  pjoint->ulimit.length(1);
1070  dReal fscale = bIsRevolute?(M_PI/180.0f):_GetUnitScale(kinematics_axis_info,_fGlobalScale);
1071  pjoint->llimit[ic] = fscale*(dReal)(resolveFloat(kinematics_axis_info->getLimits()->getMin(),kinematics_axis_info));
1072  pjoint->ulimit[ic] = fscale*(dReal)(resolveFloat(kinematics_axis_info->getLimits()->getMax(),kinematics_axis_info));
1073  }
1074  }
1075 
1076  // Search limits in the joints section
1077  if (!kinematics_axis_info || (!joint_locked && !kinematics_limits)) {
1078  // If there are NO LIMITS
1079  if( !!pdomaxis->getLimits() ) {
1080  dReal fscale = bIsRevolute?(M_PI/180.0f):_GetUnitScale(pdomaxis,_fGlobalScale);
1081  pjoint->llimit.length(1);
1082  pjoint->ulimit.length(1);
1083  pjoint->llimit[ic] = (dReal)pdomaxis->getLimits()->getMin()->getValue()*fscale;
1084  pjoint->ulimit[ic] = (dReal)pdomaxis->getLimits()->getMax()->getValue()*fscale;
1085  }
1086  else {
1087  COLLADALOG_VERBOSE(str(boost::format("There are NO LIMITS in joint %s:%d ...")%pjoint->name%kinematics_limits));
1088  }
1089  }
1090  }
1091  if( pdomlink->getAttachment_start_array().getCount() > 0 ) {
1092  COLLADALOG_WARN("openrave collada reader does not support attachment_start");
1093  }
1094  if( pdomlink->getAttachment_end_array().getCount() > 0 ) {
1095  COLLADALOG_WARN("openrave collada reader does not support attachment_end");
1096  }
1097  }
1098  return ilinkindex;
1099  }
1100 
1104  bool ExtractGeometry(BodyInfoCollada_impl* pkinbody, boost::shared_ptr<LinkInfo> plink, const DblArray12& tlink, const domNodeRef pdomnode, const std::list<JointAxisBinding>& listAxisBindings,const std::vector<std::string>& vprocessednodes)
1105  {
1106  if( !pdomnode ) {
1107  COLLADALOG_WARN(str(boost::format("fail to ExtractGeometry(LinkInfo,plink,tlink,pdomnode) of %s")%plink->name));
1108  return false;
1109  }
1110  if( !!pdomnode->getID() && find(vprocessednodes.begin(),vprocessednodes.end(),pdomnode->getID()) != vprocessednodes.end() ) {
1111  COLLADALOG_WARN(str(boost::format("could not create geometry type pnode getid %s")%pdomnode->getID()));
1112  return false;
1113  }
1114 
1115  COLLADALOG_VERBOSE(str(boost::format("ExtractGeometry(node,link) of %s")%pdomnode->getName()));
1116 
1117  bool bhasgeometry = false;
1118  // For all child nodes of pdomnode
1119  for (size_t i = 0; i < pdomnode->getNode_array().getCount(); i++) {
1120  // check if contains a joint
1121  bool contains=false;
1122  for(std::list<JointAxisBinding>::const_iterator it = listAxisBindings.begin(); it != listAxisBindings.end(); ++it) {
1123  // don't check ID's check if the reference is the same!
1124  if ( (pdomnode->getNode_array()[i]) == (it->visualnode)){
1125  contains=true;
1126  break;
1127  }
1128  }
1129  if (contains) {
1130  continue;
1131  }
1132 
1133  bhasgeometry |= ExtractGeometry(pkinbody, plink, tlink, pdomnode->getNode_array()[i],listAxisBindings,vprocessednodes);
1134  // Plink stayes the same for all children
1135  // replace pdomnode by child = pdomnode->getNode_array()[i]
1136  // hope for the best!
1137  // put everything in a subroutine in order to process pdomnode too!
1138  }
1139 
1140  unsigned int nGeomBefore = plink->shapeIndices.length(); // #of Meshes already associated to this link
1141 
1142  // get the geometry
1143  for (size_t igeom = 0; igeom < pdomnode->getInstance_geometry_array().getCount(); ++igeom) {
1144  domInstance_geometryRef domigeom = pdomnode->getInstance_geometry_array()[igeom];
1145  domGeometryRef domgeom = daeSafeCast<domGeometry> (domigeom->getUrl().getElement());
1146  if (!domgeom) {
1147  COLLADALOG_WARN(str(boost::format("link %s does not have valid geometry")%plink->name));
1148  continue;
1149  }
1150 
1151  // Gets materials
1152  map<string, int> mapmaterials;
1153  map<string, int> maptextures;
1154  if (!!domigeom->getBind_material() && !!domigeom->getBind_material()->getTechnique_common()) {
1155  const domInstance_material_Array& matarray = domigeom->getBind_material()->getTechnique_common()->getInstance_material_array();
1156  for (size_t imat = 0; imat < matarray.getCount(); ++imat) {
1157  domMaterialRef pdommat = daeSafeCast<domMaterial>(matarray[imat]->getTarget().getElement());
1158  if (!!pdommat) {
1159  int mindex = pkinbody->materials_.length();
1160  pkinbody->materials_.length(mindex+1);
1161  _FillMaterial(pkinbody->materials_[mindex],pdommat);
1162  mapmaterials[matarray[imat]->getSymbol()] = mindex;
1163 
1164  if( !!pdommat && !!pdommat->getInstance_effect() ) {
1165  domEffectRef peffect = daeSafeCast<domEffect>(pdommat->getInstance_effect()->getUrl().getElement().cast());
1166  if( !!peffect ) {
1167  domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
1168  if( !!pphong && !!pphong->getDiffuse() && !!pphong->getDiffuse()->getTexture() ) {
1169  // search for newparam that matches sid
1170  daeElementRefArray psamplers = daeSidRef(pphong->getDiffuse()->getTexture()->getTexture(), peffect).resolve().elt->getChildren();
1171  daeElementRef psampler;
1172  for (size_t i = 0; i < psamplers.getCount(); ++i ) {
1173  if ( strcmp(psamplers[i]->getElementName(),"sampler2D") == 0 ) {
1174  psampler = psamplers[i];
1175  }
1176  }
1177  //
1178  domInstance_imageRef pinstanceimage;
1179  for (size_t i = 0; i < psampler->getChildren().getCount(); ++i ) {
1180  if ( strcmp(psampler->getChildren()[i]->getElementName(), "instance_image") == 0 ) {
1181  pinstanceimage = daeSafeCast<domInstance_image>(psampler->getChildren()[i]);
1182  }
1183  }
1184  domImageRef image = daeSafeCast<domImage>(pinstanceimage->getUrl().getElement());
1185  if ( image && image->getInit_from() && image->getInit_from()->getRef() ) {
1186  int tindex = pkinbody->textures_.length();
1187  pkinbody->textures_.length(tindex+1);
1188  TextureInfo_var texture(new TextureInfo());
1189  texture->repeatS = 1;
1190  texture->repeatT = 1;
1191  texture->url = string(image->getInit_from()->getRef()->getValue().path()).c_str();
1192  pkinbody->textures_[tindex] = texture;
1193  maptextures[matarray[imat]->getSymbol()] = tindex;
1194 
1195  }
1196  }
1197  }
1198  }
1199  }
1200  }
1201  }
1202 
1203  // Gets the geometry
1204  bhasgeometry |= ExtractGeometry(pkinbody, plink, domgeom, mapmaterials, maptextures);
1205  }
1206 
1207  if( !bhasgeometry ) {
1208  return false;
1209  }
1210 
1211  DblArray12 tmnodegeom, ttemp1, ttemp2, ttemp3;
1212  PoseInverse(ttemp1, visualRootOrigin);
1213  getNodeParentTransform(ttemp2, pdomnode);
1214  PoseMult(ttemp3, ttemp1, ttemp2);
1215  _ExtractFullTransform(ttemp1, pdomnode);
1216  PoseMult(ttemp2, ttemp3, ttemp1);
1217  PoseInverse(ttemp1, tlink);
1218  PoseMult(tmnodegeom, ttemp1, ttemp2);
1219  printArray("tmnodegeom", tmnodegeom);
1220 
1221  // there is a weird bug with the viewer, but should try to normalize the rotation!
1222  DblArray4 quat;
1223  double x=tmnodegeom[3],y=tmnodegeom[7],z=tmnodegeom[11];
1224  QuatFromMatrix(quat, tmnodegeom);
1225  PoseFromQuat(tmnodegeom,quat);
1226  tmnodegeom[3] = x; tmnodegeom[7] = y; tmnodegeom[11] = z;
1227 
1228  // change only the transformations of the newly found geometries.
1229  for(unsigned int i = nGeomBefore; i < plink->shapeIndices.length(); ++i) {
1230  memcpy(plink->shapeIndices[i].transformMatrix,tmnodegeom,sizeof(tmnodegeom));
1231  plink->shapeIndices[i].inlinedShapeTransformMatrixIndex = -1;
1232  }
1233 
1234  return true;
1235  }
1236 
1242  bool _ExtractGeometry(BodyInfoCollada_impl* pkinbody, boost::shared_ptr<LinkInfo> plink, const domTrianglesRef triRef, const domVerticesRef vertsRef, const map<string,int>& mapmaterials, const map<string,int>& maptextures)
1243  {
1244  if( !triRef ) {
1245  COLLADALOG_WARN(str(boost::format("fail to _ExtractGeometry(LinkInfo,Triangles,Vertices) of %s")%plink->name));
1246  return false;
1247  }
1248  int shapeIndex = pkinbody->shapes_.length();
1249  pkinbody->shapes_.length(shapeIndex+1);
1250  ShapeInfo& shape = pkinbody->shapes_[shapeIndex];
1251  shape.primitiveType = SP_MESH;
1252  //shape.url = "";
1253  int lsindex = plink->shapeIndices.length();
1254  plink->shapeIndices.length(lsindex+1);
1255  plink->shapeIndices[lsindex].shapeIndex = shapeIndex;
1256 
1257  int aindex = pkinbody->appearances_.length();
1258  pkinbody->appearances_.length(aindex+1);
1259  AppearanceInfo& ainfo = pkinbody->appearances_[aindex];
1260  ainfo.materialIndex = -1;
1261  ainfo.normalPerVertex = 0;
1262  ainfo.solid = 0;
1263  ainfo.creaseAngle = 0;
1264  ainfo.colorPerVertex = 0;
1265  ainfo.textureIndex = -1;
1266  if( !!triRef->getMaterial() ) {
1267  map<string,int>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
1268  if( itmat != mapmaterials.end() ) {
1269  ainfo.materialIndex = itmat->second;
1270  }
1271  }
1272  shape.appearanceIndex = aindex;
1273 
1274  size_t triangleIndexStride = 0;
1275  int vertexoffset = -1, normaloffset = -1, texcoordoffset = -1;
1276  domInput_local_offsetRef indexOffsetRef, normalOffsetRef, texcoordOffsetRef;
1277 
1278  for (unsigned int w=0;w<triRef->getInput_array().getCount();w++) {
1279  size_t offset = triRef->getInput_array()[w]->getOffset();
1280  daeString str = triRef->getInput_array()[w]->getSemantic();
1281  if (!strcmp(str,"VERTEX")) {
1282  indexOffsetRef = triRef->getInput_array()[w];
1283  vertexoffset = offset;
1284  }
1285  if (!strcmp(str,"NORMAL")) {
1286  normalOffsetRef = triRef->getInput_array()[w];
1287  normaloffset = offset;
1288  }
1289  if (!strcmp(str,"TEXCOORD")) {
1290  texcoordOffsetRef = triRef->getInput_array()[w];
1291  texcoordoffset = offset;
1292  }
1293  if (offset> triangleIndexStride) {
1294  triangleIndexStride = offset;
1295  }
1296  }
1297  triangleIndexStride++;
1298 
1299  const domList_of_uints& indexArray =triRef->getP()->getValue();
1300  shape.triangles.length(triRef->getCount()*3);
1301  shape.vertices.length(triRef->getCount()*9);
1302 
1303  int itriangle = 0;
1304  for (size_t i=0;i<vertsRef->getInput_array().getCount();++i) {
1305  domInput_localRef localRef = vertsRef->getInput_array()[i];
1306  daeString str = localRef->getSemantic();
1307  if ( strcmp(str,"POSITION") == 0 ) {
1308  const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1309  if( !node ) {
1310  continue;
1311  }
1312  dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
1313  const domFloat_arrayRef flArray = node->getFloat_array();
1314  if (!!flArray) {
1315  const domList_of_floats& listFloats = flArray->getValue();
1316  int k=vertexoffset;
1317  int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
1318  int ivertex = 0;
1319  for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
1320  if(k+2*triangleIndexStride < indexArray.getCount() ) {
1321  for (int j=0;j<3;j++) {
1322  int index0 = indexArray.get(k)*vertexStride;
1323  domFloat fl0 = listFloats.get(index0);
1324  domFloat fl1 = listFloats.get(index0+1);
1325  domFloat fl2 = listFloats.get(index0+2);
1326  k+=triangleIndexStride;
1327  shape.triangles[itriangle++] = ivertex/3;
1328  shape.vertices[ivertex++] = fl0*fUnitScale;
1329  shape.vertices[ivertex++] = fl1*fUnitScale;
1330  shape.vertices[ivertex++] = fl2*fUnitScale;
1331  }
1332  }
1333  }
1334  }
1335  else {
1336  COLLADALOG_WARN("float array not defined!");
1337  }
1338 
1339  if ( normaloffset >= 0 ) {
1340  const domSourceRef normalNode = daeSafeCast<domSource>(normalOffsetRef->getSource().getElement());
1341  const domFloat_arrayRef normalArray = normalNode->getFloat_array();
1342  const domList_of_floats& normalFloats = normalArray->getValue();
1343  AppearanceInfo& ainfo = pkinbody->appearances_[shape.appearanceIndex];
1344  ainfo.normalPerVertex = 1;
1345  ainfo.normals.length(triRef->getCount()*9);
1346  int k = normaloffset;
1347  int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
1348  int ivertex = 0;
1349  for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
1350  if(k+2*triangleIndexStride < indexArray.getCount() ) {
1351  for (int j=0;j<3;j++) {
1352  int index0 = indexArray.get(k)*vertexStride;
1353  domFloat fl0 = normalFloats.get(index0);
1354  domFloat fl1 = normalFloats.get(index0+1);
1355  domFloat fl2 = normalFloats.get(index0+2);
1356  k+=triangleIndexStride;
1357  ainfo.normals[ivertex++] = fl0;
1358  ainfo.normals[ivertex++] = fl1;
1359  ainfo.normals[ivertex++] = fl2;
1360  }
1361  }
1362  }
1363  }
1364 
1365  if ( texcoordoffset >= 0 ) {
1366  const domSourceRef texcoordNode = daeSafeCast<domSource>(texcoordOffsetRef->getSource().getElement());
1367  const domFloat_arrayRef texcoordArray = texcoordNode->getFloat_array();
1368  const domList_of_floats& texcoordFloats = texcoordArray->getValue();
1369  AppearanceInfo& ainfo = pkinbody->appearances_[shape.appearanceIndex];
1370  map<string,int>::const_iterator itmat = maptextures.find(triRef->getMaterial());
1371  if( itmat != maptextures.end() ) {
1372  ainfo.textureIndex = itmat->second;
1373 
1374  ainfo.textureCoordinate.length(texcoordArray->getCount());
1375  ainfo.textureCoordIndices.length(triRef->getCount()*3);
1376  int k = texcoordoffset;
1377  int itriangle = 0;
1378  for(size_t itex = 0; itex < texcoordArray->getCount() ; ++itex) {
1379  ainfo.textureCoordinate[itex] = texcoordFloats.get(itex);
1380  }
1381  for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
1382  if(k+2*triangleIndexStride < indexArray.getCount() ) {
1383  for (int j=0;j<3;j++) {
1384  int index0 = indexArray.get(k);
1385  k+=triangleIndexStride;
1386  ainfo.textureCoordIndices[itriangle++] = index0;
1387  }
1388  }
1389  }
1390  for(int i=0,k=0; i<3; i++) {
1391  for(int j=0; j<3; j++) {
1392  if ( i==j )
1393  ainfo.textransformMatrix[k++] = 1;
1394  else
1395  ainfo.textransformMatrix[k++] = 0;
1396  }
1397  }
1398  }
1399  }
1400  } else if ( strcmp(str,"NORMAL") == 0 ) {
1401  COLLADALOG_WARN("read normals from collada file");
1402  const domSourceRef normalNode = daeSafeCast<domSource>(localRef->getSource().getElement());
1403  if( !normalNode ) {
1404  continue;
1405  }
1406  const domFloat_arrayRef normalArray = normalNode->getFloat_array();
1407  if (!!normalArray) {
1408  const domList_of_floats& normalFloats = normalArray->getValue();
1409  int k = 0;
1410  int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
1411  int ivertex = 0;
1412  AppearanceInfo& ainfo = pkinbody->appearances_[shape.appearanceIndex];
1413  ainfo.normalPerVertex = 1;
1414  ainfo.normals.length(triRef->getCount()*9);
1415  for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
1416  if(2*triangleIndexStride < indexArray.getCount() ) {
1417  for (int j=0;j<3;j++) {
1418  int index0 = indexArray.get(k)*vertexStride;
1419  domFloat fl0 = normalFloats.get(index0);
1420  domFloat fl1 = normalFloats.get(index0+1);
1421  domFloat fl2 = normalFloats.get(index0+2);
1422  k+=triangleIndexStride;
1423  ainfo.normals[ivertex++] = fl0;
1424  ainfo.normals[ivertex++] = fl1;
1425  ainfo.normals[ivertex++] = fl2;
1426  }
1427  }
1428  }
1429  }
1430  else {
1431  COLLADALOG_WARN("float array not defined!");
1432  }
1433  }
1434  }
1435 
1436  if ( ainfo.normals.length() == 0 ) {
1437  COLLADALOG_WARN("generate normals from vertices");
1438  // compute face normals
1439  std::vector<Vector3> faceNormals(itriangle/3);
1440  std::vector<std::vector<int> > vertexFaceMapping(itriangle);
1441  for(int i=0; i < itriangle/3; ++i) {
1442  int vIndex1 = shape.triangles[i*3+0];
1443  int vIndex2 = shape.triangles[i*3+1];
1444  int vIndex3 = shape.triangles[i*3+2];
1445  Vector3 a(shape.vertices[vIndex1*3+0]-
1446  shape.vertices[vIndex3*3+0],
1447  shape.vertices[vIndex1*3+1]-
1448  shape.vertices[vIndex3*3+1],
1449  shape.vertices[vIndex1*3+2]-
1450  shape.vertices[vIndex3*3+2]);
1451  Vector3 b(shape.vertices[vIndex1*3+0]-
1452  shape.vertices[vIndex2*3+0],
1453  shape.vertices[vIndex1*3+1]-
1454  shape.vertices[vIndex2*3+1],
1455  shape.vertices[vIndex1*3+2]-
1456  shape.vertices[vIndex2*3+2]);
1457  a.normalize();
1458  b.normalize();
1459  Vector3 c = a.cross(b);
1460  faceNormals[i] = c.normalized();
1461  vertexFaceMapping[vIndex1].push_back(i);
1462  vertexFaceMapping[vIndex2].push_back(i);
1463  vertexFaceMapping[vIndex3].push_back(i);
1464  }
1465  // compute vertex normals
1466  ainfo.normalPerVertex = 1;
1467  ainfo.normals.length(itriangle*3); // number_of_tris*3*3
1468  double creaseAngle = M_PI/4;
1469  for (int i=0; i<itriangle/3; i++){ // each triangle
1470  const Vector3 &currentFaceNormal = faceNormals[i];
1471  for (int vertex=0; vertex<3; vertex++){ // each vertex
1472  bool normalIsFaceNormal = true;
1473  int vIndex = shape.triangles[i*3+vertex];
1474  Vector3 normal = currentFaceNormal;
1475  const std::vector<int>& faces = vertexFaceMapping[vIndex];
1476  for (size_t k=0; k<faces.size(); k++){
1477  const Vector3& adjoingFaceNormal = faceNormals[k];
1478  double angle = acos(currentFaceNormal.dot(adjoingFaceNormal) /
1479  (currentFaceNormal.norm() * adjoingFaceNormal.norm()));
1480  if(angle > 1.0e-6 && angle < creaseAngle){
1481  normal += adjoingFaceNormal;
1482  normalIsFaceNormal = false;
1483  }
1484  }
1485  if (!normalIsFaceNormal){
1486  normal.normalize();
1487  }
1488  ainfo.normals[vIndex*3 ] = normal[0];
1489  ainfo.normals[vIndex*3+1] = normal[1];
1490  ainfo.normals[vIndex*3+2] = normal[2];
1491  }
1492  }
1493  }
1494 
1495  if( itriangle != 3*(int)triRef->getCount() ) {
1496  COLLADALOG_WARN("triangles declares wrong count!");
1497  }
1498  return true;
1499  }
1500 
1506  bool _ExtractGeometry(BodyInfoCollada_impl* pkinbody, boost::shared_ptr<LinkInfo> plink, const domTrifansRef triRef, const domVerticesRef vertsRef, const map<string,int>& mapmaterials, const map<string,int>& maptextures)
1507  {
1508  if( !triRef ) {
1509  COLLADALOG_WARN(str(boost::format("fail to _ExtractGeometry(LinkInfo,Trifans,Vertices) of %s")%plink->name));
1510  return false;
1511  }
1512  int shapeIndex = pkinbody->shapes_.length();
1513  pkinbody->shapes_.length(shapeIndex+1);
1514  ShapeInfo& shape = pkinbody->shapes_[shapeIndex];
1515  shape.primitiveType = SP_MESH;
1516  int lsindex = plink->shapeIndices.length();
1517  plink->shapeIndices.length(lsindex+1);
1518  plink->shapeIndices[lsindex].shapeIndex = shapeIndex;
1519 
1520  // resolve the material and assign correct colors to the geometry
1521  shape.appearanceIndex = -1;
1522  if( !!triRef->getMaterial() ) {
1523  map<string,int>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
1524  if( itmat != mapmaterials.end() ) {
1525  shape.appearanceIndex = itmat->second;
1526  }
1527  }
1528 
1529  size_t triangleIndexStride = 0, vertexoffset = -1;
1530  domInput_local_offsetRef indexOffsetRef;
1531 
1532  for (unsigned int w=0;w<triRef->getInput_array().getCount();w++) {
1533  size_t offset = triRef->getInput_array()[w]->getOffset();
1534  daeString str = triRef->getInput_array()[w]->getSemantic();
1535  if (!strcmp(str,"VERTEX")) {
1536  indexOffsetRef = triRef->getInput_array()[w];
1537  vertexoffset = offset;
1538  }
1539  if (offset> triangleIndexStride) {
1540  triangleIndexStride = offset;
1541  }
1542  }
1543  triangleIndexStride++;
1544  size_t primitivecount = triRef->getCount();
1545  if( primitivecount > triRef->getP_array().getCount() ) {
1546  COLLADALOG_WARN("trifans has incorrect count");
1547  primitivecount = triRef->getP_array().getCount();
1548  }
1549  int itriangle = 0, ivertex = 0;
1550  for(size_t ip = 0; ip < primitivecount; ++ip) {
1551  domList_of_uints indexArray =triRef->getP_array()[ip]->getValue();
1552  for (size_t i=0;i<vertsRef->getInput_array().getCount();++i) {
1553  domInput_localRef localRef = vertsRef->getInput_array()[i];
1554  daeString str = localRef->getSemantic();
1555  if ( strcmp(str,"POSITION") == 0 ) {
1556  const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1557  if( !node ) {
1558  continue;
1559  }
1560  dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
1561  const domFloat_arrayRef flArray = node->getFloat_array();
1562  if (!!flArray) {
1563  const domList_of_floats& listFloats = flArray->getValue();
1564  int k=vertexoffset;
1565  int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
1566  size_t usedindices = 3*(indexArray.getCount()-2);
1567  shape.triangles.length(shape.triangles.length()+usedindices);
1568  shape.vertices.length(shape.vertices.length()+3*indexArray.getCount());
1569  size_t startoffset = ivertex/3;
1570  while(k < (int)indexArray.getCount() ) {
1571  int index0 = indexArray.get(k)*vertexStride;
1572  domFloat fl0 = listFloats.get(index0);
1573  domFloat fl1 = listFloats.get(index0+1);
1574  domFloat fl2 = listFloats.get(index0+2);
1575  k+=triangleIndexStride;
1576  shape.vertices[ivertex++] = fl0*fUnitScale;
1577  shape.vertices[ivertex++] = fl1*fUnitScale;
1578  shape.vertices[ivertex++] = fl2*fUnitScale;
1579  }
1580  for(size_t ivert = 2; ivert < indexArray.getCount(); ++ivert) {
1581  shape.triangles[itriangle++] = startoffset;
1582  shape.triangles[itriangle++] = startoffset+ivert-1;
1583  shape.triangles[itriangle++] = startoffset+ivert;
1584  }
1585  }
1586  else {
1587  COLLADALOG_WARN("float array not defined!");
1588  }
1589  break;
1590  }
1591  }
1592  }
1593  return true;
1594  }
1595 
1601  bool _ExtractGeometry(BodyInfoCollada_impl* pkinbody, boost::shared_ptr<LinkInfo> plink, const domTristripsRef triRef, const domVerticesRef vertsRef, const map<string,int>& mapmaterials, const map<string,int>& maptextures)
1602  {
1603  if( !triRef ) {
1604  COLLADALOG_WARN(str(boost::format("fail to _ExtractGeometry(LinkInfo,Tristrips,Vertices) of %s")%plink->name));
1605  return false;
1606  }
1607  int shapeIndex = pkinbody->shapes_.length();
1608  pkinbody->shapes_.length(shapeIndex+1);
1609  ShapeInfo& shape = pkinbody->shapes_[shapeIndex];
1610  shape.primitiveType = SP_MESH;
1611  int lsindex = plink->shapeIndices.length();
1612  plink->shapeIndices.length(lsindex+1);
1613  plink->shapeIndices[lsindex].shapeIndex = shapeIndex;
1614 
1615  // resolve the material and assign correct colors to the geometry
1616  shape.appearanceIndex = -1;
1617  if( !!triRef->getMaterial() ) {
1618  map<string,int>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
1619  if( itmat != mapmaterials.end() ) {
1620  shape.appearanceIndex = itmat->second;
1621  }
1622  }
1623 
1624  size_t triangleIndexStride = 0, vertexoffset = -1;
1625  domInput_local_offsetRef indexOffsetRef;
1626 
1627  for (unsigned int w=0;w<triRef->getInput_array().getCount();w++) {
1628  size_t offset = triRef->getInput_array()[w]->getOffset();
1629  daeString str = triRef->getInput_array()[w]->getSemantic();
1630  if (!strcmp(str,"VERTEX")) {
1631  indexOffsetRef = triRef->getInput_array()[w];
1632  vertexoffset = offset;
1633  }
1634  if (offset> triangleIndexStride) {
1635  triangleIndexStride = offset;
1636  }
1637  }
1638  triangleIndexStride++;
1639  size_t primitivecount = triRef->getCount();
1640  if( primitivecount > triRef->getP_array().getCount() ) {
1641  COLLADALOG_WARN("tristrips has incorrect count");
1642  primitivecount = triRef->getP_array().getCount();
1643  }
1644  int itriangle = 0, ivertex = 0;
1645  for(size_t ip = 0; ip < primitivecount; ++ip) {
1646  domList_of_uints indexArray = triRef->getP_array()[ip]->getValue();
1647  for (size_t i=0;i<vertsRef->getInput_array().getCount();++i) {
1648  domInput_localRef localRef = vertsRef->getInput_array()[i];
1649  daeString str = localRef->getSemantic();
1650  if ( strcmp(str,"POSITION") == 0 ) {
1651  const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1652  if( !node ) {
1653  continue;
1654  }
1655  dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
1656  const domFloat_arrayRef flArray = node->getFloat_array();
1657  if (!!flArray) {
1658  const domList_of_floats& listFloats = flArray->getValue();
1659  int k=vertexoffset;
1660  int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
1661  size_t usedindices = 3*(indexArray.getCount()-2);
1662  shape.triangles.length(shape.triangles.length()+usedindices);
1663  shape.vertices.length(shape.vertices.length()+3*indexArray.getCount());
1664  size_t startoffset = ivertex/3;
1665  while(k < (int)indexArray.getCount() ) {
1666  int index0 = indexArray.get(k)*vertexStride;
1667  domFloat fl0 = listFloats.get(index0);
1668  domFloat fl1 = listFloats.get(index0+1);
1669  domFloat fl2 = listFloats.get(index0+2);
1670  k+=triangleIndexStride;
1671  shape.vertices[ivertex++] = fl0*fUnitScale;
1672  shape.vertices[ivertex++] = fl1*fUnitScale;
1673  shape.vertices[ivertex++] = fl2*fUnitScale;
1674  }
1675 
1676  bool bFlip = false;
1677  for(size_t ivert = 2; ivert < indexArray.getCount(); ++ivert) {
1678  shape.triangles[itriangle++] = startoffset-2;
1679  shape.triangles[itriangle++] = bFlip ? startoffset+ivert : startoffset+ivert-1;
1680  shape.triangles[itriangle++] = bFlip ? startoffset+ivert-1 : startoffset+ivert;
1681  bFlip = !bFlip;
1682  }
1683  }
1684  else {
1685  COLLADALOG_WARN("float array not defined!");
1686  }
1687  break;
1688  }
1689  }
1690  }
1691  return true;
1692  }
1693 
1699  bool _ExtractGeometry(BodyInfoCollada_impl* pkinbody, boost::shared_ptr<LinkInfo> plink, const domPolylistRef triRef, const domVerticesRef vertsRef, const map<string,int>& mapmaterials, const map<string,int>& maptextures)
1700  {
1701  if( !triRef ) {
1702  COLLADALOG_WARN(str(boost::format("fail to _ExtractGeometry(LinkInfo,Polylist,VErtices) of %s")%plink->name));
1703  return false;
1704  }
1705  int shapeIndex = pkinbody->shapes_.length();
1706  pkinbody->shapes_.length(shapeIndex+1);
1707  ShapeInfo& shape = pkinbody->shapes_[shapeIndex];
1708  shape.primitiveType = SP_MESH;
1709  int lsindex = plink->shapeIndices.length();
1710  plink->shapeIndices.length(lsindex+1);
1711  plink->shapeIndices[lsindex].shapeIndex = shapeIndex;
1712 
1713  // resolve the material and assign correct colors to the geometry
1714  shape.appearanceIndex = -1;
1715  if( !!triRef->getMaterial() ) {
1716  map<string,int>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
1717  if( itmat != mapmaterials.end() ) {
1718  shape.appearanceIndex = itmat->second;
1719  }
1720  }
1721 
1722  size_t triangleIndexStride = 0,vertexoffset = -1;
1723  domInput_local_offsetRef indexOffsetRef;
1724  for (unsigned int w=0;w<triRef->getInput_array().getCount();w++) {
1725  size_t offset = triRef->getInput_array()[w]->getOffset();
1726  daeString str = triRef->getInput_array()[w]->getSemantic();
1727  if (!strcmp(str,"VERTEX")) {
1728  indexOffsetRef = triRef->getInput_array()[w];
1729  vertexoffset = offset;
1730  }
1731  if (offset> triangleIndexStride) {
1732  triangleIndexStride = offset;
1733  }
1734  }
1735  triangleIndexStride++;
1736  const domList_of_uints& indexArray =triRef->getP()->getValue();
1737  int ivertex = 0, itriangle=0;
1738  for (size_t i=0;i<vertsRef->getInput_array().getCount();++i) {
1739  domInput_localRef localRef = vertsRef->getInput_array()[i];
1740  daeString str = localRef->getSemantic();
1741  if ( strcmp(str,"POSITION") == 0 ) {
1742  const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1743  if( !node ) {
1744  continue;
1745  }
1746  dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
1747  const domFloat_arrayRef flArray = node->getFloat_array();
1748  if (!!flArray) {
1749  const domList_of_floats& listFloats = flArray->getValue();
1750  size_t k=vertexoffset;
1751  int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
1752  for(size_t ipoly = 0; ipoly < triRef->getVcount()->getValue().getCount(); ++ipoly) {
1753  size_t numverts = triRef->getVcount()->getValue()[ipoly];
1754  if(numverts > 0 && k+(numverts-1)*triangleIndexStride < indexArray.getCount() ) {
1755  size_t startoffset = ivertex/3;
1756  shape.vertices.length(shape.vertices.length()+3*numverts);
1757  shape.triangles.length(shape.triangles.length()+3*(numverts-2));
1758  for (size_t j=0;j<numverts;j++) {
1759  int index0 = indexArray.get(k)*vertexStride;
1760  domFloat fl0 = listFloats.get(index0);
1761  domFloat fl1 = listFloats.get(index0+1);
1762  domFloat fl2 = listFloats.get(index0+2);
1763  k+=triangleIndexStride;
1764  shape.vertices[ivertex++] = fl0*fUnitScale;
1765  shape.vertices[ivertex++] = fl1*fUnitScale;
1766  shape.vertices[ivertex++] = fl2*fUnitScale;
1767  }
1768  for(size_t ivert = 2; ivert < numverts; ++ivert) {
1769  shape.triangles[itriangle++] = startoffset;
1770  shape.triangles[itriangle++] = startoffset+ivert-1;
1771  shape.triangles[itriangle++] = startoffset+ivert;
1772  }
1773  }
1774  }
1775  }
1776  else {
1777  COLLADALOG_WARN("float array not defined!");
1778  }
1779  break;
1780  }
1781  }
1782  return true;
1783  }
1784 
1789  bool ExtractGeometry(BodyInfoCollada_impl* pkinbody, boost::shared_ptr<LinkInfo> plink, const domGeometryRef geom, const map<string,int>& mapmaterials, const map<string,int>& maptextures)
1790  {
1791  if( !geom ) {
1792  COLLADALOG_WARN(str(boost::format("fail to ExtractGeometry(plink,geom) of %s")%plink->name));
1793  return false;
1794  }
1795  std::vector<Vector3> vconvexhull;
1796  if (geom->getMesh()) {
1797  const domMeshRef meshRef = geom->getMesh();
1798  for (size_t tg = 0;tg<meshRef->getTriangles_array().getCount();tg++) {
1799  _ExtractGeometry(pkinbody, plink, meshRef->getTriangles_array()[tg], meshRef->getVertices(), mapmaterials, maptextures);
1800  }
1801  for (size_t tg = 0;tg<meshRef->getTrifans_array().getCount();tg++) {
1802  _ExtractGeometry(pkinbody, plink, meshRef->getTrifans_array()[tg], meshRef->getVertices(), mapmaterials, maptextures);
1803  }
1804  for (size_t tg = 0;tg<meshRef->getTristrips_array().getCount();tg++) {
1805  _ExtractGeometry(pkinbody, plink, meshRef->getTristrips_array()[tg], meshRef->getVertices(), mapmaterials, maptextures);
1806  }
1807  for (size_t tg = 0;tg<meshRef->getPolylist_array().getCount();tg++) {
1808  _ExtractGeometry(pkinbody, plink, meshRef->getPolylist_array()[tg], meshRef->getVertices(), mapmaterials, maptextures);
1809  }
1810  if( meshRef->getPolygons_array().getCount()> 0 ) {
1811  COLLADALOG_WARN("openrave does not support collada polygons");
1812  }
1813 
1814  // if( alltrimesh.vertices.size() == 0 ) {
1815  // const domVerticesRef vertsRef = meshRef->getVertices();
1816  // for (size_t i=0;i<vertsRef->getInput_array().getCount();i++) {
1817  // domInput_localRef localRef = vertsRef->getInput_array()[i];
1818  // daeString str = localRef->getSemantic();
1819  // if ( strcmp(str,"POSITION") == 0 ) {
1820  // const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1821  // if( !node )
1822  // continue;
1823  // dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
1824  // const domFloat_arrayRef flArray = node->getFloat_array();
1825  // if (!!flArray) {
1826  // const domList_of_floats& listFloats = flArray->getValue();
1827  // int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
1828  // vconvexhull.reserve(vconvexhull.size()+listFloats.getCount());
1829  // for (size_t vertIndex = 0;vertIndex < listFloats.getCount();vertIndex+=vertexStride) {
1830  // //btVector3 verts[3];
1831  // domFloat fl0 = listFloats.get(vertIndex);
1832  // domFloat fl1 = listFloats.get(vertIndex+1);
1833  // domFloat fl2 = listFloats.get(vertIndex+2);
1834  // vconvexhull.push_back(Vector(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1835  // }
1836  // }
1837  // }
1838  // }
1839  //
1840  // _computeConvexHull(vconvexhull,alltrimesh);
1841  // }
1842 
1843  return true;
1844  }
1845  else if (geom->getConvex_mesh()) {
1846  {
1847  const domConvex_meshRef convexRef = geom->getConvex_mesh();
1848  daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
1849  if ( !!otherElemRef ) {
1850  domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
1851  COLLADALOG_WARN( "otherLinked");
1852  }
1853  else {
1854  COLLADALOG_WARN(str(boost::format("convexMesh polyCount = %d")%(int)convexRef->getPolygons_array().getCount()));
1855  COLLADALOG_WARN(str(boost::format("convexMesh triCount = %d")%(int)convexRef->getTriangles_array().getCount()));
1856  }
1857  }
1858 
1859  const domConvex_meshRef convexRef = geom->getConvex_mesh();
1860  //daeString urlref = convexRef->getConvex_hull_of().getURI();
1861  daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI();
1862  if (urlref2) {
1863  daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
1864 
1865  // Load all the geometry libraries
1866  for ( size_t i = 0; i < _dom->getLibrary_geometries_array().getCount(); i++) {
1867  domLibrary_geometriesRef libgeom = _dom->getLibrary_geometries_array()[i];
1868  for (size_t i = 0; i < libgeom->getGeometry_array().getCount(); i++) {
1869  domGeometryRef lib = libgeom->getGeometry_array()[i];
1870  if (!strcmp(lib->getId(),urlref2+1)) { // skip the # at the front of urlref2
1871  //found convex_hull geometry
1872  domMesh *meshElement = lib->getMesh();//linkedGeom->getMesh();
1873  if (meshElement) {
1874  const domVerticesRef vertsRef = meshElement->getVertices();
1875  for (size_t i=0;i<vertsRef->getInput_array().getCount();i++) {
1876  domInput_localRef localRef = vertsRef->getInput_array()[i];
1877  daeString str = localRef->getSemantic();
1878  if ( strcmp(str,"POSITION") == 0) {
1879  const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1880  if( !node ) {
1881  continue;
1882  }
1883  dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
1884  const domFloat_arrayRef flArray = node->getFloat_array();
1885  if (!!flArray) {
1886  vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
1887  const domList_of_floats& listFloats = flArray->getValue();
1888  for (size_t k=0;k+2<flArray->getCount();k+=3) {
1889  domFloat fl0 = listFloats.get(k);
1890  domFloat fl1 = listFloats.get(k+1);
1891  domFloat fl2 = listFloats.get(k+2);
1892  vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1893  }
1894  }
1895  }
1896  }
1897  }
1898  }
1899  }
1900  }
1901  }
1902  else {
1903  //no getConvex_hull_of but direct vertices
1904  const domVerticesRef vertsRef = convexRef->getVertices();
1905  for (size_t i=0;i<vertsRef->getInput_array().getCount();i++) {
1906  domInput_localRef localRef = vertsRef->getInput_array()[i];
1907  daeString str = localRef->getSemantic();
1908  if ( strcmp(str,"POSITION") == 0 ) {
1909  const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
1910  if( !node ) {
1911  continue;
1912  }
1913  dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
1914  const domFloat_arrayRef flArray = node->getFloat_array();
1915  if (!!flArray) {
1916  const domList_of_floats& listFloats = flArray->getValue();
1917  vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
1918  for (size_t k=0;k+2<flArray->getCount();k+=3) {
1919  domFloat fl0 = listFloats.get(k);
1920  domFloat fl1 = listFloats.get(k+1);
1921  domFloat fl2 = listFloats.get(k+2);
1922  vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
1923  }
1924  }
1925  }
1926  }
1927  }
1928 
1929  if( vconvexhull.size()> 0 ) {
1930  COLLADALOG_ERROR("convex hulls not supported");
1931  //plink->_listGeomProperties.push_back(KinBody::Link::GEOMPROPERTIES(plink));
1932  //KinBody::Link::GEOMPROPERTIES& geom = plink->_listGeomProperties.back();
1933  //KinBody::Link::TRIMESH& trimesh = geom.collisionmesh;
1934  //geom._type = KinBody::Link::GEOMPROPERTIES::GeomTrimesh;
1935  //geom.InitCollisionMesh();
1936  }
1937  return true;
1938  }
1939 
1940  return false;
1941  }
1942 
1946  void _FillMaterial(MaterialInfo& mat, const domMaterialRef pdommat)
1947  {
1948  mat.ambientIntensity = 0.1;
1949  mat.diffuseColor[0] = 0.8; mat.diffuseColor[1] = 0.8; mat.diffuseColor[2] = 0.8;
1950  mat.emissiveColor[0] = 0; mat.emissiveColor[1] = 0; mat.emissiveColor[2] = 0;
1951  mat.shininess = 0;
1952  mat.specularColor[0] = 0; mat.specularColor[1] = 0; mat.specularColor[2] = 0;
1953  mat.transparency = 0; // 0 is opaque
1954  if( !!pdommat && !!pdommat->getInstance_effect() ) {
1955  domEffectRef peffect = daeSafeCast<domEffect>(pdommat->getInstance_effect()->getUrl().getElement().cast());
1956  if( !!peffect ) {
1957  domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
1958  if( !!pphong ) {
1959  if( !!pphong->getAmbient() && !!pphong->getAmbient()->getColor() ) {
1960  domFx_color c = pphong->getAmbient()->getColor()->getValue();
1961  mat.ambientIntensity = (fabs(c[0])+fabs(c[1])+fabs(c[2]))/3;
1962  }
1963  if( !!pphong->getDiffuse() && !!pphong->getDiffuse()->getColor() ) {
1964  domFx_color c = pphong->getDiffuse()->getColor()->getValue();
1965  mat.diffuseColor[0] = c[0];
1966  mat.diffuseColor[1] = c[1];
1967  mat.diffuseColor[2] = c[2];
1968  }
1969  }
1970  domProfile_common::domTechnique::domLambertRef plambert = daeSafeCast<domProfile_common::domTechnique::domLambert>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domLambert::ID())));
1971  if( !!plambert ) {
1972  if( !!plambert->getAmbient() && !!plambert->getAmbient()->getColor() ) {
1973  domFx_color c = plambert->getAmbient()->getColor()->getValue();
1974  mat.ambientIntensity = (fabs(c[0])+fabs(c[1])+fabs(c[2]))/3;
1975  }
1976  if( !!plambert->getDiffuse() && !!plambert->getDiffuse()->getColor() ) {
1977  domFx_color c = plambert->getDiffuse()->getColor()->getValue();
1978  mat.diffuseColor[0] = c[0];
1979  mat.diffuseColor[1] = c[1];
1980  mat.diffuseColor[2] = c[2];
1981  }
1982  }
1983  }
1984  }
1985  }
1986 
1988  void ExtractRobotManipulators(BodyInfoCollada_impl* probot, const domArticulated_systemRef as)
1989  {
1990  for(size_t ie = 0; ie < as->getExtra_array().getCount(); ++ie) {
1991  domExtraRef pextra = as->getExtra_array()[ie];
1992  if( strcmp(pextra->getType(), "manipulator") == 0 ) {
1993  string name = pextra->getAttribute("name");
1994  if( name.size() == 0 ) {
1995  name = str(boost::format("manipulator%d")%_nGlobalManipulatorId++);
1996  }
1997  domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
1998  if( !!tec ) {
1999 
2000  }
2001  else {
2002  COLLADALOG_WARN(str(boost::format("cannot create robot manipulator %s")%name));
2003  }
2004  }
2005  }
2006  }
2007 
2009  void ExtractRobotAttachedSensors(BodyInfoCollada_impl* probot, const domArticulated_systemRef as)
2010  {
2011  for (size_t ie = 0; ie < as->getExtra_array().getCount(); ie++) {
2012  domExtraRef pextra = as->getExtra_array()[ie];
2013  if( strcmp(pextra->getType(), "attach_sensor") == 0 ) {
2014  string name = pextra->getAttribute("name");
2015  if( name.size() == 0 ) {
2016  name = str(boost::format("sensor%d")%_nGlobalSensorId++);
2017  }
2018  domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
2019  if( !!tec ) {
2020  daeElement* pframe_origin = tec->getChild("frame_origin");
2021  if( !!pframe_origin ) {
2022  domLinkRef pdomlink = daeSafeCast<domLink>(daeSidRef(pframe_origin->getAttribute("link"), as).resolve().elt);
2023  SensorInfo_var psensor(new SensorInfo());
2024  psensor->name = CORBA::string_dup( name.c_str() );
2025  boost::shared_ptr<LinkInfo> plink;
2026  if( _mapLinkNames.find(_ExtractLinkName(pdomlink)) != _mapLinkNames.end() ) {
2027  plink = _mapLinkNames[_ExtractLinkName(pdomlink)];
2028  } else {
2029  COLLADALOG_WARN(str(boost::format("unknown joint %s")%_ExtractLinkName(pdomlink)));
2030  }
2031 
2032  if( plink && _ExtractSensor(psensor,tec->getChild("instance_sensor")) ) {
2033 
2034  DblArray12 ttemp;
2035  _ExtractFullTransformFromChildren(ttemp, pframe_origin);
2036  //std::cerr << psensor->type << std::endl;
2037  if ( string(psensor->type) == "Vision" ) {
2038  DblArray4 quat;
2039  DblArray12 rotation, ttemp2;
2040  QuatFromAxisAngle(quat, Vector4(1,0,0,M_PI));
2041  PoseFromQuat(rotation,quat);
2042  PoseMult(ttemp2,ttemp,rotation);
2043  AxisAngleTranslationFromPose(psensor->rotation,psensor->translation,ttemp2);
2044  } else {
2045  AxisAngleTranslationFromPose(psensor->rotation,psensor->translation,ttemp);
2046  }
2047 
2048  int numSensors = plink->sensors.length();
2049  plink->sensors.length(numSensors + 1);
2050  plink->sensors[numSensors] = psensor;
2051  } else {
2052  COLLADALOG_WARN(str(boost::format("cannot find instance_sensor for attached sensor %s:%s")%probot->name_%name));
2053  }
2054  }
2055  }
2056  else {
2057  COLLADALOG_WARN(str(boost::format("cannot create robot attached sensor %s")%name));
2058  }
2059  }
2060  }
2061  }
2062 
2064  void ExtractRobotAttachedActuators(BodyInfoCollada_impl* probot, const domArticulated_systemRef as)
2065  {
2066  for (size_t ie = 0; ie < as->getExtra_array().getCount(); ie++) {
2067  domExtraRef pextra = as->getExtra_array()[ie];
2068  if( strcmp(pextra->getType(), "attach_actuator") == 0 ) {
2069  string name = pextra->getAttribute("name");
2070  if( name.size() == 0 ) {
2071  name = str(boost::format("actuator%d")%_nGlobalActuatorId++);
2072  }
2073  domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
2074  if( !!tec ) {
2075  if ( GetLink(name) && _ExtractActuator(GetLink(name), tec->getChild("instance_actuator")) ) {
2076  } else {
2077  COLLADALOG_WARN(str(boost::format("cannot find instance_actuator for attached sensor %s:%s")%probot->name_%name));
2078  }
2079  }
2080  else {
2081  COLLADALOG_WARN(str(boost::format("cannot create robot attached actuators %s")%name));
2082  }
2083  }
2084  }
2085  }
2086 
2088  bool _ExtractSensor(SensorInfo &psensor, daeElementRef instance_sensor)
2089  {
2090  if( !instance_sensor ) {
2091  COLLADALOG_WARN("could not find instance_sensor");
2092  return false;
2093  }
2094  if( !instance_sensor->hasAttribute("url") ) {
2095  COLLADALOG_WARN("instance_sensor has no url");
2096  return false;
2097  }
2098 
2099  std::string instance_id = instance_sensor->getAttribute("id");
2100  std::string instance_url = instance_sensor->getAttribute("url");
2101  daeElementRef domsensor = _getElementFromUrl(daeURI(*instance_sensor,instance_url));
2102  if( !domsensor ) {
2103  COLLADALOG_WARN(str(boost::format("failed to find senor id %s url=%s")%instance_id%instance_url));
2104  return false;
2105  }
2106  if( !domsensor->hasAttribute("type") ) {
2107  COLLADALOG_WARN("collada <sensor> needs type attribute");
2108  return false;
2109  }
2110  if ( domsensor->hasAttribute("sid")) {
2111  psensor.id = boost::lexical_cast<int>(domsensor->getAttribute("sid"));
2112  } else {
2113  psensor.id = 0;
2114  }
2115  std::string sensortype = domsensor->getAttribute("type");
2116  if ( sensortype == "base_imu" ) {// AccelerationSensor // GyroSeesor
2117  psensor.specValues.length( CORBA::ULong(3) );
2118  daeElement *max_angular_velocity = domsensor->getChild("max_angular_velocity");
2119  daeElement *max_acceleration = domsensor->getChild("max_acceleration");
2120  if ( !! max_angular_velocity ) {
2121  istringstream ins(max_angular_velocity->getCharData());
2122  ins >> psensor.specValues[0] >> psensor.specValues[1] >> psensor.specValues[2];
2123  psensor.type = CORBA::string_dup( "RateGyro" );
2124  } else if ( !! max_acceleration ) {
2125  istringstream ins(max_acceleration->getCharData());
2126  ins >> psensor.specValues[0] >> psensor.specValues[1] >> psensor.specValues[2];
2127  psensor.type = CORBA::string_dup( "Acceleration" );
2128  } else {
2129  COLLADALOG_WARN(str(boost::format("couldn't find max_angular_velocity nor max_acceleration%s")%sensortype));
2130  }
2131  return true;
2132  }
2133  if ( sensortype == "base_pinhole_camera" ) { // VisionSensor
2134  psensor.type = CORBA::string_dup( "Vision" );
2135  psensor.specValues.length( CORBA::ULong(7) );
2136  //psensor.specValues[0] = frontClipDistance
2137  //psensor.specValues[1] = backClipDistance
2138  psensor.specValues[1] = 10;
2139  //psensor.specValues[2] = fieldOfView
2140  //psensor.specValues[3] = OpenHRP::Camera::COLOR; // type
2141  //psensor.specValues[4] = width
2142  //psensor.specValues[5] = height
2143  //psensor.specValues[6] = frameRate
2144  daeElement *measurement_time = domsensor->getChild("measurement_time");
2145  if ( !! measurement_time ) {
2146  psensor.specValues[6] = 1.0/(boost::lexical_cast<double>(measurement_time->getCharData()));
2147  } else {
2148  COLLADALOG_WARN(str(boost::format("couldn't find measurement_time %s")%sensortype));
2149  }
2150  daeElement *focal_length = domsensor->getChild("focal_length");
2151  if ( !! focal_length ) {
2152  psensor.specValues[0] = boost::lexical_cast<double>(focal_length->getCharData());
2153  } else {
2154  COLLADALOG_WARN(str(boost::format("couldn't find focal_length %s")%sensortype));
2155  psensor.specValues[0] = 0.1;
2156  }
2157  daeElement *image_format = domsensor->getChild("format");
2158  std::string string_format = "uint8";
2159  if ( !! image_format ) {
2160  string_format = image_format->getCharData();
2161  }
2162  daeElement *intrinsic = domsensor->getChild("intrinsic");
2163  if ( !! intrinsic ) {
2164  istringstream ins(intrinsic->getCharData());
2165  float f0,f1,f2,f3,f4,f5;
2166  ins >> f0 >> f1 >> f2 >> f3 >> f4 >> f5;
2167  psensor.specValues[2] = atan( f2 / f0) * 2.0;
2168  } else {
2169  COLLADALOG_WARN(str(boost::format("couldn't find intrinsic %s")%sensortype));
2170  psensor.specValues[2] = 0.785398;
2171  }
2172  daeElement *image_dimensions = domsensor->getChild("image_dimensions");
2173  if ( !! image_dimensions ) {
2174  istringstream ins(image_dimensions->getCharData());
2175  int ichannel;
2176  ins >> psensor.specValues[4] >> psensor.specValues[5] >> ichannel;
2177  switch (ichannel) {
2178  case 1:
2179  if ( string_format == "uint8") {
2180  psensor.specValues[3] = OpenHRP::Camera::MONO;
2181  } else if ( string_format == "float32") {
2182  psensor.specValues[3] = OpenHRP::Camera::DEPTH;
2183  } else {
2184  COLLADALOG_WARN(str(boost::format("unknown image format %s %d")%string_format%ichannel));
2185  }
2186  break;
2187  case 2:
2188  if ( string_format == "float32") {
2189  psensor.specValues[3] = OpenHRP::Camera::MONO_DEPTH;
2190  } else {
2191  COLLADALOG_WARN(str(boost::format("unknown image format %s %d")%string_format%ichannel));
2192  }
2193  break;
2194  case 3:
2195  if ( string_format == "uint8") {
2196  psensor.specValues[3] = OpenHRP::Camera::COLOR;
2197  } else {
2198  COLLADALOG_WARN(str(boost::format("unknown image format %s %d")%string_format%ichannel));
2199  }
2200  break;
2201  case 4:
2202  if ( string_format == "float32") {
2203  psensor.specValues[3] = OpenHRP::Camera::COLOR_DEPTH;
2204  } else {
2205  COLLADALOG_WARN(str(boost::format("unknown image format %s %d")%string_format%ichannel));
2206  }
2207  break;
2208  default:
2209  COLLADALOG_WARN(str(boost::format("unknown image format %s %d")%string_format%ichannel));
2210  }
2211 
2212  } else {
2213  COLLADALOG_WARN(str(boost::format("couldn't find image_dimensions %s")%sensortype));
2214  psensor.specValues[4] = 320;
2215  psensor.specValues[5] = 240;
2216  }
2217  return true;
2218  }
2219  if ( sensortype == "base_force6d" ) { // ForceSensor
2220  psensor.type = CORBA::string_dup( "Force" );
2221  psensor.specValues.length( CORBA::ULong(6) );
2222  daeElement *max_force = domsensor->getChild("load_range_force");
2223  if ( !! max_force ) {
2224  istringstream ins(max_force->getCharData());
2225  ins >> psensor.specValues[0] >> psensor.specValues[1] >> psensor.specValues[2];
2226  } else {
2227  COLLADALOG_WARN(str(boost::format("couldn't find load_range_force %s")%sensortype));
2228  }
2229  daeElement *max_torque = domsensor->getChild("load_range_torque");
2230  if ( !! max_torque ) {
2231  istringstream ins(max_torque->getCharData());
2232  ins >> psensor.specValues[3] >> psensor.specValues[4] >> psensor.specValues[5];
2233  } else {
2234  COLLADALOG_WARN(str(boost::format("couldn't findload_range_torque %s")%sensortype));
2235  }
2236  return true;
2237  }
2238  if ( sensortype == "base_laser1d" ) { // RangeSensor
2239  psensor.type = CORBA::string_dup( "Range" );
2240  psensor.specValues.length( CORBA::ULong(4) );
2241  daeElement *scan_angle = domsensor->getChild("angle_range");
2242  if ( !! scan_angle ) {
2243  psensor.specValues[0] = boost::lexical_cast<double>(scan_angle->getCharData());
2244  } else {
2245  COLLADALOG_WARN(str(boost::format("couldn't find angle_range %s")%sensortype));
2246  }
2247  daeElement *scan_step = domsensor->getChild("angle_increment");
2248  if ( !! scan_step ) {
2249  psensor.specValues[1] = boost::lexical_cast<double>(scan_step->getCharData());
2250  } else {
2251  COLLADALOG_WARN(str(boost::format("couldn't find angle_incremnet %s")%sensortype));
2252  }
2253  daeElement *scan_rate = domsensor->getChild("measurement_time");
2254  if ( !! scan_rate ) {
2255  psensor.specValues[2] = 1.0/boost::lexical_cast<double>(scan_rate->getCharData());
2256  } else {
2257  COLLADALOG_WARN(str(boost::format("couldn't find measurement_time %s")%sensortype));
2258  }
2259  daeElement *max_distance = domsensor->getChild("distance_range");
2260  if ( !! max_distance ) {
2261  psensor.specValues[3] = boost::lexical_cast<double>(max_distance->getCharData());
2262  } else {
2263  COLLADALOG_WARN(str(boost::format("couldn't find distance_range %s")%sensortype));
2264  }
2265  return true;
2266  }
2267  COLLADALOG_WARN(str(boost::format("need to create sensor type: %s")%sensortype));
2268  return false;
2269  }
2270 
2272  bool _ExtractActuator(boost::shared_ptr<LinkInfo> plink, daeElementRef instance_actuator)
2273  {
2274  if( !instance_actuator ) {
2275  return false;
2276  }
2277  if( !instance_actuator->hasAttribute("url") ) {
2278  COLLADALOG_WARN("instance_actuator has no url");
2279  return false;
2280  }
2281 
2282  std::string instance_id = instance_actuator->getAttribute("id");
2283  std::string instance_url = instance_actuator->getAttribute("url");
2284  daeElementRef domactuator = _getElementFromUrl(daeURI(*instance_actuator,instance_url));
2285  if( !domactuator ) {
2286  COLLADALOG_WARN(str(boost::format("failed to find actuator id %s url=%s")%instance_id%instance_url));
2287  return false;
2288  }
2289  if( !domactuator->hasAttribute("type") ) {
2290  COLLADALOG_WARN("collada <actuator> needs type attribute");
2291  return false;
2292  }
2293  std::string actuatortype = domactuator->getAttribute("type");
2294  daeElement *rotor_inertia = domactuator->getChild("rotor_inertia");
2295  if ( !! rotor_inertia ) {
2296  plink->rotorInertia = boost::lexical_cast<double>(rotor_inertia->getCharData());
2297  }
2298  daeElement *rotor_resistor = domactuator->getChild("rotor_resistor");
2299  if ( !! rotor_resistor ) {
2300  plink->rotorResistor = boost::lexical_cast<double>(rotor_resistor->getCharData());
2301  }
2302  daeElement *gear_ratio = domactuator->getChild("gear_ratio");
2303  if ( !! gear_ratio ) {
2304  plink->gearRatio = boost::lexical_cast<double>(gear_ratio->getCharData());
2305  }
2306  daeElement *torque_const = domactuator->getChild("torque_constant");
2307  if ( !! torque_const ) {
2308  plink->torqueConst = boost::lexical_cast<double>(torque_const->getCharData());
2309  }
2310  daeElement *encoder_pulse = domactuator->getChild("encoder_pulse");
2311  if ( !! encoder_pulse ) {
2312  plink->encoderPulse = boost::lexical_cast<double>(encoder_pulse->getCharData());
2313  }
2314  daeElement *nom_torque = domactuator->getChild("nominal_torque");
2315  if ( !! nom_torque ) {
2316  if(plink->climit.length() < 1) plink->climit.length(1);
2317  plink->climit[0] = boost::lexical_cast<double>(nom_torque->getCharData()) / ( plink->gearRatio * plink->torqueConst );
2318  }
2319  return true;
2320  }
2321 
2322  inline daeElementRef _getElementFromUrl(const daeURI &uri)
2323  {
2324  return daeStandardURIResolver(*_dae).resolveElement(uri);
2325  }
2326 
2327  static daeElement* searchBinding(domCommon_sidref_or_paramRef paddr, daeElementRef parent)
2328  {
2329  if( !!paddr->getSIDREF() ) {
2330  return daeSidRef(paddr->getSIDREF()->getValue(),parent).resolve().elt;
2331  }
2332  if (!!paddr->getParam()) {
2333  return searchBinding(paddr->getParam()->getValue(),parent);
2334  }
2335  return NULL;
2336  }
2337 
2341  static daeElement* searchBinding(daeString ref, daeElementRef parent)
2342  {
2343  if( !parent ) {
2344  return NULL;
2345  }
2346  daeElement* pelt = NULL;
2347  domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene>(parent.cast());
2348  if( !!kscene ) {
2349  pelt = searchBindingArray(ref,kscene->getInstance_articulated_system_array());
2350  if( !!pelt ) {
2351  return pelt;
2352  }
2353  return searchBindingArray(ref,kscene->getInstance_kinematics_model_array());
2354  }
2355  domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system>(parent.cast());
2356  if( !!articulated_system ) {
2357  if( !!articulated_system->getKinematics() ) {
2358  pelt = searchBindingArray(ref,articulated_system->getKinematics()->getInstance_kinematics_model_array());
2359  if( !!pelt ) {
2360  return pelt;
2361  }
2362  }
2363  if( !!articulated_system->getMotion() ) {
2364  return searchBinding(ref,articulated_system->getMotion()->getInstance_articulated_system());
2365  }
2366  return NULL;
2367  }
2368  // try to get a bind array
2369  daeElementRef pbindelt;
2370  const domKinematics_bind_Array* pbindarray = NULL;
2371  const domKinematics_newparam_Array* pnewparamarray = NULL;
2372  domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(parent.cast());
2373  if( !!ias ) {
2374  pbindarray = &ias->getBind_array();
2375  pbindelt = ias->getUrl().getElement();
2376  pnewparamarray = &ias->getNewparam_array();
2377  }
2378  if( !pbindarray || !pbindelt ) {
2379  domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(parent.cast());
2380  if( !!ikm ) {
2381  pbindarray = &ikm->getBind_array();
2382  pbindelt = ikm->getUrl().getElement();
2383  pnewparamarray = &ikm->getNewparam_array();
2384  }
2385  }
2386  if( !!pbindarray && !!pbindelt ) {
2387  for (size_t ibind = 0; ibind < pbindarray->getCount(); ++ibind) {
2388  domKinematics_bindRef pbind = (*pbindarray)[ibind];
2389  if( !!pbind->getSymbol() && strcmp(pbind->getSymbol(), ref) == 0 ) {
2390  // found a match
2391  if( !!pbind->getParam() ) {
2392  //return searchBinding(pbind->getParam()->getRef(), pbindelt);
2393  return daeSidRef(pbind->getParam()->getRef(), pbindelt).resolve().elt;
2394  }
2395  else if( !!pbind->getSIDREF() ) {
2396  return daeSidRef(pbind->getSIDREF()->getValue(), pbindelt).resolve().elt;
2397  }
2398  }
2399  }
2400  for(size_t inewparam = 0; inewparam < pnewparamarray->getCount(); ++inewparam) {
2401  domKinematics_newparamRef newparam = (*pnewparamarray)[inewparam];
2402  if( !!newparam->getSid() && strcmp(newparam->getSid(), ref) == 0 ) {
2403  if( !!newparam->getSIDREF() ) { // can only bind with SIDREF
2404  return daeSidRef(newparam->getSIDREF()->getValue(),pbindelt).resolve().elt;
2405  }
2406  COLLADALOG_WARN(str(boost::format("newparam sid=%s does not have SIDREF")%newparam->getSid()));
2407  }
2408  }
2409  }
2410  COLLADALOG_WARN(str(boost::format("failed to get binding '%s' for element: %s")%ref%parent->getElementName()));
2411  return NULL;
2412  }
2413 
2414  static daeElement* searchBindingArray(daeString ref, const domInstance_articulated_system_Array& paramArray)
2415  {
2416  for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
2417  daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
2418  if( !!pelt ) {
2419  return pelt;
2420  }
2421  }
2422  return NULL;
2423  }
2424 
2425  static daeElement* searchBindingArray(daeString ref, const domInstance_kinematics_model_Array& paramArray)
2426  {
2427  for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
2428  daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
2429  if( !!pelt ) {
2430  return pelt;
2431  }
2432  }
2433  return NULL;
2434  }
2435 
2436  template <typename U> static xsBoolean resolveBool(domCommon_bool_or_paramRef paddr, const U& parent) {
2437  if( !!paddr->getBool() ) {
2438  return paddr->getBool()->getValue();
2439  }
2440  if( !paddr->getParam() ) {
2441  COLLADALOG_WARN("param not specified, setting to 0");
2442  return false;
2443  }
2444  for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
2445  domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
2446  if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
2447  if( !!pnewparam->getBool() ) {
2448  return pnewparam->getBool()->getValue();
2449  }
2450  else if( !!pnewparam->getSIDREF() ) {
2451  domKinematics_newparam::domBoolRef ptarget = daeSafeCast<domKinematics_newparam::domBool>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt);
2452  if( !ptarget ) {
2453  COLLADALOG_WARN(str(boost::format("failed to resolve %s from %s")%pnewparam->getSIDREF()->getValue()%paddr->getID()));
2454  continue;
2455  }
2456  return ptarget->getValue();
2457  }
2458  }
2459  }
2460  COLLADALOG_WARN(str(boost::format("failed to resolve %s")%paddr->getParam()->getValue()));
2461  return false;
2462  }
2463  template <typename U> static domFloat resolveFloat(domCommon_float_or_paramRef paddr, const U& parent) {
2464  if( !!paddr->getFloat() ) {
2465  return paddr->getFloat()->getValue();
2466  }
2467  if( !paddr->getParam() ) {
2468  COLLADALOG_WARN("param not specified, setting to 0");
2469  return 0;
2470  }
2471  for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
2472  domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
2473  if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
2474  if( !!pnewparam->getFloat() ) {
2475  return pnewparam->getFloat()->getValue();
2476  }
2477  else if( !!pnewparam->getSIDREF() ) {
2478  domKinematics_newparam::domFloatRef ptarget = daeSafeCast<domKinematics_newparam::domFloat>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt);
2479  if( !ptarget ) {
2480  COLLADALOG_WARN(str(boost::format("failed to resolve %s from %s")%pnewparam->getSIDREF()->getValue()%paddr->getID()));
2481  continue;
2482  }
2483  return ptarget->getValue();
2484  }
2485  }
2486  }
2487  COLLADALOG_WARN(str(boost::format("failed to resolve %s")%paddr->getParam()->getValue()));
2488  return 0;
2489  }
2490 
2491  static bool resolveCommon_float_or_param(daeElementRef pcommon, daeElementRef parent, float& f)
2492  {
2493  daeElement* pfloat = pcommon->getChild("float");
2494  if( !!pfloat ) {
2495  stringstream sfloat(pfloat->getCharData());
2496  sfloat >> f;
2497  return !!sfloat;
2498  }
2499  daeElement* pparam = pcommon->getChild("param");
2500  if( !!pparam ) {
2501  if( pparam->hasAttribute("ref") ) {
2502  COLLADALOG_WARN("cannot process param ref");
2503  }
2504  else {
2505  daeElement* pelt = daeSidRef(pparam->getCharData(),parent).resolve().elt;
2506  if( !!pelt ) {
2507  COLLADALOG_WARN(str(boost::format("found param ref: %s from %s")%pelt->getCharData()%pparam->getCharData()));
2508  }
2509  }
2510  }
2511  return false;
2512  }
2513 
2515  void getTransform(DblArray12& tres, daeElementRef pelt)
2516  {
2517  domRotateRef protate = daeSafeCast<domRotate>(pelt);
2518  if( !!protate ) {
2519  DblArray4 quat;
2520  QuatFromAxisAngle(quat, protate->getValue(),M_PI/180.0);
2521  PoseFromQuat(tres,quat);
2522  return;
2523  }
2524 
2525  dReal fscale = _GetUnitScale(pelt,_fGlobalScale);
2526  domTranslateRef ptrans = daeSafeCast<domTranslate>(pelt);
2527  if( !!ptrans ) {
2528  PoseIdentity(tres);
2529  tres[3] = ptrans->getValue()[0]*fscale;
2530  tres[7] = ptrans->getValue()[1]*fscale;
2531  tres[11] = ptrans->getValue()[2]*fscale;
2532  return;
2533  }
2534 
2535  domMatrixRef pmat = daeSafeCast<domMatrix>(pelt);
2536  if( !!pmat ) {
2537  for(int i = 0; i < 12; ++i) {
2538  tres[i] = pmat->getValue()[i];
2539  }
2540  tres[3] *= fscale;
2541  tres[7] *= fscale;
2542  tres[11] *= fscale;
2543  return;
2544  }
2545 
2546  PoseIdentity(tres);
2547  domScaleRef pscale = daeSafeCast<domScale>(pelt);
2548  if( !!pscale ) {
2549  tres[0] = pscale->getValue()[0];
2550  tres[5] = pscale->getValue()[1];
2551  tres[10] = pscale->getValue()[2];
2552  return;
2553  }
2554 
2555  domLookatRef pcamera = daeSafeCast<domLookat>(pelt);
2556  if( pelt->typeID() == domLookat::ID() ) {
2557  COLLADALOG_ERROR("lookat not implemented");
2558  return;
2559  }
2560 
2561  domSkewRef pskew = daeSafeCast<domSkew>(pelt);
2562  if( !!pskew ) {
2563  COLLADALOG_ERROR("skew transform not implemented");
2564  return;
2565  }
2566  }
2567 
2570  template <typename T> void getNodeParentTransform(DblArray12& tres, const T pelt) {
2571  PoseIdentity(tres);
2572  domNodeRef pnode = daeSafeCast<domNode>(pelt->getParent());
2573  if( !!pnode ) {
2574  DblArray12 ttemp, ttemp2;
2575  _ExtractFullTransform(ttemp, pnode);
2576  getNodeParentTransform(tres, pnode);
2577  PoseMult(ttemp2,tres,ttemp);
2578  memcpy(&tres[0],&ttemp2[0],sizeof(DblArray12));
2579  }
2580  }
2581 
2583  template <typename T> void _ExtractFullTransform(DblArray12& tres, const T pelt) {
2584  PoseIdentity(tres);
2585  DblArray12 ttemp, ttemp2;
2586  for(size_t i = 0; i < pelt->getContents().getCount(); ++i) {
2587  getTransform(ttemp, pelt->getContents()[i]);
2588  PoseMult(ttemp2,tres,ttemp);
2589  memcpy(&tres[0],&ttemp2[0],sizeof(DblArray12));
2590  }
2591  }
2592 
2594  template <typename T> void _ExtractFullTransformFromChildren(DblArray12& tres, const T pelt) {
2595  PoseIdentity(tres);
2596  DblArray12 ttemp, ttemp2;
2597  daeTArray<daeElementRef> children;
2598  pelt->getChildren(children);
2599  for(size_t i = 0; i < children.getCount(); ++i) {
2600  getTransform(ttemp, children[i]);
2601  PoseMult(ttemp2,tres,ttemp);
2602  memcpy(&tres[0],&ttemp2[0],sizeof(DblArray12));
2603  }
2604  }
2605 
2606  // decompose a matrix into a scale and rigid transform (necessary for model scales)
2607 // static void _decompose(const DblArray12& tm, DblArray12& tout, DblArray3& vscale)
2608 // {
2609 // DblArray4 quat;
2610 // QuatFromMatrix(quat,tm);
2611 // PoseFromQuat(tout,quat);
2612 // tout[3] = tm[3];
2613 // tout[7] = tm[7];
2614 // tout[11] = tm[11];
2615 // for(int i = 0; i < 3; ++i) {
2616 // vscale[i] = (fabs(tm[0+i])+fabs(tm[4+i])+fabs(tm[8+i]))/(fabs(tout[0+i])+fabs(tout[4+i])+fabs(tout[8+i]));
2617 // }
2618 // }
2619 
2620  virtual void handleError( daeString msg )
2621  {
2622  if( _bOpeningZAE && (msg == string("Document is empty\n") || msg == string("Error parsing XML in daeLIBXMLPlugin::read\n")) ) {
2623  return; // collada-dom prints these messages even if no error
2624  }
2625  COLLADALOG_ERROR(str(boost::format("COLLADA error: %s")%msg));
2626  }
2627 
2628  virtual void handleWarning( daeString msg )
2629  {
2630  COLLADALOG_WARN(str(boost::format("COLLADA warning: %s")%msg));
2631  }
2632 
2633  private:
2634 
2639  static void _ExtractKinematicsVisualBindings(domInstance_with_extraRef viscene, domInstance_kinematics_sceneRef kiscene, KinematicsSceneBindings& bindings)
2640  {
2641  domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
2642  if (!kscene) {
2643  return;
2644  }
2645  for (size_t imodel = 0; imodel < kiscene->getBind_kinematics_model_array().getCount(); imodel++) {
2646  domArticulated_systemRef articulated_system; // if filled, contains robot-specific information, so create a robot
2647  domBind_kinematics_modelRef kbindmodel = kiscene->getBind_kinematics_model_array()[imodel];
2648  if (!kbindmodel->getNode()) {
2649  COLLADALOG_WARN("do not support kinematics models without references to nodes");
2650  continue;
2651  }
2652 
2653  // visual information
2654  domNodeRef node = daeSafeCast<domNode>(daeSidRef(kbindmodel->getNode(), viscene->getUrl().getElement()).resolve().elt);
2655  if (!node) {
2656  COLLADALOG_WARN(str(boost::format("bind_kinematics_model does not reference valid node %sn")%kbindmodel->getNode()));
2657  continue;
2658  }
2659 
2660  // kinematics information
2661  daeElement* pelt = searchBinding(kbindmodel,kscene);
2662  domInstance_kinematics_modelRef kimodel = daeSafeCast<domInstance_kinematics_model>(pelt);
2663  if (!kimodel) {
2664  if( !pelt ) {
2665  COLLADALOG_WARN("bind_kinematics_model does not reference element");
2666  }
2667  else {
2668  COLLADALOG_WARN(str(boost::format("bind_kinematics_model cannot find reference to %s:%s:n")%kbindmodel->getNode()%pelt->getElementName()));
2669  }
2670  continue;
2671  }
2672  bindings.listKinematicsVisualBindings.push_back(make_pair(node,kimodel));
2673  }
2674  // axis info
2675  for (size_t ijoint = 0; ijoint < kiscene->getBind_joint_axis_array().getCount(); ++ijoint) {
2676  domBind_joint_axisRef bindjoint = kiscene->getBind_joint_axis_array()[ijoint];
2677  daeElementRef pjtarget = daeSidRef(bindjoint->getTarget(), viscene->getUrl().getElement()).resolve().elt;
2678  if (!pjtarget) {
2679  COLLADALOG_WARN(str(boost::format("Target Node '%s' not found")%bindjoint->getTarget()));
2680  continue;
2681  }
2682  daeElement* pelt = searchBinding(bindjoint->getAxis(),kscene);
2683  domAxis_constraintRef pjointaxis = daeSafeCast<domAxis_constraint>(pelt);
2684  if (!pjointaxis) {
2685  COLLADALOG_WARN(str(boost::format("joint axis for target %s")%bindjoint->getTarget()));
2686  continue;
2687  }
2688  bindings.listAxisBindings.push_back(JointAxisBinding(pjtarget, pjointaxis, bindjoint->getValue(), NULL, NULL));
2689  }
2690  }
2691 
2692  static void _ExtractPhysicsBindings(domCOLLADA::domSceneRef allscene, KinematicsSceneBindings& bindings)
2693  {
2694  for(size_t iphysics = 0; iphysics < allscene->getInstance_physics_scene_array().getCount(); ++iphysics) {
2695  domPhysics_sceneRef pscene = daeSafeCast<domPhysics_scene>(allscene->getInstance_physics_scene_array()[iphysics]->getUrl().getElement().cast());
2696  for(size_t imodel = 0; imodel < pscene->getInstance_physics_model_array().getCount(); ++imodel) {
2697  domInstance_physics_modelRef ipmodel = pscene->getInstance_physics_model_array()[imodel];
2698  domPhysics_modelRef pmodel = daeSafeCast<domPhysics_model> (ipmodel->getUrl().getElement().cast());
2699  domNodeRef nodephysicsoffset = daeSafeCast<domNode>(ipmodel->getParent().getElement().cast());
2700  for(size_t ibody = 0; ibody < ipmodel->getInstance_rigid_body_array().getCount(); ++ibody) {
2701  LinkBinding lb;
2702  lb.irigidbody = ipmodel->getInstance_rigid_body_array()[ibody];
2703  lb.node = daeSafeCast<domNode>(lb.irigidbody->getTarget().getElement().cast());
2704  lb.rigidbody = daeSafeCast<domRigid_body>(daeSidRef(lb.irigidbody->getBody(),pmodel).resolve().elt);
2705  lb.nodephysicsoffset = nodephysicsoffset;
2706  if( !!lb.rigidbody && !!lb.node ) {
2707  bindings.listLinkBindings.push_back(lb);
2708  }
2709  }
2710  for(size_t iconst = 0; iconst < ipmodel->getInstance_rigid_constraint_array().getCount(); ++iconst) {
2711  ConstraintBinding cb;
2712  cb.irigidconstraint = ipmodel->getInstance_rigid_constraint_array()[iconst];
2713  cb.rigidconstraint = daeSafeCast<domRigid_constraint>(daeSidRef(cb.irigidconstraint->getConstraint(),pmodel).resolve().elt);
2714  cb.node = daeSafeCast<domNode>(cb.rigidconstraint->getAttachment()->getRigid_body().getElement());
2715  if( !!cb.rigidconstraint ) {
2716  bindings.listConstraintBindings.push_back(cb);
2717  }
2718  }
2719  }
2720  }
2721  }
2722 
2723  domTechniqueRef _ExtractOpenRAVEProfile(const domTechnique_Array& arr)
2724  {
2725  for(size_t i = 0; i < arr.getCount(); ++i) {
2726  if( strcmp(arr[i]->getProfile(), "OpenRAVE") == 0 ) {
2727  return arr[i];
2728  }
2729  }
2730  return domTechniqueRef();
2731  }
2732 
2733  daeElementRef _ExtractOpenRAVEProfile(const daeElementRef pelt)
2734  {
2735  daeTArray<daeElementRef> children;
2736  pelt->getChildren(children);
2737  for(size_t i = 0; i < children.getCount(); ++i) {
2738  if( children[i]->getElementName() == string("technique") && children[i]->hasAttribute("profile") && children[i]->getAttribute("profile") == string("OpenRAVE") ) {
2739  return children[i];
2740  }
2741  }
2742  return daeElementRef();
2743  }
2744 
2746  boost::shared_ptr<std::string> _ExtractInterfaceType(const daeElementRef pelt) {
2747  daeTArray<daeElementRef> children;
2748  pelt->getChildren(children);
2749  for(size_t i = 0; i < children.getCount(); ++i) {
2750  if( children[i]->getElementName() == string("interface_type") ) {
2751  daeElementRef ptec = _ExtractOpenRAVEProfile(children[i]);
2752  if( !!ptec ) {
2753  daeElement* ptype = ptec->getChild("interface");
2754  if( !!ptype ) {
2755  return boost::shared_ptr<std::string>(new std::string(ptype->getCharData()));
2756  }
2757  }
2758  }
2759  }
2760  return boost::shared_ptr<std::string>();
2761  }
2762 
2764  boost::shared_ptr<std::string> _ExtractInterfaceType(const domExtra_Array& arr) {
2765  for(size_t i = 0; i < arr.getCount(); ++i) {
2766  if( strcmp(arr[i]->getType(),"interface_type") == 0 ) {
2767  domTechniqueRef tec = _ExtractOpenRAVEProfile(arr[i]->getTechnique_array());
2768  if( !!tec ) {
2769  daeElement* ptype = tec->getChild("interface");
2770  if( !!ptype ) {
2771  return boost::shared_ptr<std::string>(new std::string(ptype->getCharData()));
2772  }
2773  }
2774  }
2775  }
2776  return boost::shared_ptr<std::string>();
2777  }
2778 
2779  std::string _ExtractLinkName(domLinkRef pdomlink) {
2780  std::string linkname;
2781  if( !!pdomlink ) {
2782  if( !!pdomlink->getName() ) {
2783  linkname = pdomlink->getName();
2784  }
2785  if( linkname.size() == 0 && !!pdomlink->getID() ) {
2786  linkname = pdomlink->getID();
2787  }
2788  }
2789  return _ConvertToValidName(linkname);
2790  }
2791 
2792  bool _checkMathML(daeElementRef pelt,const string& type)
2793  {
2794  if( pelt->getElementName()==type ) {
2795  return true;
2796  }
2797  // check the substring after ':', the substring before is the namespace set in some parent attribute
2798  string name = pelt->getElementName();
2799  size_t pos = name.find_last_of(':');
2800  if( pos == string::npos ) {
2801  return false;
2802  }
2803  return name.substr(pos+1)==type;
2804  }
2805 
2806  std::pair<boost::shared_ptr<LinkInfo> ,domJointRef> _getJointFromRef(xsToken targetref, daeElementRef peltref, BodyInfoCollada_impl* pkinbody) {
2807  daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt;
2808  domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
2809  if (!pdomjoint) {
2810  domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
2811  if (!!pdomijoint) {
2812  pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
2813  }
2814  }
2815 
2816  if (!pdomjoint) {
2817  COLLADALOG_WARN(str(boost::format("could not find collada joint '%s'!")%targetref));
2818  return std::make_pair(boost::shared_ptr<LinkInfo>(),domJointRef());
2819  }
2820 
2821  if( string(targetref).find("./") != 0 ) {
2822  std::map<std::string,boost::shared_ptr<LinkInfo> >::iterator itjoint = _mapJointIds.find(targetref);
2823  if( itjoint != _mapJointIds.end() ) {
2824  return std::make_pair(itjoint->second,pdomjoint);
2825  }
2826  COLLADALOG_WARN(str(boost::format("failed to find joint target '%s' in _mapJointIds")%targetref));
2827  }
2828 
2829  boost::shared_ptr<LinkInfo> pjoint = GetLink(pdomjoint->getName());
2830  if(!pjoint) {
2831  COLLADALOG_WARN(str(boost::format("could not find openrave joint '%s'!")%pdomjoint->getName()));
2832  }
2833  return std::make_pair(pjoint,pdomjoint);
2834  }
2835 
2837  std::string _getElementName(daeElementRef pelt) {
2838  std::string name = pelt->getElementName();
2839  std::size_t pos = name.find_last_of(':');
2840  if( pos != string::npos ) {
2841  return name.substr(pos+1);
2842  }
2843  return name;
2844  }
2845 
2846  std::string _ExtractParentId(daeElementRef p) {
2847  while(!!p) {
2848  if( p->hasAttribute("id") ) {
2849  return p->getAttribute("id");
2850  }
2851  p = p->getParent();
2852  }
2853  return "";
2854  }
2855 
2857  std::string _ExtractMathML(daeElementRef proot, BodyInfoCollada_impl* pkinbody, daeElementRef pelt)
2858  {
2859  std::string name = _getElementName(pelt);
2860  std::string eq;
2861  daeTArray<daeElementRef> children;
2862  pelt->getChildren(children);
2863  if( name == "math" ) {
2864  for(std::size_t ic = 0; ic < children.getCount(); ++ic) {
2865  std::string childname = _getElementName(children[ic]);
2866  if( childname == "apply" || childname == "csymbol" || childname == "cn" || childname == "ci" ) {
2867  eq = _ExtractMathML(proot, pkinbody, children[ic]);
2868  }
2869  else {
2870  throw ModelLoader::ModelLoaderException(str(boost::format("_ExtractMathML: do not support element %s in mathml")%childname).c_str());
2871  }
2872  }
2873  }
2874  else if( name == "apply" ) {
2875  if( children.getCount() == 0 ) {
2876  return eq;
2877  }
2878  string childname = _getElementName(children[0]);
2879  if( childname == "plus" ) {
2880  eq += '(';
2881  for(size_t ic = 1; ic < children.getCount(); ++ic) {
2882  eq += _ExtractMathML(proot, pkinbody, children[ic]);
2883  if( ic+1 < children.getCount() ) {
2884  eq += '+';
2885  }
2886  }
2887  eq += ')';
2888  }
2889  else if( childname == "quotient" ) {
2890  COLLADA_ASSERT(children.getCount()==3);
2891  eq += str(boost::format("floor(%s/%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
2892  }
2893  else if( childname == "divide" ) {
2894  COLLADA_ASSERT(children.getCount()==3);
2895  eq += str(boost::format("(%s/%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
2896  }
2897  else if( childname == "minus" ) {
2898  COLLADA_ASSERT(children.getCount()>1 && children.getCount()<=3);
2899  if( children.getCount() == 2 ) {
2900  eq += str(boost::format("(-%s)")%_ExtractMathML(proot,pkinbody,children[1]));
2901  }
2902  else {
2903  eq += str(boost::format("(%s-%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
2904  }
2905  }
2906  else if( childname == "power" ) {
2907  COLLADA_ASSERT(children.getCount()==3);
2908  std::string sbase = _ExtractMathML(proot,pkinbody,children[1]);
2909  std::string sexp = _ExtractMathML(proot,pkinbody,children[2]);
2910 // try {
2911 // int degree = boost::lexical_cast<int>(sexp);
2912 // if( degree == 1 ) {
2913 // eq += str(boost::format("(%s)")%sbase);
2914 // }
2915 // else if( degree == 2 ) {
2916 // eq += str(boost::format("sqr(%s)")%sbase);
2917 // }
2918 // else {
2919 // eq += str(boost::format("pow(%s,%s)")%sbase%sexp);
2920 // }
2921 // }
2922 // catch(const boost::bad_lexical_cast&) {
2923  eq += str(boost::format("pow(%s,%s)")%sbase%sexp);
2924  //}
2925  }
2926  else if( childname == "rem" ) {
2927  COLLADA_ASSERT(children.getCount()==3);
2928  eq += str(boost::format("(%s%%%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
2929  }
2930  else if( childname == "times" ) {
2931  eq += '(';
2932  for(size_t ic = 1; ic < children.getCount(); ++ic) {
2933  eq += _ExtractMathML(proot, pkinbody, children[ic]);
2934  if( ic+1 < children.getCount() ) {
2935  eq += '*';
2936  }
2937  }
2938  eq += ')';
2939  }
2940  else if( childname == "root" ) {
2941  COLLADA_ASSERT(children.getCount()==3);
2942  string sdegree, snum;
2943  for(size_t ic = 1; ic < children.getCount(); ++ic) {
2944  if( _getElementName(children[ic]) == string("degree") ) {
2945  sdegree = _ExtractMathML(proot,pkinbody,children[ic]->getChildren()[0]);
2946  }
2947  else {
2948  snum = _ExtractMathML(proot,pkinbody,children[ic]);
2949  }
2950  }
2951  try {
2952  int degree = boost::lexical_cast<int>(sdegree);
2953  if( degree == 1 ) {
2954  eq += str(boost::format("(%s)")%snum);
2955  }
2956  else if( degree == 2 ) {
2957  eq += str(boost::format("sqrt(%s)")%snum);
2958  }
2959  else if( degree == 3 ) {
2960  eq += str(boost::format("cbrt(%s)")%snum);
2961  }
2962  else {
2963  eq += str(boost::format("pow(%s,1.0/%s)")%snum%sdegree);
2964  }
2965  }
2966  catch(const boost::bad_lexical_cast&) {
2967  eq += str(boost::format("pow(%s,1.0/%s)")%snum%sdegree);
2968  }
2969  }
2970  else if( childname == "and" ) {
2971  eq += '(';
2972  for(size_t ic = 1; ic < children.getCount(); ++ic) {
2973  eq += _ExtractMathML(proot, pkinbody, children[ic]);
2974  if( ic+1 < children.getCount() ) {
2975  eq += '&';
2976  }
2977  }
2978  eq += ')';
2979  }
2980  else if( childname == "or" ) {
2981  eq += '(';
2982  for(size_t ic = 1; ic < children.getCount(); ++ic) {
2983  eq += _ExtractMathML(proot, pkinbody, children[ic]);
2984  if( ic+1 < children.getCount() ) {
2985  eq += '|';
2986  }
2987  }
2988  eq += ')';
2989  }
2990  else if( childname == "not" ) {
2991  COLLADA_ASSERT(children.getCount()==2);
2992  eq += str(boost::format("(!%s)")%_ExtractMathML(proot,pkinbody,children[1]));
2993  }
2994  else if( childname == "floor" ) {
2995  COLLADA_ASSERT(children.getCount()==2);
2996  eq += str(boost::format("floor(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
2997  }
2998  else if( childname == "ceiling" ) {
2999  COLLADA_ASSERT(children.getCount()==2);
3000  eq += str(boost::format("ceil(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3001  }
3002  else if( childname == "eq" ) {
3003  COLLADA_ASSERT(children.getCount()==3);
3004  eq += str(boost::format("(%s=%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
3005  }
3006  else if( childname == "neq" ) {
3007  COLLADA_ASSERT(children.getCount()==3);
3008  eq += str(boost::format("(%s!=%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
3009  }
3010  else if( childname == "gt" ) {
3011  COLLADA_ASSERT(children.getCount()==3);
3012  eq += str(boost::format("(%s>%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
3013  }
3014  else if( childname == "lt" ) {
3015  COLLADA_ASSERT(children.getCount()==3);
3016  eq += str(boost::format("(%s<%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
3017  }
3018  else if( childname == "geq" ) {
3019  COLLADA_ASSERT(children.getCount()==3);
3020  eq += str(boost::format("(%s>=%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
3021  }
3022  else if( childname == "leq" ) {
3023  COLLADA_ASSERT(children.getCount()==3);
3024  eq += str(boost::format("(%s<=%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
3025  }
3026  else if( childname == "ln" ) {
3027  COLLADA_ASSERT(children.getCount()==2);
3028  eq += str(boost::format("log(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3029  }
3030  else if( childname == "log" ) {
3031  COLLADA_ASSERT(children.getCount()==2 || children.getCount()==3);
3032  string sbase="10", snum;
3033  for(size_t ic = 1; ic < children.getCount(); ++ic) {
3034  if( _getElementName(children[ic]) == string("logbase") ) {
3035  sbase = _ExtractMathML(proot,pkinbody,children[ic]->getChildren()[0]);
3036  }
3037  else {
3038  snum = _ExtractMathML(proot,pkinbody,children[ic]);
3039  }
3040  }
3041  try {
3042  int base = boost::lexical_cast<int>(sbase);
3043  if( base == 10 ) {
3044  eq += str(boost::format("log10(%s)")%snum);
3045  }
3046  else if( base == 2 ) {
3047  eq += str(boost::format("log2(%s)")%snum);
3048  }
3049  else {
3050  eq += str(boost::format("(log(%s)/log(%s))")%snum%sbase);
3051  }
3052  }
3053  catch(const boost::bad_lexical_cast&) {
3054  eq += str(boost::format("(log(%s)/log(%s))")%snum%sbase);
3055  }
3056  }
3057  else if( childname == "arcsin" ) {
3058  COLLADA_ASSERT(children.getCount()==2);
3059  eq += str(boost::format("asin(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3060  }
3061  else if( childname == "arccos" ) {
3062  COLLADA_ASSERT(children.getCount()==2);
3063  eq += str(boost::format("acos(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3064  }
3065  else if( childname == "arctan" ) {
3066  COLLADA_ASSERT(children.getCount()==2);
3067  eq += str(boost::format("atan(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3068  }
3069  else if( childname == "arccosh" ) {
3070  COLLADA_ASSERT(children.getCount()==2);
3071  eq += str(boost::format("acosh(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3072  }
3073  else if( childname == "arccot" ) {
3074  COLLADA_ASSERT(children.getCount()==2);
3075  eq += str(boost::format("acot(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3076  }
3077  else if( childname == "arccoth" ) {
3078  COLLADA_ASSERT(children.getCount()==2);
3079  eq += str(boost::format("acoth(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3080  }
3081  else if( childname == "arccsc" ) {
3082  COLLADA_ASSERT(children.getCount()==2);
3083  eq += str(boost::format("acsc(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3084  }
3085  else if( childname == "arccsch" ) {
3086  COLLADA_ASSERT(children.getCount()==2);
3087  eq += str(boost::format("acsch(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3088  }
3089  else if( childname == "arcsec" ) {
3090  COLLADA_ASSERT(children.getCount()==2);
3091  eq += str(boost::format("asec(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3092  }
3093  else if( childname == "arcsech" ) {
3094  COLLADA_ASSERT(children.getCount()==2);
3095  eq += str(boost::format("asech(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3096  }
3097  else if( childname == "arcsinh" ) {
3098  COLLADA_ASSERT(children.getCount()==2);
3099  eq += str(boost::format("asinh(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3100  }
3101  else if( childname == "arctanh" ) {
3102  COLLADA_ASSERT(children.getCount()==2);
3103  eq += str(boost::format("atanh(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
3104  }
3105  else if( childname == "implies" || childname == "forall" || childname == "exists" || childname == "conjugate" || childname == "arg" || childname == "real" || childname == "imaginary" || childname == "lcm" || childname == "factorial" || childname == "xor") {
3106  throw ModelLoader::ModelLoaderException(str(boost::format("_ExtractMathML: %s function in <apply> tag not supported")%childname).c_str());
3107  }
3108  else if( childname == "csymbol" ) {
3109  if( children[0]->getAttribute("encoding")==string("text/xml") ) {
3110  domFormulaRef pformula;
3111  string functionname;
3112  if( children[0]->hasAttribute("definitionURL") ) {
3113  // search for the formula in library_formulas
3114  string formulaurl = children[0]->getAttribute("definitionURL");
3115  if( formulaurl.size() > 0 ) {
3116  daeElementRef pelt = _getElementFromUrl(daeURI(*children[0],formulaurl));
3117  pformula = daeSafeCast<domFormula>(pelt);
3118  if( !pformula ) {
3119  COLLADALOG_WARN(str(boost::format("could not find csymbol %s formula")%children[0]->getAttribute("definitionURL")));
3120  }
3121  else {
3122  COLLADALOG_DEBUG(str(boost::format("csymbol formula %s found")%pformula->getId()));
3123  }
3124  }
3125  }
3126  if( !pformula ) {
3127  if( children[0]->hasAttribute("type") ) {
3128  if( children[0]->getAttribute("type") == "function" ) {
3129  functionname = children[0]->getCharData();
3130  }
3131  }
3132  }
3133  else {
3134  if( !!pformula->getName() ) {
3135  functionname = pformula->getName();
3136  }
3137  else {
3138  functionname = children[0]->getCharData();
3139  }
3140  }
3141 
3142  if( functionname == "INRANGE" ) {
3143  COLLADA_ASSERT(children.getCount()==4);
3144  string a = _ExtractMathML(proot,pkinbody,children[1]), b = _ExtractMathML(proot,pkinbody,children[2]), c = _ExtractMathML(proot,pkinbody,children[3]);
3145  eq += str(boost::format("((%s>=%s)&(%s<=%s))")%a%b%a%c);
3146  }
3147  else if( functionname == "SSSA" || functionname == "SASA" || functionname == "SASS" ) {
3148  COLLADA_ASSERT(children.getCount()==4);
3149  eq += str(boost::format("%s(%s,%s,%s)")%functionname%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2])%_ExtractMathML(proot,pkinbody,children[3]));
3150  }
3151  else if( functionname == "atan2") {
3152  COLLADA_ASSERT(children.getCount()==3);
3153  eq += str(boost::format("atan2(%s,%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
3154  }
3155  else {
3156  COLLADALOG_WARN(str(boost::format("csymbol %s not implemented")%functionname));
3157  eq += "1";
3158  }
3159  }
3160  else if( children[0]->getAttribute("encoding")!=string("COLLADA") ) {
3161  COLLADALOG_WARN(str(boost::format("_ExtractMathML: csymbol '%s' has unknown encoding '%s'")%children[0]->getCharData()%children[0]->getAttribute("encoding")));
3162  }
3163  else {
3164  eq += _ExtractMathML(proot,pkinbody,children[0]);
3165  }
3166  }
3167  else {
3168  // make a function call
3169  eq += childname;
3170  eq += '(';
3171  for(size_t ic = 1; ic < children.getCount(); ++ic) {
3172  eq += _ExtractMathML(proot, pkinbody, children[ic]);
3173  if( ic+1 < children.getCount() ) {
3174  eq += ',';
3175  }
3176  }
3177  eq += ')';
3178  }
3179  }
3180  else if( name == "csymbol" ) {
3181  if( !pelt->hasAttribute("encoding") ) {
3182  COLLADALOG_WARN(str(boost::format("_ExtractMathML: csymbol '%s' does not have any encoding")%pelt->getCharData()));
3183  }
3184  else if( pelt->getAttribute("encoding")!=string("COLLADA") ) {
3185  COLLADALOG_WARN(str(boost::format("_ExtractMathML: csymbol '%s' has unknown encoding '%s'")%pelt->getCharData()%pelt->getAttribute("encoding")));
3186  }
3187  boost::shared_ptr<LinkInfo> pjoint = _getJointFromRef(pelt->getCharData().c_str(),proot,pkinbody).first;
3188  if( !pjoint ) {
3189  COLLADALOG_WARN(str(boost::format("_ExtractMathML: failed to find csymbol: %s")%pelt->getCharData()));
3190  eq = pelt->getCharData();
3191  }
3192  string jointname(CORBA::String_var(pjoint->name));
3193  //if( pjoint->GetDOF() > 1 ) {
3194  //COLLADALOG_WARN(str(boost::format("formulas do not support using joints with > 1 DOF yet (%s)")%pjoint->GetName()));
3195  if( _mapJointUnits.find(pjoint) != _mapJointUnits.end() && _mapJointUnits[pjoint].at(0) != 1 ) {
3196  eq = str(boost::format("(%f*%s)")%(1/_mapJointUnits[pjoint].at(0))%jointname);
3197  }
3198  else {
3199  eq = jointname;
3200  }
3201  }
3202  else if( name == "cn" ) {
3203  eq = pelt->getCharData();
3204  }
3205  else if( name == "ci" ) {
3206  eq = pelt->getCharData();
3207  }
3208  else if( name == "pi" ) {
3209  eq = "3.14159265358979323846";
3210  }
3211  else {
3212  COLLADALOG_WARN(str(boost::format("mathml unprocessed tag: %s")));
3213  }
3214  return eq;
3215  }
3216 
3217  static inline bool _IsValidCharInName(char c) { return isalnum(c) || c == '_' || c == '.' || c == '-' || c == '/'; }
3218  static inline bool _IsValidName(const std::string& s) {
3219  if( s.size() == 0 ) {
3220  return false;
3221  }
3222  return std::count_if(s.begin(), s.end(), _IsValidCharInName) == (int)s.size();
3223  }
3224 
3225  inline std::string _ConvertToValidName(const std::string& name) {
3226  if( name.size() == 0 ) {
3227  return str(boost::format("__dummy%d")%_nGlobalIndex++);
3228  }
3229  if( _IsValidName(name) ) {
3230  return name;
3231  }
3232  std::string newname = name;
3233  for(size_t i = 0; i < newname.size(); ++i) {
3234  if( !_IsValidCharInName(newname[i]) ) {
3235  newname[i] = '_';
3236  }
3237  }
3238  COLLADALOG_WARN(str(boost::format("name '%s' is not a valid name, converting to '%s'")%name%newname));
3239  return newname;
3240  }
3241 
3242  inline static dReal _GetUnitScale(daeElementRef pelt, dReal startscale)
3243  {
3244  // getChild could be optimized since asset tag is supposed to appear as the first element
3245  domExtraRef pextra = daeSafeCast<domExtra> (pelt->getChild("extra"));
3246  if( !!pextra && !!pextra->getAsset() && !!pextra->getAsset()->getUnit() ) {
3247  return pextra->getAsset()->getUnit()->getMeter();
3248  }
3249  if( !!pelt->getParent() ) {
3250  return _GetUnitScale(pelt->getParent(),startscale);
3251  }
3252  return startscale;
3253  }
3254 
3255  boost::shared_ptr<DAE> _dae;
3257  domCOLLADA* _dom;
3259  std::map<boost::shared_ptr<LinkInfo> , std::vector<dReal> > _mapJointUnits;
3260  std::map<std::string,boost::shared_ptr<LinkInfo> > _mapJointIds;
3261  std::map<std::string,boost::shared_ptr<LinkInfo> > _mapLinkNames;
3262  std::vector<boost::shared_ptr<LinkInfo> > _veclinks;
3263  std::vector<std::string> _veclinknames;
3264  int _nGlobalSensorId, _nGlobalActuatorId, _nGlobalManipulatorId, _nGlobalIndex;
3265  std::string _filename;
3266  DblArray12 rootOrigin;
3267  DblArray12 visualRootOrigin;
3268 };
3269 
3270 BodyInfoCollada_impl::BodyInfoCollada_impl(PortableServer::POA_ptr poa) :
3271  ShapeSetInfo_impl(poa)
3272 {
3273  lastUpdate_ = 0;
3274 }
3275 
3276 
3278 {
3279 }
3280 
3281 const std::string& BodyInfoCollada_impl::topUrl()
3282 {
3283  return url_;
3284 }
3285 
3286 
3288 {
3289  return CORBA::string_dup(name_.c_str());
3290 }
3291 
3292 
3294 {
3295  return CORBA::string_dup(url_.c_str());
3296 }
3297 
3298 
3300 {
3301  return new StringSequence(info_);
3302 }
3303 
3304 
3305 LinkInfoSequence* BodyInfoCollada_impl::links()
3306 {
3307  return new LinkInfoSequence(links_);
3308 }
3309 
3310 
3311 AllLinkShapeIndexSequence* BodyInfoCollada_impl::linkShapeIndices()
3312 {
3313  return new AllLinkShapeIndexSequence(linkShapeIndices_);
3314 }
3315 
3316 ExtraJointInfoSequence* BodyInfoCollada_impl::extraJoints()
3317 {
3318  return new ExtraJointInfoSequence(extraJoints_);
3319 }
3320 
3321 boost::mutex BodyInfoCollada_impl::lock_;
3323 {
3324  boost::mutex::scoped_lock lock(lock_);
3326  if( !reader.InitFromURL(url) ) {
3327  throw ModelLoader::ModelLoaderException("The model file cannot be found.");
3328  }
3329  if( !reader.Extract(this) ) {
3330  throw ModelLoader::ModelLoaderException("The model file cannot be loaded.");
3331  }
3332 }
3333 
3334 void BodyInfoCollada_impl::setParam(std::string param, bool value)
3335 {
3336  if(param == "readImage") {
3337  readImage_ = value;
3338  }
3339 }
3340 
3341 bool BodyInfoCollada_impl::getParam(std::string param)
3342 {
3343  if(param == "readImage") {
3344  return readImage_;
3345  }
3346  return false;
3347 }
3348 
3349 void BodyInfoCollada_impl::setParam(std::string param, int value)
3350 {
3351  if(param == "AABBType") {
3352  AABBdataType_ = (OpenHRP::ModelLoader::AABBdataType)value;
3353  }
3354 }
3355 
3356 void BodyInfoCollada_impl::setColdetModel(ColdetModelPtr& coldetModel, TransformedShapeIndexSequence shapeIndices, const Matrix44& Tparent, int& vertexIndex, int& triangleIndex){
3357  for(unsigned int i=0; i < shapeIndices.length(); i++){
3358  setColdetModelTriangles(coldetModel, shapeIndices[i], Tparent, vertexIndex, triangleIndex);
3359  }
3360 }
3361 
3363 (ColdetModelPtr& coldetModel, const TransformedShapeIndex& tsi, const Matrix44& Tparent, int& vertexIndex, int& triangleIndex)
3364 {
3365  short shapeIndex = tsi.shapeIndex;
3366  const DblArray12& M = tsi.transformMatrix;;
3367  Matrix44 T, Tlocal;
3368  Tlocal << M[0], M[1], M[2], M[3],
3369  M[4], M[5], M[6], M[7],
3370  M[8], M[9], M[10], M[11],
3371  0.0, 0.0, 0.0, 1.0;
3372  T = Tparent * Tlocal;
3373 
3374  const ShapeInfo& shapeInfo = shapes_[shapeIndex];
3375 
3376  const FloatSequence& vertices = shapeInfo.vertices;
3377  const LongSequence& triangles = shapeInfo.triangles;
3378  const int numTriangles = triangles.length() / 3;
3379 
3380  coldetModel->setNumTriangles(coldetModel->getNumTriangles()+numTriangles);
3381  int numVertices = numTriangles * 3;
3382  coldetModel->setNumVertices(coldetModel->getNumVertices()+numVertices);
3383  for(int j=0; j < numTriangles; ++j){
3384  int vertexIndexTop = vertexIndex;
3385  for(int k=0; k < 3; ++k){
3386  long orgVertexIndex = shapeInfo.triangles[j * 3 + k];
3387  int p = orgVertexIndex * 3;
3388  Vector4 v(T * Vector4(vertices[p+0], vertices[p+1], vertices[p+2], 1.0));
3389  coldetModel->setVertex(vertexIndex++, (float)v[0], (float)v[1], (float)v[2]);
3390  }
3391  coldetModel->setTriangle(triangleIndex++, vertexIndexTop, vertexIndexTop + 1, vertexIndexTop + 2);
3392  }
3393 
3394 }
3395 
3396 void BodyInfoCollada_impl::changetoBoundingBox(unsigned int* inputData){
3397  const double EPS = 1.0e-6;
3399  std::vector<Vector3> boxSizeMap;
3400  std::vector<Vector3> boundingBoxData;
3401 
3402  for(unsigned int i=0; i<links_.length(); i++){
3403  int _depth;
3404  if( AABBdataType_ == OpenHRP::ModelLoader::AABB_NUM )
3405  _depth = linkColdetModels[i]->numofBBtoDepth(inputData[i]);
3406  else
3407  _depth = inputData[i];
3408  if( _depth >= links_[i].AABBmaxDepth)
3409  _depth = links_[i].AABBmaxDepth-1;
3410  if(_depth >= 0 ){
3411  linkColdetModels[i]->getBoundingBoxData(_depth, boundingBoxData);
3412  std::vector<TransformedShapeIndex> tsiMap;
3413  links_[i].shapeIndices.length(0);
3414  SensorInfoSequence& sensors = links_[i].sensors;
3415  for (unsigned int j=0; j<sensors.length(); j++){
3416  SensorInfo& sensor = sensors[j];
3417  sensor.shapeIndices.length(0);
3418  }
3419 
3420  for(unsigned int j=0; j<boundingBoxData.size()/2; j++){
3421 
3422  bool flg=false;
3423  unsigned int k=0;
3424  for( ; k<boxSizeMap.size(); k++)
3425  if((boxSizeMap[k] - boundingBoxData[j*2+1]).norm() < EPS)
3426  break;
3427  if( k<boxSizeMap.size() )
3428  flg=true;
3429  else{
3430  boxSizeMap.push_back(boundingBoxData[j*2+1]);
3431  setBoundingBoxData(boundingBoxData[j*2+1],k);
3432  }
3433 
3434  if(flg){
3435  unsigned int l=0;
3436  for( ; l<tsiMap.size(); l++){
3437  Vector3 p(tsiMap[l].transformMatrix[3],tsiMap[l].transformMatrix[7],tsiMap[l].transformMatrix[11]);
3438  if((p - boundingBoxData[j*2]).norm() < EPS && tsiMap[l].shapeIndex == (int)k)
3439  break;
3440  }
3441  if( l==tsiMap.size() )
3442  flg=false;
3443  }
3444 
3445  if(!flg){
3446  int num = links_[i].shapeIndices.length();
3447  links_[i].shapeIndices.length(num+1);
3448  TransformedShapeIndex& tsi = links_[i].shapeIndices[num];
3449  tsi.inlinedShapeTransformMatrixIndex = -1;
3450  tsi.shapeIndex = k;
3451  Matrix44 T(Matrix44::Identity());
3452  for(int p = 0,row=0; row < 3; ++row)
3453  for(int col=0; col < 4; ++col)
3454  if(col==3){
3455  switch(row){
3456  case 0:
3457  tsi.transformMatrix[p++] = boundingBoxData[j*2][0];
3458  break;
3459  case 1:
3460  tsi.transformMatrix[p++] = boundingBoxData[j*2][1];
3461  break;
3462  case 2:
3463  tsi.transformMatrix[p++] = boundingBoxData[j*2][2];
3464  break;
3465  default:
3466  ;
3467  }
3468  }else
3469  tsi.transformMatrix[p++] = T(row, col);
3470 
3471  tsiMap.push_back(tsi);
3472  }
3473  }
3474  }
3475  linkShapeIndices_[i] = links_[i].shapeIndices;
3476  }
3477 }
3478 
3480 {
3481  bool ret=true;
3482  for( std::map<std::string, time_t>::iterator it=fileTimeMap.begin(); it != fileTimeMap.end(); ++it){
3483  struct stat statbuff;
3484  time_t mtime = 0;
3485  if( stat( it->first.c_str(), &statbuff ) == 0 ){
3486  mtime = statbuff.st_mtime;
3487  }
3488  if(it->second!=mtime){
3489  ret=false;
3490  break;
3491  }
3492  }
3493  return ret;
3494 }
inter-collada bindings for a kinematics scene
void ExtractRobotAttachedSensors(BodyInfoCollada_impl *probot, const domArticulated_systemRef as)
Extract Sensors attached to a Robot.
virtual AllLinkShapeIndexSequence * linkShapeIndices()
AllLinkShapeIndexSequence linkShapeIndices_
bool ExtractKinematicsModel(BodyInfoCollada_impl *pkinbody, domInstance_kinematics_modelRef ikm, KinematicsSceneBindings &bindings)
std::vector< ColdetModelPtr > linkColdetModels
bool getParam(std::string param)
std::list< JointAxisBinding > listAxisBindings
void _ExtractFullTransform(DblArray12 &tres, const T pelt)
Travel by the transformation array and calls the getTransform method.
int c
Definition: autoplay.py:16
bool ExtractGeometry(BodyInfoCollada_impl *pkinbody, boost::shared_ptr< LinkInfo > plink, const domGeometryRef geom, const map< string, int > &mapmaterials, const map< string, int > &maptextures)
static boost::mutex lock_
void PoseInverse(OpenHRP::DblArray12 &poseinv, const OpenHRP::DblArray12 &pose)
Definition: ColladaUtil.h:124
ExtraJointInfoSequence extraJoints_
void setBoundingBoxData(const Vector3 &boxSize, int shapeIndex)
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
std::string _getElementName(daeElementRef pelt)
get the element name without the namespace
void getNodeParentTransform(DblArray12 &tres, const T pelt)
bool _checkMathML(daeElementRef pelt, const string &type)
char * filename
Definition: cdjpeg.h:133
void COLLADALOG_DEBUG(const std::string &s)
Definition: ColladaUtil.h:84
void PoseMult(OpenHRP::DblArray12 &mres, const T &m0, const OpenHRP::DblArray12 &m1)
Definition: ColladaUtil.h:133
* x
Definition: IceUtils.h:98
static dReal _GetUnitScale(daeElementRef pelt, dReal startscale)
boost::shared_ptr< std::string > _ExtractInterfaceType(const daeElementRef pelt)
returns an openrave interface type from the extra array
domCommon_float_or_paramRef jointvalue
bool ExtractGeometry(BodyInfoCollada_impl *pkinbody, boost::shared_ptr< LinkInfo > plink, const DblArray12 &tlink, const domNodeRef pdomnode, const std::list< JointAxisBinding > &listAxisBindings, const std::vector< std::string > &vprocessednodes)
void PostProcess(BodyInfoCollada_impl *probot)
void * reader(void *arg)
void _ExtractFullTransformFromChildren(DblArray12 &tres, const T pelt)
Travel by the transformation array and calls the getTransform method.
void COLLADALOG_ERROR(const std::string &s)
Definition: ColladaUtil.h:93
domInstance_rigid_constraintRef irigidconstraint
void setColdetModelTriangles(ColdetModelPtr &coldetModel, const TransformedShapeIndex &tsi, const Matrix44 &Tparent, int &vertexIndex, int &triangleIndex)
png_infop png_charpp name
Definition: png.h:2382
png_voidp int value
Definition: png.h:2113
domTechniqueRef _ExtractOpenRAVEProfile(const domTechnique_Array &arr)
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
std::string _ExtractParentId(daeElementRef p)
#define EPS
w
static bool _IsValidCharInName(char c)
png_uint_32 i
Definition: png.h:2735
void _FillMaterial(MaterialInfo &mat, const domMaterialRef pdommat)
Eigen::Matrix4d Matrix44
Definition: Eigen4d.h:18
virtual ExtraJointInfoSequence * extraJoints()
long b
Definition: jpegint.h:371
png_bytepp image
Definition: png.h:1772
reads in collada files and initializes a BodyInfo struct
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
#define FOREACH(it, v)
#define COLLADA_ASSERT(b)
Definition: ColladaUtil.h:74
static daeElement * searchBindingArray(daeString ref, const domInstance_kinematics_model_Array &paramArray)
std::vector< std::string > _veclinknames
std::map< std::string, boost::shared_ptr< LinkInfo > > _mapLinkNames
OpenHRP::vector3 Vector3
lib
std::map< std::string, time_t > fileTimeMap
static xsBoolean resolveBool(domCommon_bool_or_paramRef paddr, const U &parent)
bool _ExtractGeometry(BodyInfoCollada_impl *pkinbody, boost::shared_ptr< LinkInfo > plink, const domTrifansRef triRef, const domVerticesRef vertsRef, const map< string, int > &mapmaterials, const map< string, int > &maptextures)
bool InitFromData(const string &pdata)
daeElementRef _getElementFromUrl(const daeURI &uri)
bool ExtractKinematicsModel(BodyInfoCollada_impl *pkinbody, domKinematics_modelRef kmodel, domNodeRef pnode, domPhysics_modelRef pmodel, const KinematicsSceneBindings bindings)
append the kinematics model to the kinbody
std::vector< boost::shared_ptr< LinkInfo > > _veclinks
double dReal
Definition: ColladaUtil.h:78
def j(str, encoding="cp932")
bool _ExtractGeometry(BodyInfoCollada_impl *pkinbody, boost::shared_ptr< LinkInfo > plink, const domTristripsRef triRef, const domVerticesRef vertsRef, const map< string, int > &mapmaterials, const map< string, int > &maptextures)
CORBA::Long find(const CorbaSequence &seq, Functor f)
int ExtractLink(BodyInfoCollada_impl *pkinbody, const domLinkRef pdomlink, const domNodeRef pdomnode, const DblArray12 &tParentWorldLink, const DblArray12 &tParentLink, const std::vector< domJointRef > &vdomjoints, const KinematicsSceneBindings bindings)
Extract Link info and add it to an existing body.
#define printArray
void loadModelFile(const std::string &filename)
std::list< ConstraintBinding > listConstraintBindings
static daeElement * searchBinding(domCommon_sidref_or_paramRef paddr, daeElementRef parent)
string a
std::map< std::string, boost::shared_ptr< LinkInfo > > _mapJointIds
void AxisAngleTranslationFromPose(OpenHRP::DblArray4 &rotation, OpenHRP::DblArray3 &translation, const OpenHRP::DblArray12 &pose)
Definition: ColladaUtil.h:275
bool _ExtractActuator(boost::shared_ptr< LinkInfo > plink, daeElementRef instance_actuator)
Extract an instance of a sensor.
png_bytepp row
Definition: png.h:1759
void setParam(std::string param, bool value)
void PoseIdentity(OpenHRP::DblArray12 &pose)
Definition: ColladaUtil.h:117
bool ExtractKinematicsModel(BodyInfoCollada_impl *pkinbody, domNodeRef pdomnode, const KinematicsSceneBindings &bindings, const std::vector< std::string > &vprocessednodes)
extract one rigid link composed of the node hierarchy
Eigen::Vector4d Vector4
Definition: Eigen4d.h:21
static daeElement * searchBinding(daeString ref, daeElementRef parent)
virtual LinkInfoSequence * links()
void COLLADALOG_WARN(const std::string &s)
Definition: ColladaUtil.h:90
void setColdetModel(ColdetModelPtr &coldetModel, TransformedShapeIndexSequence shapeIndices, const Matrix44 &Tparent, int &vertexIndex, int &triangleIndex)
BodyInfoCollada_impl(PortableServer::POA_ptr poa)
std::list< std::pair< domNodeRef, domInstance_kinematics_modelRef > > listKinematicsVisualBindings
domMotion_axis_infoRef motion_axis_info
#define M_PI
void COLLADALOG_INFO(const std::string &s)
Definition: ColladaUtil.h:87
HRP_UTIL_EXPORT string deleteURLScheme(string url)
Definition: UrlUtil.cpp:25
domKinematics_axis_infoRef kinematics_axis_info
boost::shared_ptr< LinkInfo > GetLink(const std::string &name)
typedef int
Definition: png.h:1113
bool Extract(BodyInfoCollada_impl *probot)
the first possible robot in the scene
std::string _ExtractLinkName(domLinkRef pdomlink)
std::string _ConvertToValidName(const std::string &name)
void QuatFromMatrix(OpenHRP::DblArray4 &quat, const T &mat)
Definition: ColladaUtil.h:151
void ExtractRobotAttachedActuators(BodyInfoCollada_impl *probot, const domArticulated_systemRef as)
Extract Sensors attached to a Robot.
domInstance_rigid_bodyRef irigidbody
virtual void handleWarning(daeString msg)
static bool resolveCommon_float_or_param(daeElementRef pcommon, daeElementRef parent, float &f)
bool _ExtractGeometry(BodyInfoCollada_impl *pkinbody, boost::shared_ptr< LinkInfo > plink, const domPolylistRef triRef, const domVerticesRef vertsRef, const map< string, int > &mapmaterials, const map< string, int > &maptextures)
bool InitFromURL(const string &url)
std::pair< boost::shared_ptr< LinkInfo >,domJointRef > _getJointFromRef(xsToken targetref, daeElementRef peltref, BodyInfoCollada_impl *pkinbody)
std::map< boost::shared_ptr< LinkInfo >, std::vector< dReal > > _mapJointUnits
ShapeInfoSequence shapes_
HRP_UTIL_EXPORT void calcRodrigues(Matrix33 &out_R, const Vector3 &axis, double q)
Definition: Eigen3d.cpp:22
void changetoBoundingBox(unsigned int *depth)
boost::shared_ptr< std::string > _ExtractInterfaceType(const domExtra_Array &arr)
returns an openrave interface type from the extra array
bool AddAxisInfo(const domInstance_kinematics_model_Array &arr, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info)
void PoseFromQuat(OpenHRP::DblArray12 &pose, const OpenHRP::DblArray4 &quat)
Definition: ColladaUtil.h:246
void QuatFromAxisAngle(OpenHRP::DblArray4 &quat, const T &rotation, dReal fanglemult=1)
Definition: ColladaUtil.h:227
static void _ExtractPhysicsBindings(domCOLLADA::domSceneRef allscene, KinematicsSceneBindings &bindings)
int num
Definition: png.h:1502
OpenHRP::ModelLoader::AABBdataType AABBdataType_
daeElementRef _ExtractOpenRAVEProfile(const daeElementRef pelt)
void operator<<(std::ostream &os, const OpenHRP::DblArray12 &ttemp)
The colladadom reader that fills in the BodyInfoCollada_impl class.
boost::shared_ptr< DAE > _dae
virtual StringSequence * info()
boost::intrusive_ptr< ColdetModel > ColdetModelPtr
Definition: ColdetModel.h:268
* y
Definition: IceUtils.h:97
bool _ExtractGeometry(BodyInfoCollada_impl *pkinbody, boost::shared_ptr< LinkInfo > plink, const domTrianglesRef triRef, const domVerticesRef vertsRef, const map< string, int > &mapmaterials, const map< string, int > &maptextures)
void getTransform(DblArray12 &tres, daeElementRef pelt)
Gets all transformations applied to the node.
static bool _IsValidName(const std::string &s)
void COLLADALOG_VERBOSE(const std::string &s)
Definition: ColladaUtil.h:81
std::string _ExtractMathML(daeElementRef proot, BodyInfoCollada_impl *pkinbody, daeElementRef pelt)
Extracts MathML into fparser equation format.
virtual void handleError(daeString msg)
virtual const std::string & topUrl()
static domFloat resolveFloat(domCommon_float_or_paramRef paddr, const U &parent)
bool _ExtractSensor(SensorInfo &psensor, daeElementRef instance_sensor)
Extract an instance of a sensor.
bool ExtractArticulatedSystem(BodyInfoCollada_impl *&probot, domInstance_articulated_systemRef ias, KinematicsSceneBindings &bindings)
extracts an articulated system. Note that an articulated system can include other articulated systems...
static daeElement * searchBindingArray(daeString ref, const domInstance_articulated_system_Array &paramArray)
JointAxisBinding(daeElementRef pvisualtrans, domAxis_constraintRef pkinematicaxis, domCommon_float_or_paramRef jointvalue, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info)
void ExtractRobotManipulators(BodyInfoCollada_impl *probot, const domArticulated_systemRef as)
extract the robot manipulators
static void _ExtractKinematicsVisualBindings(domInstance_with_extraRef viscene, domInstance_kinematics_sceneRef kiscene, KinematicsSceneBindings &bindings)
go through all kinematics binds to get a kinematics/visual pair


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:19