00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00022 #ifndef OPENHRP_COLLADA_WRITER_H
00023 #define OPENHRP_COLLADA_WRITER_H
00024
00025 #include "ColladaUtil.h"
00026 #include <hrpUtil/MatrixSolvers.h>
00027
00028 using namespace std;
00029 using namespace ColladaUtil;
00030
00031 #include <hrpCorba/ViewSimulator.hh>
00032
00033
00034
00035
00036
00037
00038
00039
00040 struct ManipulatorInfo
00041 {
00042 ManipulatorInfo() {
00043 rotation[0] = rotation[1] = rotation[2] = rotation[3] = 0;
00044 translation[0] = translation[1] = translation[2] = 0;
00045 }
00046 std::string name;
00047 std::string basename, effectorname;
00048 std::list<std::string> grippernames;
00049 std::list<std::string> gripperdir;
00050
00051 DblArray4 rotation;
00052 DblArray3 translation;
00053 };
00054
00055 class ColladaWriter : public daeErrorHandler
00056 {
00057 static bool ComparePair(const std::pair<int,int>& p0,const std::pair<int,int>& p1)
00058 {
00059 return p0.second < p1.second;
00060 }
00061
00062 public:
00063 struct SCENE
00064 {
00065 domVisual_sceneRef vscene;
00066 domKinematics_sceneRef kscene;
00067 domPhysics_sceneRef pscene;
00068 domInstance_with_extraRef viscene;
00069 domInstance_kinematics_sceneRef kiscene;
00070 domInstance_with_extraRef piscene;
00071 };
00072
00073 struct LINKOUTPUT
00074 {
00075 list<pair<int,std::string> > listusedlinks;
00076 daeElementRef plink;
00077 domNodeRef pnode;
00078 };
00079
00080 struct physics_model_output
00081 {
00082 domPhysics_modelRef pmodel;
00083 std::vector<std::string > vrigidbodysids;
00084 };
00085
00086 struct kinematics_model_output
00087 {
00088 struct axis_output
00089 {
00090
00091 axis_output() : iaxis(0) {
00092 }
00093 string sid;
00094 string nodesid;
00095 int ijoint;
00096 int iaxis;
00097 string jointnodesid;
00098 };
00099 domKinematics_modelRef kmodel;
00100 std::vector<axis_output> vaxissids;
00101 std::vector<std::string > vlinksids;
00102 std::map<std::string, int> maplinknames, mapjointnames;
00103 };
00104
00105 struct axis_sids
00106 {
00107 axis_sids(const string& axissid, const string& valuesid, const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {
00108 }
00109 string axissid, valuesid, jointnodesid;
00110 };
00111
00112 struct instance_kinematics_model_output
00113 {
00114 domInstance_kinematics_modelRef ikm;
00115 std::vector<axis_sids> vaxissids;
00116 boost::shared_ptr<kinematics_model_output> kmout;
00117 std::vector<std::pair<std::string,std::string> > vkinematicsbindings;
00118 };
00119
00120 struct instance_articulated_system_output
00121 {
00122 domInstance_articulated_systemRef ias;
00123 std::vector<axis_sids> vaxissids;
00124
00125
00126 std::vector<std::pair<std::string,std::string> > vkinematicsbindings;
00127 };
00128
00129 struct instance_physics_model_output
00130 {
00131 domInstance_physics_modelRef ipm;
00132 boost::shared_ptr<physics_model_output> pmout;
00133 };
00134
00135 struct kinbody_models
00136 {
00137 std::string xmlfilename, kinematicsgeometryhash;
00138 boost::shared_ptr<kinematics_model_output> kmout;
00139 boost::shared_ptr<physics_model_output> pmout;
00140 };
00141
00142 ColladaWriter(const std::list<ManipulatorInfo>& listmanipulators, const char* comment_str) : _dom(NULL) {
00143 _listmanipulators = listmanipulators;
00144 daeErrorHandler::setErrorHandler(this);
00145 COLLADALOG_INFO(str(boost::format("init COLLADA writer version: %s, namespace: %s")%COLLADA_VERSION%COLLADA_NAMESPACE));
00146 _collada.reset(new DAE);
00147 _collada->setIOPlugin( NULL );
00148 _collada->setDatabase( NULL );
00149
00150 const char* documentName = "openrave_snapshot";
00151 daeInt error = _collada->getDatabase()->insertDocument(documentName, &_doc );
00152 BOOST_ASSERT( error == DAE_OK && !!_doc );
00153 _dom = daeSafeCast<domCOLLADA>(_doc->getDomRoot());
00154
00155
00156 domAssetRef asset = daeSafeCast<domAsset>( _dom->add( COLLADA_ELEMENT_ASSET ) );
00157 {
00158
00159 boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%dT%H:%M:%s");
00160 std::stringstream ss;
00161 ss.imbue(std::locale(ss.getloc(), facet));
00162 ss << boost::posix_time::second_clock::local_time();
00163
00164 domAsset::domCreatedRef created = daeSafeCast<domAsset::domCreated>( asset->add( COLLADA_ELEMENT_CREATED ) );
00165 created->setValue(ss.str().c_str());
00166 domAsset::domModifiedRef modified = daeSafeCast<domAsset::domModified>( asset->add( COLLADA_ELEMENT_MODIFIED ) );
00167 modified->setValue(ss.str().c_str());
00168
00169 domAsset::domContributorRef contrib = daeSafeCast<domAsset::domContributor>( asset->add( COLLADA_TYPE_CONTRIBUTOR ) );
00170 domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast<domAsset::domContributor::domAuthoring_tool>( contrib->add( COLLADA_ELEMENT_AUTHORING_TOOL ) );
00171 authoringtool->setValue("OpenHRP3 Collada Writer");
00172 domAsset::domContributor::domCommentsRef comments = daeSafeCast<domAsset::domContributor::domComments>( contrib->add( COLLADA_ELEMENT_COMMENTS ) );
00173 comments->setValue(comment_str);
00174
00175
00176 domAsset::domUnitRef units = daeSafeCast<domAsset::domUnit>( asset->add( COLLADA_ELEMENT_UNIT ) );
00177 units->setMeter(1);
00178 units->setName("meter");
00179
00180 domAsset::domUp_axisRef zup = daeSafeCast<domAsset::domUp_axis>( asset->add( COLLADA_ELEMENT_UP_AXIS ) );
00181 zup->setValue(UP_AXIS_Z_UP);
00182 }
00183
00184 _globalscene = _dom->getScene();
00185 if( !_globalscene ) {
00186 _globalscene = daeSafeCast<domCOLLADA::domScene>( _dom->add( COLLADA_ELEMENT_SCENE ) );
00187 }
00188
00189 _visualScenesLib = daeSafeCast<domLibrary_visual_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
00190 _visualScenesLib->setId("vscenes");
00191 _geometriesLib = daeSafeCast<domLibrary_geometries>(_dom->add(COLLADA_ELEMENT_LIBRARY_GEOMETRIES));
00192 _geometriesLib->setId("geometries");
00193 _effectsLib = daeSafeCast<domLibrary_effects>(_dom->add(COLLADA_ELEMENT_LIBRARY_EFFECTS));
00194 _effectsLib->setId("effects");
00195 _imagesLib = daeSafeCast<domLibrary_images>(_dom->add(COLLADA_ELEMENT_LIBRARY_IMAGES));
00196 _materialsLib = daeSafeCast<domLibrary_materials>(_dom->add(COLLADA_ELEMENT_LIBRARY_MATERIALS));
00197 _materialsLib->setId("materials");
00198 _kinematicsModelsLib = daeSafeCast<domLibrary_kinematics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
00199 _kinematicsModelsLib->setId("kmodels");
00200 _articulatedSystemsLib = daeSafeCast<domLibrary_articulated_systems>(_dom->add(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS));
00201 _articulatedSystemsLib->setId("asystems");
00202 _kinematicsScenesLib = daeSafeCast<domLibrary_kinematics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
00203 _kinematicsScenesLib->setId("kscenes");
00204 _physicsScenesLib = daeSafeCast<domLibrary_physics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
00205 _physicsScenesLib->setId("pscenes");
00206 _physicsModelsLib = daeSafeCast<domLibrary_physics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_MODELS));
00207 _physicsModelsLib->setId("pmodels");
00208 domExtraRef pextra_library_sensors = daeSafeCast<domExtra>(_dom->add(COLLADA_ELEMENT_EXTRA));
00209 pextra_library_sensors->setId("sensors");
00210 pextra_library_sensors->setType("library_sensors");
00211 _sensorsLib = daeSafeCast<domTechnique>(pextra_library_sensors->add(COLLADA_ELEMENT_TECHNIQUE));
00212 _sensorsLib->setProfile("OpenRAVE");
00213 _nextsensorid = 0;
00214 domExtraRef pextra_library_actuators = daeSafeCast<domExtra>(_dom->add(COLLADA_ELEMENT_EXTRA));
00215 pextra_library_actuators->setId("actuators");
00216 pextra_library_actuators->setType("library_actuators");
00217 _actuatorsLib = daeSafeCast<domTechnique>(pextra_library_actuators->add(COLLADA_ELEMENT_TECHNIQUE));
00218 _actuatorsLib->setProfile("OpenRAVE");
00219 _nextactuatorid = 0;
00220 }
00221 virtual ~ColladaWriter() {
00222 _collada.reset();
00223 DAE::cleanup();
00224 }
00225
00226
00228 virtual void Save(const string& filename)
00229 {
00230 bool bcompress = filename.size() >= 4 && filename[filename.size()-4] == '.' && ::tolower(filename[filename.size()-3]) == 'z' && ::tolower(filename[filename.size()-2]) == 'a' && ::tolower(filename[filename.size()-1]) == 'e';
00231 if( !bcompress ) {
00232 if(!_collada->writeTo(_doc->getDocumentURI()->getURI(), filename.c_str()) ) {
00233 throw ModelLoader::ModelLoaderException(str(boost::format("failed to save collada file to %s")%filename).c_str());
00234 }
00235 return;
00236 }
00237 COLLADALOG_WARN("cannot save as compressed collada file\n");
00238 }
00239
00240 virtual bool Write(BodyInfo_impl* bodyInfo) {
00241 _CreateScene();
00242 boost::shared_ptr<instance_articulated_system_output> iasout = _WriteRobot(bodyInfo);
00243 if( !iasout ) {
00244 return false;
00245 }
00246 _WriteBindingsInstance_kinematics_scene(_scene.kiscene,bodyInfo,iasout->vaxissids,iasout->vkinematicsbindings);
00247 }
00248
00250 virtual boost::shared_ptr<instance_articulated_system_output> _WriteRobot(BodyInfo_impl* bodyInfo)
00251 {
00252 COLLADALOG_VERBOSE(str(boost::format("writing robot as instance_articulated_system (%d) %s\n")%_GetRobotId(bodyInfo)%bodyInfo->name()));
00253 string asid = str(boost::format("robot%d")%_GetRobotId(bodyInfo));
00254 string askid = str(boost::format("%s_kinematics")%asid);
00255 string asmid = str(boost::format("%s_motion")%asid);
00256 string iassid = str(boost::format("%s_inst")%asmid);
00257
00258 domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(_scene.kscene->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
00259 ias->setSid(iassid.c_str());
00260 ias->setUrl((string("#")+asmid).c_str());
00261 ias->setName(bodyInfo->name());
00262
00263 boost::shared_ptr<instance_articulated_system_output> iasout(new instance_articulated_system_output());
00264 iasout->ias = ias;
00265
00266
00267 domArticulated_systemRef articulated_system_motion = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
00268 articulated_system_motion->setId(asmid.c_str());
00269 domMotionRef motion = daeSafeCast<domMotion>(articulated_system_motion->add(COLLADA_ELEMENT_MOTION));
00270 domMotion_techniqueRef mt = daeSafeCast<domMotion_technique>(motion->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00271 domInstance_articulated_systemRef ias_motion = daeSafeCast<domInstance_articulated_system>(motion->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
00272 ias_motion->setUrl(str(boost::format("#%s")%askid).c_str());
00273
00274
00275 domArticulated_systemRef articulated_system_kinematics = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
00276 articulated_system_kinematics->setId(askid.c_str());
00277 domKinematicsRef kinematics = daeSafeCast<domKinematics>(articulated_system_kinematics->add(COLLADA_ELEMENT_KINEMATICS));
00278 domKinematics_techniqueRef kt = daeSafeCast<domKinematics_technique>(kinematics->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00279
00280 boost::shared_ptr<instance_kinematics_model_output> ikmout = _WriteInstance_kinematics_model(bodyInfo,kinematics,askid);
00281 LinkInfoSequence_var links = bodyInfo->links();
00282 for(size_t idof = 0; idof < ikmout->vaxissids.size(); ++idof) {
00283 string axis_infosid = str(boost::format("axis_info_inst%d")%idof);
00284 LinkInfo& pjoint = links[ikmout->kmout->vaxissids.at(idof).ijoint];
00285 string jointType(CORBA::String_var(pjoint.jointType));
00286
00287
00288 domKinematics_axis_infoRef kai = daeSafeCast<domKinematics_axis_info>(kt->add(COLLADA_ELEMENT_AXIS_INFO));
00289 kai->setAxis(str(boost::format("%s/%s")%ikmout->kmout->kmodel->getID()%ikmout->kmout->vaxissids.at(idof).sid).c_str());
00290 kai->setSid(axis_infosid.c_str());
00291 domCommon_bool_or_paramRef active = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_ACTIVE));
00292 daeSafeCast<domCommon_bool_or_param::domBool>(active->add(COLLADA_ELEMENT_BOOL))->setValue(jointType != "fixed");
00293 domCommon_bool_or_paramRef locked = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_LOCKED));
00294 daeSafeCast<domCommon_bool_or_param::domBool>(locked->add(COLLADA_ELEMENT_BOOL))->setValue(false);
00295
00296 dReal fmult = jointType == "rotate" ? (180.0/M_PI) : 1.0;
00297 vector<dReal> vmin, vmax;
00298 if( jointType == "fixed" ) {
00299 vmin.push_back(0);
00300 vmax.push_back(0);
00301 }
00302 else {
00303 if( pjoint.llimit.length() > 0 ) {
00304 vmin.push_back(fmult*pjoint.llimit[0]);
00305 }
00306 if( pjoint.ulimit.length() > 0 ) {
00307 vmax.push_back(fmult*pjoint.ulimit[0]);
00308 }
00309 }
00310
00311 if( vmin.size() > 0 || vmax.size() > 0 ) {
00312 domKinematics_limitsRef plimits = daeSafeCast<domKinematics_limits>(kai->add(COLLADA_ELEMENT_LIMITS));
00313 if( vmin.size() > 0 ) {
00314 daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MIN)->add(COLLADA_ELEMENT_FLOAT))->setValue(vmin[0]);
00315 }
00316 if( vmax.size() > 0 ) {
00317 daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MAX)->add(COLLADA_ELEMENT_FLOAT))->setValue(vmax[0]);
00318 }
00319 }
00320
00321
00322 domMotion_axis_infoRef mai = daeSafeCast<domMotion_axis_info>(mt->add(COLLADA_ELEMENT_AXIS_INFO));
00323 mai->setAxis(str(boost::format("%s/%s")%askid%axis_infosid).c_str());
00324 if( pjoint.uvlimit.length() > 0 ) {
00325 domCommon_float_or_paramRef speed = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_SPEED));
00326 daeSafeCast<domCommon_float_or_param::domFloat>(speed->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint.uvlimit[0]);
00327 }
00328 if( pjoint.climit.length() > 0 ) {
00329 domCommon_float_or_paramRef accel = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_ACCELERATION));
00330 daeSafeCast<domCommon_float_or_param::domFloat>(accel->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint.climit[0] * pjoint.torqueConst * pjoint.gearRatio);
00331 }
00332 }
00333
00334
00335 string asmsym = str(boost::format("%s_%s")%asmid%ikmout->ikm->getSid());
00336 string assym = str(boost::format("%s_%s")%_scene.kscene->getID()%ikmout->ikm->getSid());
00337 for(std::vector<std::pair<std::string,std::string> >::iterator it = ikmout->vkinematicsbindings.begin(); it != ikmout->vkinematicsbindings.end(); ++it) {
00338 domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
00339 abm->setSid(asmsym.c_str());
00340 daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%it->first).c_str());
00341 domKinematics_newparamRef ab = daeSafeCast<domKinematics_newparam>(ias->add(COLLADA_ELEMENT_NEWPARAM));
00342 ab->setSid(assym.c_str());
00343 daeSafeCast<domKinematics_newparam::domSIDREF>(ab->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%asmid%asmsym).c_str());
00344 iasout->vkinematicsbindings.push_back(make_pair(string(ab->getSid()), it->second));
00345 }
00346 for(size_t idof = 0; idof < ikmout->vaxissids.size(); ++idof) {
00347 const axis_sids& kas = ikmout->vaxissids.at(idof);
00348 domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
00349 abm->setSid(str(boost::format("%s_%s")%asmid%kas.axissid).c_str());
00350 daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.axissid).c_str());
00351 domKinematics_newparamRef ab = daeSafeCast<domKinematics_newparam>(ias->add(COLLADA_ELEMENT_NEWPARAM));
00352 ab->setSid(str(boost::format("%s_%s")%assym%kas.axissid).c_str());
00353 daeSafeCast<domKinematics_newparam::domSIDREF>(ab->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s_%s")%asmid%asmid%kas.axissid).c_str());
00354 string valuesid;
00355 if( kas.valuesid.size() > 0 ) {
00356 domKinematics_newparamRef abmvalue = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
00357 abmvalue->setSid(str(boost::format("%s_%s")%asmid%kas.valuesid).c_str());
00358 daeSafeCast<domKinematics_newparam::domSIDREF>(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.valuesid).c_str());
00359 domKinematics_newparamRef abvalue = daeSafeCast<domKinematics_newparam>(ias->add(COLLADA_ELEMENT_NEWPARAM));
00360 valuesid = str(boost::format("%s_%s")%assym%kas.valuesid);
00361 abvalue->setSid(valuesid.c_str());
00362 daeSafeCast<domKinematics_newparam::domSIDREF>(abvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s_%s")%asmid%asmid%kas.valuesid).c_str());
00363 }
00364 iasout->vaxissids.push_back(axis_sids(string(ab->getSid()),valuesid,kas.jointnodesid));
00365 }
00366
00367 boost::shared_ptr<kinematics_model_output> kmout = _GetKinematics_model(bodyInfo);
00368 string kmodelid = kmout->kmodel->getID(); kmodelid += "/";
00369
00370
00371 for(std::list<ManipulatorInfo>::const_iterator itmanip = _listmanipulators.begin(); itmanip != _listmanipulators.end(); ++itmanip) {
00372 if (kmout->maplinknames.find(itmanip->basename) == kmout->maplinknames.end()){
00373 std::cerr << "can't find a link named " << itmanip->basename << ", manipulator " << itmanip->name << " is not defined" << std::endl;
00374 continue;
00375 }
00376 if (kmout->maplinknames.find(itmanip->effectorname) == kmout->maplinknames.end()){
00377 std::cerr << "can't find a link named " << itmanip->effectorname << ", manipulator " << itmanip->name << " is not defined" << std::endl;
00378 continue;
00379 }
00380 domExtraRef pextra = daeSafeCast<domExtra>(articulated_system_motion->add(COLLADA_ELEMENT_EXTRA));
00381 pextra->setName(itmanip->name.c_str());
00382 pextra->setType("manipulator");
00383 domTechniqueRef ptec = daeSafeCast<domTechnique>(pextra->add(COLLADA_ELEMENT_TECHNIQUE));
00384 ptec->setProfile("OpenRAVE");
00385 daeElementRef frame_origin = ptec->add("frame_origin");
00386 frame_origin->setAttribute("link",(kmodelid+_GetLinkSid(kmout->maplinknames[itmanip->basename])).c_str());
00387 daeElementRef frame_tip = ptec->add("frame_tip");
00388 frame_tip->setAttribute("link",(kmodelid+_GetLinkSid(kmout->maplinknames[itmanip->effectorname])).c_str());
00389 _WriteTransformation(frame_tip,itmanip->rotation, itmanip->translation);
00390 BOOST_ASSERT(itmanip->gripperdir.size() == itmanip->grippernames.size());
00391 std::list<std::string>::const_iterator itgripperdir = itmanip->gripperdir.begin();
00392 std::list<std::string>::const_iterator itgrippername = itmanip->grippernames.begin();
00393 while(itgrippername != itmanip->grippernames.end() ) {
00394 daeElementRef gripper_joint = ptec->add("gripper_joint");
00395 gripper_joint->setAttribute("joint", str(boost::format("%sjointsid%d")%kmodelid%kmout->mapjointnames[*itgrippername]).c_str());
00396 daeElementRef closing_direction = gripper_joint->add("closing_direction");
00397 closing_direction->setAttribute("axis","./axis0");
00398 closing_direction->add("float")->setCharData(*itgripperdir);
00399 ++itgrippername;
00400 ++itgripperdir;
00401 }
00402 }
00403
00404 boost::shared_ptr<instance_physics_model_output> ipmout = _WriteInstance_physics_model(bodyInfo,_scene.pscene,_scene.pscene->getID());
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416 for(size_t ilink = 0; ilink < links->length(); ++ilink) {
00417 LinkInfo& linkInfo = links[ilink];
00418 for(size_t isensor = 0; isensor < linkInfo.sensors.length(); ++isensor) {
00419 SensorInfo& sensor = linkInfo.sensors[isensor];
00420 daeElementRef domsensor = WriteSensor(sensor, bodyInfo->name());
00421
00422 domExtraRef extra_attach_sensor = daeSafeCast<domExtra>(articulated_system_motion->add(COLLADA_ELEMENT_EXTRA));
00423 extra_attach_sensor->setName(sensor.name);
00424 extra_attach_sensor->setType("attach_sensor");
00425 domTechniqueRef attach_sensor = daeSafeCast<domTechnique>(extra_attach_sensor->add(COLLADA_ELEMENT_TECHNIQUE));
00426 attach_sensor->setProfile("OpenRAVE");
00427
00428 string strurl = str(boost::format("#%s")%domsensor->getID());
00429 daeElementRef isensor0 = attach_sensor->add("instance_sensor");
00430 isensor0->setAttribute("url",strurl.c_str());
00431
00432 daeElementRef frame_origin = attach_sensor->add("frame_origin");
00433 frame_origin->setAttribute("link",(kmodelid+_GetLinkSid(kmout->maplinknames[string(linkInfo.segments[0].name)])).c_str());
00434 if( string(domsensor->getAttribute("type")).find("camera") != string::npos ) {
00435
00436 DblArray4 rotation; rotation[0] = 1; rotation[1] = 0; rotation[2] = 0; rotation[3] = M_PI;
00437 _SetRotate(daeSafeCast<domRotate>(frame_origin->add(COLLADA_ELEMENT_ROTATE,0)), rotation);
00438 }
00439 _WriteTransformation(frame_origin,sensor.rotation, sensor.translation);
00440 }
00441 }
00442
00443
00444 std::vector< pair<int, int> > vjointids(links->length());
00445 for(size_t ilink = 0; ilink < links->length(); ++ilink) {
00446 vjointids[ilink].first = ilink;
00447 vjointids[ilink].second = links[ilink].jointId;
00448 }
00449 sort(vjointids.begin(),vjointids.end(),ComparePair);
00450 for(size_t ipair = 0; ipair < vjointids.size(); ++ipair) {
00451 size_t ilink = vjointids[ipair].first;
00452 LinkInfo& linkInfo = links[ilink];
00453 daeElementRef domactuator = WriteActuator(linkInfo, bodyInfo->name());
00454
00455 domExtraRef extra_attach_actuator = daeSafeCast<domExtra>(articulated_system_motion->add(COLLADA_ELEMENT_EXTRA));
00456 extra_attach_actuator->setName(linkInfo.name);
00457 extra_attach_actuator->setType("attach_actuator");
00458 domTechniqueRef attach_actuator = daeSafeCast<domTechnique>(extra_attach_actuator->add(COLLADA_ELEMENT_TECHNIQUE));
00459 attach_actuator->setProfile("OpenRAVE");
00460
00461 string strurl = str(boost::format("#%s")%domactuator->getID());
00462 daeElementRef iactuator = attach_actuator->add("instance_actuator");
00463 iactuator->setAttribute("url",strurl.c_str());
00464 attach_actuator->add("bind_actuator")->setAttribute("joint",str(boost::format("%sjointsid%d")%kmodelid%kmout->mapjointnames[string(linkInfo.name)]).c_str());
00465 }
00466
00467 return iasout;
00468 }
00469
00471 virtual boost::shared_ptr<instance_kinematics_model_output> _WriteInstance_kinematics_model(BodyInfo_impl* bodyInfo, daeElementRef parent, const string& sidscope)
00472 {
00473 COLLADALOG_VERBOSE(str(boost::format("writing instance_kinematics_model (%d) %s\n")%_GetRobotId(bodyInfo)%bodyInfo->name()));
00474 boost::shared_ptr<kinematics_model_output> kmout = WriteKinematics_model(bodyInfo);
00475
00476 boost::shared_ptr<instance_kinematics_model_output> ikmout(new instance_kinematics_model_output());
00477 ikmout->kmout = kmout;
00478 ikmout->ikm = daeSafeCast<domInstance_kinematics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
00479
00480 string symscope, refscope;
00481 if( sidscope.size() > 0 ) {
00482 symscope = sidscope+string("_");
00483 refscope = sidscope+string("/");
00484 }
00485 string ikmsid = str(boost::format("%s_inst")%kmout->kmodel->getID());
00486 ikmout->ikm->setUrl(str(boost::format("#%s")%kmout->kmodel->getID()).c_str());
00487 ikmout->ikm->setSid(ikmsid.c_str());
00488
00489 domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
00490 kbind->setSid((symscope+ikmsid).c_str());
00491 daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid).c_str());
00492 ikmout->vkinematicsbindings.push_back(make_pair(string(kbind->getSid()), str(boost::format("visual%d/node0")%_GetRobotId(bodyInfo))));
00493 LinkInfoSequence_var links = bodyInfo->links();
00494 ikmout->vaxissids.reserve(kmout->vaxissids.size());
00495 int i = 0;
00496 for(std::vector<kinematics_model_output::axis_output>::iterator it = kmout->vaxissids.begin(); it != kmout->vaxissids.end(); ++it) {
00497 domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
00498 string ref = it->sid;
00499 size_t index = ref.find("/");
00500 while(index != string::npos) {
00501 ref[index] = '.';
00502 index = ref.find("/",index+1);
00503 }
00504 string sid = symscope+ikmsid+"_"+ref;
00505 kbind->setSid(sid.c_str());
00506 daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid+"/"+it->sid).c_str());
00507 domKinematics_newparamRef pvalueparam = daeSafeCast<domKinematics_newparam>(ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
00508 pvalueparam->setSid((sid+string("_value")).c_str());
00509 daeSafeCast<domKinematics_newparam::domFloat>(pvalueparam->add(COLLADA_ELEMENT_FLOAT))->setValue(links[it->ijoint].jointValue);
00510 ikmout->vaxissids.push_back(axis_sids(sid,pvalueparam->getSid(),kmout->vaxissids.at(i).jointnodesid));
00511 ++i;
00512 }
00513
00514 return ikmout;
00515 }
00516
00517 virtual boost::shared_ptr<instance_physics_model_output> _WriteInstance_physics_model(BodyInfo_impl* bodyInfo, daeElementRef parent, const string& sidscope)
00518 {
00519 boost::shared_ptr<physics_model_output> pmout = WritePhysics_model(bodyInfo);
00520 boost::shared_ptr<instance_physics_model_output> ipmout(new instance_physics_model_output());
00521 ipmout->pmout = pmout;
00522 ipmout->ipm = daeSafeCast<domInstance_physics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_PHYSICS_MODEL));
00523 ipmout->ipm->setParent(xsAnyURI(*ipmout->ipm,string("#")+_GetNodeId(bodyInfo)));
00524 string symscope, refscope;
00525 if( sidscope.size() > 0 ) {
00526 symscope = sidscope+string("_");
00527 refscope = sidscope+string("/");
00528 }
00529 string ipmsid = str(boost::format("%s_inst")%pmout->pmodel->getID());
00530 ipmout->ipm->setUrl(str(boost::format("#%s")%pmout->pmodel->getID()).c_str());
00531 ipmout->ipm->setSid(ipmsid.c_str());
00532 for(size_t i = 0; i < pmout->vrigidbodysids.size(); ++i) {
00533 domInstance_rigid_bodyRef pirb = daeSafeCast<domInstance_rigid_body>(ipmout->ipm->add(COLLADA_ELEMENT_INSTANCE_RIGID_BODY));
00534 pirb->setBody(pmout->vrigidbodysids[i].c_str());
00535 pirb->setTarget(xsAnyURI(*pirb,str(boost::format("#%s")%_GetNodeId(bodyInfo,i))));
00536 }
00537
00538 if ( pmout->vrigidbodysids.size() >= 0) {
00539 LinkInfoSequence_var links = bodyInfo->links();
00540 if ( links->length() > 0 ) {
00541 LinkInfo& linkInfo = links[0];
00542 string jointType(CORBA::String_var(linkInfo.jointType));
00543 if ( jointType == "fixed" ) {
00544 domInstance_rigid_constraintRef pirc = daeSafeCast<domInstance_rigid_constraint>(ipmout->ipm->add(COLLADA_ELEMENT_INSTANCE_RIGID_CONSTRAINT));
00545 pirc->setConstraint("rigid_constraint0");
00546 }
00547 }
00548 }
00549 return ipmout;
00550 }
00551
00552 virtual boost::shared_ptr<kinematics_model_output> WriteKinematics_model(BodyInfo_impl* bodyInfo) {
00553 boost::shared_ptr<kinematics_model_output> kmout = _GetKinematics_model(bodyInfo);
00554 if( !!kmout ) {
00555 return kmout;
00556 }
00557
00558 domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(_kinematicsModelsLib->add(COLLADA_ELEMENT_KINEMATICS_MODEL));
00559 string kmodelid = str(boost::format("kmodel%d")%_GetRobotId(bodyInfo));
00560 kmodel->setId(kmodelid.c_str());
00561 kmodel->setName(bodyInfo->name());
00562
00563 domKinematics_model_techniqueRef ktec = daeSafeCast<domKinematics_model_technique>(kmodel->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00564
00565
00566 domNodeRef pnoderoot = daeSafeCast<domNode>(_scene.vscene->add(COLLADA_ELEMENT_NODE));
00567 string bodyid = _GetNodeId(bodyInfo);
00568 pnoderoot->setId(bodyid.c_str());
00569 pnoderoot->setSid(bodyid.c_str());
00570 pnoderoot->setName(bodyInfo->name());
00571
00572 LinkInfoSequence_var links = bodyInfo->links();
00573 vector<domJointRef> vdomjoints(links->length());
00574 kmout.reset(new kinematics_model_output());
00575 kmout->kmodel = kmodel;
00576 kmout->vaxissids.resize(0);
00577 kmout->vlinksids.resize(links->length());
00578
00579
00580 kmout->mapjointnames[std::string(links[0].name)] = 1000;
00581 for(size_t ilink = 0; ilink < vdomjoints.size(); ++ilink) {
00582 LinkInfo& linkInfo = links[ilink];
00583 kmout->maplinknames[std::string(linkInfo.segments[0].name)] = ilink;
00584 string jointType(CORBA::String_var(linkInfo.jointType));
00585 daeString colladaelement;
00586 int dof = 1;
00587 dReal fmult = 1;
00588 vector<dReal> lmin, lmax;
00589 if( jointType == "fixed" ) {
00590 colladaelement = COLLADA_ELEMENT_REVOLUTE;
00591 lmin.push_back(0);
00592 lmax.push_back(0);
00593 }
00594 else {
00595 if( jointType == "rotate" ) {
00596 colladaelement = COLLADA_ELEMENT_REVOLUTE;
00597 fmult = 180.0f/M_PI;
00598 }
00599 else if( jointType == "slide" ) {
00600 colladaelement = COLLADA_ELEMENT_PRISMATIC;
00601 }
00602 else {
00603 COLLADALOG_INFO(str(boost::format("joint type %s not supported")%jointType));
00604 continue;
00605 }
00606 if( linkInfo.llimit.length() > 0 ) {
00607 lmin.push_back(fmult*linkInfo.llimit[0]);
00608 }
00609 if( linkInfo.ulimit.length() > 0 ) {
00610 lmax.push_back(fmult*linkInfo.ulimit[0]);
00611 }
00612 }
00613
00614 if ( linkInfo.jointId >= 0 ) {
00615 kmout->mapjointnames[std::string(linkInfo.name)] = linkInfo.jointId;
00616 } else {
00617 kmout->mapjointnames[std::string(linkInfo.name)] = 1000+ilink;
00618 }
00619
00620 domJointRef pdomjoint = daeSafeCast<domJoint>(ktec->add(COLLADA_ELEMENT_JOINT));
00621 string jointsid = str(boost::format("jointsid%d")%kmout->mapjointnames[std::string(linkInfo.name)]);
00622 pdomjoint->setSid( jointsid.c_str() );
00623
00624 pdomjoint->setName(linkInfo.name);
00625 vector<domAxis_constraintRef> vaxes(dof);
00626 for(int ia = 0; ia < dof; ++ia) {
00627 vaxes[ia] = daeSafeCast<domAxis_constraint>(pdomjoint->add(colladaelement));
00628 string axisid = str(boost::format("axis%d")%ia);
00629 vaxes[ia]->setSid(axisid.c_str());
00630 kinematics_model_output::axis_output axissid;
00631 axissid.ijoint = ilink;
00632 axissid.sid = jointsid+string("/")+axisid;
00633 axissid.iaxis = ia;
00634 axissid.jointnodesid = str(boost::format("%s/%s")%bodyid%_GetJointNodeSid(ilink,ia));
00635 kmout->vaxissids.push_back(axissid);
00636 domAxisRef paxis = daeSafeCast<domAxis>(vaxes.at(ia)->add(COLLADA_ELEMENT_AXIS));
00637 paxis->getValue().setCount(3);
00638 paxis->getValue()[0] = linkInfo.jointAxis[0];
00639 paxis->getValue()[1] = linkInfo.jointAxis[1];
00640 paxis->getValue()[2] = linkInfo.jointAxis[2];
00641 if( lmin.size() > 0 || lmax.size() > 0 ) {
00642 domJoint_limitsRef plimits = daeSafeCast<domJoint_limits>(vaxes[ia]->add(COLLADA_TYPE_LIMITS));
00643 if( ia < (int)lmin.size() ) {
00644 daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MIN))->getValue() = lmin.at(ia);
00645 }
00646 if( ia < (int)lmax.size() ) {
00647 daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MAX))->getValue() = lmax.at(ia);
00648 }
00649 }
00650 }
00651 vdomjoints.at(ilink) = pdomjoint;
00652 }
00653
00654 list<int> listunusedlinks;
00655 for(unsigned int i = 0; i < links->length(); ++i) {
00656 listunusedlinks.push_back(i);
00657 }
00658
00659 while(listunusedlinks.size()>0) {
00660 int ilink = listunusedlinks.front();
00661 LINKOUTPUT childinfo = _WriteLink(bodyInfo, ilink, ktec, pnoderoot, kmodel->getID(), kmout->mapjointnames);
00662 _WriteTransformation(childinfo.plink, links[ilink].rotation, links[ilink].translation);
00663 _WriteTransformation(childinfo.pnode, links[ilink].rotation, links[ilink].translation);
00664 for(list<pair<int,std::string> >::iterator itused = childinfo.listusedlinks.begin(); itused != childinfo.listusedlinks.end(); ++itused) {
00665 kmout->vlinksids.at(itused->first) = itused->second;
00666 listunusedlinks.remove(itused->first);
00667 }
00668 }
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696 _AddKinematics_model(bodyInfo,kmout);
00697 return kmout;
00698 }
00699
00700 virtual boost::shared_ptr<physics_model_output> WritePhysics_model(BodyInfo_impl* bodyInfo) {
00701 boost::shared_ptr<physics_model_output> pmout = _GetPhysics_model(bodyInfo);
00702 if( !!pmout ) {
00703 return pmout;
00704 }
00705 pmout.reset(new physics_model_output());
00706 pmout->pmodel = daeSafeCast<domPhysics_model>(_physicsModelsLib->add(COLLADA_ELEMENT_PHYSICS_MODEL));
00707 string pmodelid = str(boost::format("pmodel%d")%_GetRobotId(bodyInfo));
00708 pmout->pmodel->setId(pmodelid.c_str());
00709 pmout->pmodel->setName(bodyInfo->name());
00710 LinkInfoSequence_var links = bodyInfo->links();
00711 for(unsigned int ilink = 0; ilink < links->length(); ++ilink) {
00712 LinkInfo& link = links[ilink];
00713 domRigid_bodyRef rigid_body = daeSafeCast<domRigid_body>(pmout->pmodel->add(COLLADA_ELEMENT_RIGID_BODY));
00714 string rigidsid = str(boost::format("rigid%d")%ilink);
00715 pmout->vrigidbodysids.push_back(rigidsid);
00716 rigid_body->setId(rigidsid.c_str());
00717 rigid_body->setSid(rigidsid.c_str());
00718 rigid_body->setName(link.segments[0].name);
00719 domRigid_body::domTechnique_commonRef ptec = daeSafeCast<domRigid_body::domTechnique_common>(rigid_body->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00720 domTargetable_floatRef mass = daeSafeCast<domTargetable_float>(ptec->add(COLLADA_ELEMENT_MASS));
00721 mass->setValue(link.mass);
00722 dmatrix inertia, evec;
00723 dvector eval;
00724 inertia.resize(3,3);
00725 inertia(0,0) = link.inertia[0]; inertia(0,1) = link.inertia[1]; inertia(0,2) = link.inertia[2];
00726 inertia(1,0) = link.inertia[3]; inertia(1,1) = link.inertia[4]; inertia(1,2) = link.inertia[5];
00727 inertia(2,0) = link.inertia[6]; inertia(2,1) = link.inertia[7]; inertia(2,2) = link.inertia[8];
00728 evec.resize(3,3);
00729 eval.resize(3);
00730 hrp::calcEigenVectors(inertia,evec,eval);
00731 if (det(evec) < 0.0) {
00732
00733 evec(2,0) *= -1.0; evec(2,1) *= -1.0; evec(2,2) *= -1.0;
00734 }
00735 DblArray12 tinertiaframe;
00736 for(int j = 0; j < 3; ++j) {
00737
00738 tinertiaframe[4*0+j] = evec(j, 0);
00739 tinertiaframe[4*1+j] = evec(j, 1);
00740 tinertiaframe[4*2+j] = evec(j, 2);
00741 }
00742 DblArray4 quat, rotation;
00743 DblArray3 translation;
00744 QuatFromMatrix(quat,tinertiaframe);
00745 AxisAngleFromQuat(rotation,quat);
00746 domTargetable_float3Ref pdominertia = daeSafeCast<domTargetable_float3>(ptec->add(COLLADA_ELEMENT_INERTIA));
00747 pdominertia->getValue().setCount(3);
00748 pdominertia->getValue()[0] = eval[0]; pdominertia->getValue()[1] = eval[1]; pdominertia->getValue()[2] = eval[2];
00749 daeElementRef mass_frame = ptec->add(COLLADA_ELEMENT_MASS_FRAME);
00750 _WriteTransformation(mass_frame, rotation, link.centerOfMass);
00751
00752 int icurlink = ilink;
00753 while(icurlink > 0) {
00754 _WriteTransformation(mass_frame, links[icurlink].rotation, links[icurlink].translation);
00755 icurlink = links[icurlink].parentIndex;
00756 }
00757
00758
00759 for(unsigned int igeom = 0; igeom < link.shapeIndices.length(); ++igeom) {
00760 const TransformedShapeIndex& tsi = link.shapeIndices[igeom];
00761 DblArray12 transformMatrix;
00762 if( tsi.inlinedShapeTransformMatrixIndex >= 0 ) {
00763 PoseMult(transformMatrix, link.inlinedShapeTransformMatrices[tsi.inlinedShapeTransformMatrixIndex],tsi.transformMatrix);
00764 }
00765 else {
00766 for(int i = 0; i < 12; ++i) {
00767 transformMatrix[i] = tsi.transformMatrix[i];
00768 }
00769 }
00770 domRigid_body::domTechnique_common::domShapeRef pdomshape = daeSafeCast<domRigid_body::domTechnique_common::domShape>(ptec->add(COLLADA_ELEMENT_SHAPE));
00771
00772 QuatFromMatrix(quat,transformMatrix);
00773 AxisAngleFromQuat(rotation,quat);
00774 translation[0] = transformMatrix[4*0+3]; translation[1] = transformMatrix[4*1+3]; translation[2] = transformMatrix[4*2+3];
00775 _WriteTransformation(pdomshape,rotation,translation);
00776 icurlink = ilink;
00777 while(icurlink >= 0) {
00778 _WriteTransformation(pdomshape, links[icurlink].rotation, links[icurlink].translation);
00779 icurlink = links[icurlink].parentIndex;
00780 }
00781 domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pdomshape->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
00782 pinstgeom->setUrl(xsAnyURI(*pinstgeom,string("#")+_GetGeometryId(bodyInfo, ilink,igeom)));
00783 }
00784 }
00785 if ( links->length() > 0 && std::string(CORBA::String_var(links[0].jointType)) == std::string("fixed") ) {
00786 domRigid_constraintRef rigid_constraint = daeSafeCast<domRigid_constraint>(pmout->pmodel->add(COLLADA_ELEMENT_RIGID_CONSTRAINT));
00787 rigid_constraint->setSid("rigid_constraint0");
00788 rigid_constraint->setName(links[0].segments[0].name);
00789 domRigid_constraint::domAttachmentRef patt = daeSafeCast<domRigid_constraint::domAttachment>(rigid_constraint->add(COLLADA_TYPE_ATTACHMENT));
00790 patt->setRigid_body("#rigid0");
00791 domRigid_constraint::domRef_attachmentRef prefatt = daeSafeCast<domRigid_constraint::domRef_attachment>(rigid_constraint->add(COLLADA_TYPE_REF_ATTACHMENT));
00792 prefatt->setRigid_body("#visual1");
00793 }
00794
00795 return pmout;
00796 }
00797
00801 virtual domGeometryRef WriteGeometry(BodyInfo_impl* bodyInfo, const ShapeInfo& shapeInfo, const DblArray12& transformMatrix, const string& parentid)
00802 {
00803 const FloatSequence& vertices = shapeInfo.vertices;
00804 const LongSequence& triangles = shapeInfo.triangles;
00805 string effid = parentid+string("_eff");
00806 string matid = parentid+string("_mat");
00807 string texid = parentid+string("_tex");
00808
00809 AppearanceInfo& appearanceInfo = (*bodyInfo->appearances())[shapeInfo.appearanceIndex];
00810 const FloatSequence& normals = appearanceInfo.normals;
00811 const FloatSequence& textures = appearanceInfo.textureCoordinate;
00812 domEffectRef pdomeff;
00813 if ( appearanceInfo.materialIndex < 0 ) {
00814 MaterialInfo matInfo;
00815 pdomeff = WriteEffect(matInfo);
00816 } else {
00817 pdomeff = WriteEffect((*bodyInfo->materials())[appearanceInfo.materialIndex]);
00818 }
00819 pdomeff->setId(effid.c_str());
00820
00821 if ( appearanceInfo.textureIndex >= 0 ) {
00822 TextureInfo texture = (*bodyInfo->textures())[appearanceInfo.textureIndex];
00823
00824 domProfile_commonRef pprofile = daeSafeCast<domProfile_common>(pdomeff->getDescendant(daeElement::matchType(domProfile_common::ID())));
00825
00826 domFx_common_newparamRef pparam = daeSafeCast<domFx_common_newparam>(pprofile->add(COLLADA_ELEMENT_NEWPARAM));
00827 pparam->setSid("file1-sampler");
00828 domFx_sampler2DRef psampler = daeSafeCast<domFx_sampler2D>(pparam->add(COLLADA_ELEMENT_SAMPLER2D));
00829 daeElementRef psurface = pparam->add("surface");
00830 daeSafeCast<domInstance_image>(psampler->add("instance_image"))->setUrl(string("#"+texid).c_str());
00831 psampler->add("minfilter")->setCharData("LINEAR_MIPMAP_LINEAR");
00832 psampler->add("magfilter")->setCharData("LINEAR");
00833
00834 domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(pdomeff->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
00835 {
00836 domFx_common_color_or_textureRef pdiffuse = daeSafeCast<domFx_common_color_or_texture>(pphong->getDescendant(daeElement::matchType(domFx_common_color_or_texture::ID())));
00837 pphong->removeFromParent(pphong->getDiffuse());
00838 }
00839
00840 domFx_common_color_or_textureRef pdiffuse = daeSafeCast<domFx_common_color_or_texture>(pphong->add(COLLADA_ELEMENT_DIFFUSE));
00841 domFx_common_color_or_texture::domTextureRef pdiffusetexture = daeSafeCast<domFx_common_color_or_texture::domTexture>(pdiffuse->add(COLLADA_ELEMENT_TEXTURE));
00842 pdiffusetexture->setAttribute("texture", "file1-sampler");
00843 pdiffusetexture->setAttribute("texcoord", "TEX0");
00844
00845 domImageRef pdomtex;
00846 pdomtex = WriteTexture((*bodyInfo->textures())[appearanceInfo.textureIndex]);
00847 pdomtex->setId(texid.c_str());
00848 }
00849
00850 domMaterialRef pdommat = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
00851 pdommat->setId(matid.c_str());
00852 domInstance_effectRef pdominsteff = daeSafeCast<domInstance_effect>(pdommat->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
00853 pdominsteff->setUrl((string("#")+effid).c_str());
00854
00855
00856 if( shapeInfo.primitiveType != SP_MESH ) {
00857 COLLADALOG_WARN("shape index is not SP_MESH type, could result in inaccuracies");
00858 }
00859 domGeometryRef pdomgeom = daeSafeCast<domGeometry>(_geometriesLib->add(COLLADA_ELEMENT_GEOMETRY));
00860 {
00861 pdomgeom->setId(parentid.c_str());
00862 domMeshRef pdommesh = daeSafeCast<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH));
00863 {
00864 domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
00865 {
00866 pvertsource->setId((parentid+string("_positions")).c_str());
00867
00868 domFloat_arrayRef parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
00869 parray->setId((parentid+string("_positions-array")).c_str());
00870 parray->setCount(vertices.length());
00871 parray->setDigits(6);
00872 parray->getValue().setCount(vertices.length());
00873
00874 for(size_t ind = 0; ind < vertices.length(); ind += 3) {
00875 DblArray3 v, vnew;
00876 v[0] = vertices[ind]; v[1] = vertices[ind+1]; v[2] = vertices[ind+2];
00877 PoseMultVector(vnew, transformMatrix, v);
00878 parray->getValue()[ind+0] = vnew[0];
00879 parray->getValue()[ind+1] = vnew[1];
00880 parray->getValue()[ind+2] = vnew[2];
00881 }
00882
00883 domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00884 domAccessorRef pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
00885 pacc->setCount(vertices.length()/3);
00886 pacc->setSource(xsAnyURI(*pacc, string("#")+parentid+string("_positions-array")));
00887 pacc->setStride(3);
00888
00889 domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
00890 px->setName("X"); px->setType("float");
00891 domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
00892 py->setName("Y"); py->setType("float");
00893 domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
00894 pz->setName("Z"); pz->setType("float");
00895 }
00896
00897 domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->add(COLLADA_ELEMENT_VERTICES));
00898 {
00899 pverts->setId("vertices");
00900 domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->add(COLLADA_ELEMENT_INPUT));
00901 pvertinput->setSemantic("POSITION");
00902 pvertinput->setSource(domUrifragment(*pvertsource, string("#")+parentid+string("_positions")));
00903 }
00904
00905 domSourceRef pnormsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
00906 if ( normals.length() > 0 && appearanceInfo.normalIndices.length() > 0 )
00907 {
00908 pnormsource->setId((parentid+string("_normals")).c_str());
00909
00910 domFloat_arrayRef parray = daeSafeCast<domFloat_array>(pnormsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
00911 parray->setId((parentid+string("_normals-array")).c_str());
00912 parray->setCount(normals.length());
00913 parray->setDigits(6);
00914 parray->getValue().setCount(normals.length());
00915
00916 for(size_t ind = 0; ind < normals.length(); ++ind) {
00917 parray->getValue()[ind] = normals[ind];
00918 }
00919
00920 domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pnormsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00921 domAccessorRef pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
00922 pacc->setCount(normals.length()/3);
00923 pacc->setSource(xsAnyURI(*pacc, string("#")+parentid+string("_normals-array")));
00924 pacc->setStride(3);
00925
00926 domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
00927 px->setName("X"); px->setType("float");
00928 domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
00929 py->setName("Y"); py->setType("float");
00930 domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
00931 pz->setName("Z"); pz->setType("float");
00932 }
00933
00934 domSourceRef ptexsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
00935 if ( textures.length() > 0 && appearanceInfo.textureCoordIndices.length() >= 0 )
00936 {
00937 ptexsource->setId((parentid+string("_texcoords")).c_str());
00938
00939 domFloat_arrayRef parray = daeSafeCast<domFloat_array>(ptexsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
00940 parray->setId((parentid+string("_texcoords-array")).c_str());
00941 parray->setCount(textures.length());
00942 parray->setDigits(6);
00943 parray->getValue().setCount(textures.length());
00944
00945 for(size_t ind = 0; ind < textures.length(); ++ind) {
00946 parray->getValue()[ind] = textures[ind];
00947 }
00948
00949 domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(ptexsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00950 domAccessorRef pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
00951 pacc->setCount(vertices.length()/2);
00952 pacc->setSource(xsAnyURI(*pacc, string("#")+parentid+string("_textures-array")));
00953 pacc->setStride(2);
00954
00955 domParamRef ps = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
00956 ps->setName("S"); ps->setType("float");
00957 domParamRef pt = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
00958 pt->setName("T"); pt->setType("float");
00959 }
00960
00961 domTrianglesRef ptris = daeSafeCast<domTriangles>(pdommesh->add(COLLADA_ELEMENT_TRIANGLES));
00962 {
00963 int offset = 0;
00964 ptris->setCount(triangles.length()/3);
00965 ptris->setMaterial("mat0");
00966
00967 domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
00968 pvertoffset->setSemantic("VERTEX");
00969 pvertoffset->setOffset(offset++);
00970 pvertoffset->setSource(domUrifragment(*pverts, string("#")+parentid+string("/vertices")));
00971
00972 domPRef pindices = daeSafeCast<domP>(ptris->add(COLLADA_ELEMENT_P));
00973
00974 if ( normals.length() > 0 && appearanceInfo.normalIndices.length() > 0 ){
00975 domInput_local_offsetRef pnormoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
00976 pnormoffset->setSemantic("NORMAL");
00977 pnormoffset->setOffset(offset++);
00978 pnormoffset->setSource(domUrifragment(*pverts, string("#")+parentid+string("_normals")));
00979
00980 }
00981 if ( textures.length() > 0 && appearanceInfo.textureCoordIndices.length() > 0 ){
00982 domInput_local_offsetRef ptexoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
00983 ptexoffset->setSemantic("TEXCOORD");
00984 ptexoffset->setOffset(offset++);
00985 ptexoffset->setSource(domUrifragment(*pverts, string("#")+parentid+string("_texcoords")));
00986 }
00987 pindices->getValue().setCount(triangles.length()*offset);
00988
00989 for(size_t ind = 0; ind < triangles.length(); ++ind) {
00990 int i = ind * offset;
00991 pindices->getValue()[i++] = triangles[ind];
00992 if ( normals.length() > 0 && appearanceInfo.normalIndices.length() > 0 ){
00993 if ( appearanceInfo.normalPerVertex == 1 ) {
00994 pindices->getValue()[i++] = appearanceInfo.normalIndices[ind];
00995 } else {
00996 pindices->getValue()[i++] = appearanceInfo.normalIndices[triangles[ind]/3];
00997 }
00998 }
00999 if (textures.length() > 0 && appearanceInfo.textureCoordIndices.length() > 0 ){
01000 pindices->getValue()[i++] = appearanceInfo.textureCoordIndices[ind];
01001 }
01002 }
01003 }
01004 }
01005 }
01006
01007 return pdomgeom;
01008 }
01009
01013 virtual domEffectRef WriteEffect(const MaterialInfo& material)
01014 {
01015 domEffectRef pdomeff = daeSafeCast<domEffect>(_effectsLib->add(COLLADA_ELEMENT_EFFECT));
01016
01017 domProfile_commonRef pprofile = daeSafeCast<domProfile_common>(pdomeff->add(COLLADA_ELEMENT_PROFILE_COMMON));
01018
01019 domProfile_common::domTechniqueRef ptec = daeSafeCast<domProfile_common::domTechnique>(pprofile->add(COLLADA_ELEMENT_TECHNIQUE));
01020
01021 domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(ptec->add(COLLADA_ELEMENT_PHONG));
01022
01023 domFx_common_color_or_textureRef pambient = daeSafeCast<domFx_common_color_or_texture>(pphong->add(COLLADA_ELEMENT_AMBIENT));
01024 domFx_common_color_or_texture::domColorRef pambientcolor = daeSafeCast<domFx_common_color_or_texture::domColor>(pambient->add(COLLADA_ELEMENT_COLOR));
01025 pambientcolor->getValue().setCount(4);
01026 pambientcolor->getValue()[0] = material.ambientIntensity;
01027 pambientcolor->getValue()[1] = material.ambientIntensity;
01028 pambientcolor->getValue()[2] = material.ambientIntensity;
01029 pambientcolor->getValue()[3] = 1;
01030
01031 domFx_common_color_or_textureRef pdiffuse = daeSafeCast<domFx_common_color_or_texture>(pphong->add(COLLADA_ELEMENT_DIFFUSE));
01032 domFx_common_color_or_texture::domColorRef pdiffusecolor = daeSafeCast<domFx_common_color_or_texture::domColor>(pdiffuse->add(COLLADA_ELEMENT_COLOR));
01033 pdiffusecolor->getValue().setCount(4);
01034 pdiffusecolor->getValue()[0] = material.diffuseColor[0];
01035 pdiffusecolor->getValue()[1] = material.diffuseColor[1];
01036 pdiffusecolor->getValue()[2] = material.diffuseColor[2];
01037 pdiffusecolor->getValue()[3] = 1;
01038
01039 domFx_common_color_or_textureRef pspecular = daeSafeCast<domFx_common_color_or_texture>(pphong->add(COLLADA_ELEMENT_SPECULAR));
01040 domFx_common_color_or_texture::domColorRef pspecularcolor = daeSafeCast<domFx_common_color_or_texture::domColor>(pspecular->add(COLLADA_ELEMENT_COLOR));
01041 pspecularcolor->getValue().setCount(4);
01042 pspecularcolor->getValue()[0] = material.specularColor[0];
01043 pspecularcolor->getValue()[1] = material.specularColor[1];
01044 pspecularcolor->getValue()[2] = material.specularColor[2];
01045 pspecularcolor->getValue()[3] = 1;
01046
01047 domFx_common_color_or_textureRef pemission = daeSafeCast<domFx_common_color_or_texture>(pphong->add(COLLADA_ELEMENT_EMISSION));
01048 domFx_common_color_or_texture::domColorRef pemissioncolor = daeSafeCast<domFx_common_color_or_texture::domColor>(pemission->add(COLLADA_ELEMENT_COLOR));
01049 pemissioncolor->getValue().setCount(4);
01050 pemissioncolor->getValue()[0] = material.emissiveColor[0];
01051 pemissioncolor->getValue()[1] = material.emissiveColor[1];
01052 pemissioncolor->getValue()[2] = material.emissiveColor[2];
01053 pemissioncolor->getValue()[3] = 1;
01054
01055
01056
01057 return pdomeff;
01058 }
01059
01061 virtual domImageRef WriteTexture(const TextureInfo& texture)
01062 {
01063 domImageRef pdomimg = daeSafeCast<domImage>(_imagesLib->add(COLLADA_ELEMENT_IMAGE));
01064 pdomimg->setName(texture.url);
01065 domImage::domInit_fromRef pdominitfrom = daeSafeCast<domImage::domInit_from>(pdomimg->add(COLLADA_ELEMENT_INIT_FROM));
01066 if ( ! pdominitfrom->setCharData(string(texture.url)) ) {
01067 domImage_source::domRefRef pdomref = daeSafeCast<domImage_source::domRef>(pdominitfrom->add(COLLADA_ELEMENT_REF));
01068 pdomref->setValue(texture.url);
01069 }
01070 return pdomimg;
01071 }
01072
01073 virtual daeElementRef WriteSensor(const SensorInfo& sensor, const string& parentid)
01074 {
01075 daeElementRef domsensor = _sensorsLib->add("sensor");
01076 _nextsensorid++; domsensor->setAttribute("id",str(boost::format("sensor%d")%_nextsensorid).c_str());
01077
01078 string vrmltype = tolowerstring(string(sensor.type));
01079 domsensor->setAttribute("sid",str(boost::format("%d")%sensor.id).c_str());
01080 if( vrmltype == "force" ) {
01081 domsensor->setAttribute("type","base_force6d");
01082 domsensor->add("load_range_force")->setCharData(str(boost::format("%f %f %f")%sensor.specValues[0]%sensor.specValues[1]%sensor.specValues[2]));
01083 domsensor->add("load_range_torque")->setCharData(str(boost::format("%f %f %f")%sensor.specValues[3]%sensor.specValues[4]%sensor.specValues[5]));
01084 }
01085 else if( vrmltype == "rategyro") {
01086 domsensor->setAttribute("type","base_imu");
01087 domsensor->add("max_angular_velocity")->setCharData(str(boost::format("%f %f %f")%sensor.specValues[0]%sensor.specValues[1]%sensor.specValues[2]));
01088 }
01089 else if( vrmltype == "acceleration" ) {
01090 domsensor->setAttribute("type","base_imu");
01091 domsensor->add("max_acceleration")->setCharData(str(boost::format("%f %f %f")%sensor.specValues[0]%sensor.specValues[1]%sensor.specValues[2]));
01092 }
01093 else if( vrmltype == "vision" ) {
01094 domsensor->setAttribute("type","base_pinhole_camera");
01095
01096 if( sensor.specValues.length() != 7 ) {
01097 COLLADALOG_WARN(str(boost::format("vision sensor has wrong number of values! %d!=7")%sensor.specValues.length()));
01098 }
01099 domsensor->add("focal_length")->setCharData(str(boost::format("%f")%sensor.specValues[0]));
01100 double fieldOfView = sensor.specValues[2], width = sensor.specValues[4], height = sensor.specValues[5];
01101 stringstream sintrinsic; sintrinsic << std::setprecision(15);
01102 double fx = 0.5/(tanf(fieldOfView*0.5));
01103 sintrinsic << fx*width << " 0 " << 0.5*width << " 0 " << fx*height << " " << 0.5*height;
01104 domsensor->add("intrinsic")->setCharData(sintrinsic.str());
01105 stringstream simage_dimensions; simage_dimensions << (int)width << " " << (int)height << " ";
01106 string format = "uint8";
01107 Camera::CameraType cameratype = Camera::CameraType((int)sensor.specValues[3]);
01108 switch(cameratype) {
01109 case Camera::NONE:
01110 COLLADALOG_WARN("no camera type specified!");
01111 break;
01112 case Camera::COLOR:
01113 simage_dimensions << 3;
01114 break;
01115 case Camera::MONO:
01116 simage_dimensions << 1;
01117 break;
01118 case Camera::DEPTH:
01119 simage_dimensions << 1;
01120 format = "float32";
01121 break;
01122 case Camera::COLOR_DEPTH:
01123 format = "float32";
01124 simage_dimensions << 4;
01125 break;
01126 case Camera::MONO_DEPTH:
01127 format = "float32";
01128 simage_dimensions << 2;
01129 break;
01130 }
01131 domsensor->add("format")->setCharData(format);
01132 domsensor->add("image_dimensions")->setCharData(simage_dimensions.str());
01133 domsensor->add("measurement_time")->setCharData(str(boost::format("%f")%(1.0/sensor.specValues[6])));
01134 }
01135 else if( vrmltype == "range" ) {
01136 domsensor->setAttribute("type","base_laser1d");
01137 domsensor->add("angle_range")->setCharData(str(boost::format("%f")%(sensor.specValues[0])));
01138 domsensor->add("angle_increment")->setCharData(str(boost::format("%f")%(sensor.specValues[1])));
01139 domsensor->add("measurement_time")->setCharData(str(boost::format("%f")%(1.0/sensor.specValues[2])));
01140 domsensor->add("distance_range")->setCharData(str(boost::format("%f")%(sensor.specValues[3])));
01141 }
01142 return domsensor;
01143 }
01144
01145 virtual daeElementRef WriteActuator(const LinkInfo& plink, const string& parentid)
01146 {
01147 daeElementRef domactuator = _actuatorsLib->add("actuator");
01148 _nextactuatorid++; domactuator->setAttribute("id",str(boost::format("actuator%d")%_nextactuatorid).c_str());
01149 domactuator->setAttribute("type","electric_motor");
01150 domactuator->add("assigned_power_rating")->setCharData("1.0");
01151 double max_speed = plink.uvlimit.length()/2*M_PI > 0 ? plink.uvlimit[0] : 0;
01152 domactuator->add("max_speed")->setCharData(str(boost::format("%f")%max_speed));
01153 domactuator->add("no_load_speed")->setCharData(str(boost::format("%f")%max_speed));
01154 double max_torque = (plink.climit.length() > 0 ? plink.climit[0] : 0)*plink.gearRatio*plink.torqueConst;
01155 domactuator->add("nominal_torque")->setCharData(str(boost::format("%f")%max_torque));
01156 domactuator->add("nominal_voltage")->setCharData("0");
01157 domactuator->add("rotor_inertia")->setCharData(str(boost::format("%f")%(plink.rotorInertia)));
01158 domactuator->add("speed_constant")->setCharData("0");
01159 domactuator->add("speed_torque_gradient")->setCharData("0");
01160 domactuator->add("starting_current")->setCharData("0");
01161 domactuator->add("terminal_resistance")->setCharData(str(boost::format("%f")%(plink.rotorResistor)));
01162 domactuator->add("torque_constant")->setCharData(str(boost::format("%f")%(plink.torqueConst)));
01163 domactuator->add("gear_ratio")->setCharData(str(boost::format("%f")%(plink.gearRatio)));
01164 domactuator->add("encoder_pulse")->setCharData(str(boost::format("%f")%(plink.encoderPulse)));
01165
01166 return domactuator;
01167 }
01168
01169 private:
01170
01172 virtual void _CreateScene()
01173 {
01174
01175 _scene.vscene = daeSafeCast<domVisual_scene>(_visualScenesLib->add(COLLADA_ELEMENT_VISUAL_SCENE));
01176 _scene.vscene->setId("vscene");
01177 _scene.vscene->setName("OpenRAVE Visual Scene");
01178
01179
01180 _scene.kscene = daeSafeCast<domKinematics_scene>(_kinematicsScenesLib->add(COLLADA_ELEMENT_KINEMATICS_SCENE));
01181 _scene.kscene->setId("kscene");
01182 _scene.kscene->setName("OpenRAVE Kinematics Scene");
01183
01184
01185 _scene.pscene = daeSafeCast<domPhysics_scene>(_physicsScenesLib->add(COLLADA_ELEMENT_PHYSICS_SCENE));
01186 _scene.pscene->setId("pscene");
01187 _scene.pscene->setName("OpenRAVE Physics Scene");
01188
01189
01190 _scene.viscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE ));
01191 _scene.viscene->setUrl( (string("#") + string(_scene.vscene->getID())).c_str() );
01192
01193
01194 _scene.kiscene = daeSafeCast<domInstance_kinematics_scene>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE ));
01195 _scene.kiscene->setUrl( (string("#") + string(_scene.kscene->getID())).c_str() );
01196
01197
01198 _scene.piscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE ));
01199 _scene.piscene->setUrl( (string("#") + string(_scene.pscene->getID())).c_str() );
01200 }
01201
01210 virtual LINKOUTPUT _WriteLink(BodyInfo_impl* bodyInfo, int ilink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri, std::map<std::string, int> mapjointnames)
01211 {
01212 LinkInfo& plink = (*bodyInfo->links())[ilink];
01213 LINKOUTPUT out;
01214 string linksid = _GetLinkSid(ilink);
01215 domLinkRef pdomlink = daeSafeCast<domLink>(pkinparent->add(COLLADA_ELEMENT_LINK));
01216 pdomlink->setName(plink.segments[0].name);
01217 pdomlink->setSid(linksid.c_str());
01218
01219 domNodeRef pnode = daeSafeCast<domNode>(pnodeparent->add(COLLADA_ELEMENT_NODE));
01220 std::string nodeid = _GetNodeId(bodyInfo,ilink);
01221 pnode->setId( nodeid.c_str() );
01222 string nodesid = str(boost::format("node%d")%ilink);
01223 pnode->setSid(nodesid.c_str());
01224 pnode->setName(plink.segments[0].name);
01225 ShapeInfoSequence* curShapeInfoSeq = bodyInfo->shapes();
01226
01227 for(unsigned int igeom = 0; igeom < plink.shapeIndices.length(); ++igeom) {
01228 string geomid = _GetGeometryId(bodyInfo, ilink,igeom);
01229 const TransformedShapeIndex& tsi = plink.shapeIndices[igeom];
01230 DblArray12 transformMatrix;
01231
01232 for(int i = 0; i < 12; ++i) {
01233 transformMatrix[i] = tsi.transformMatrix[i];
01234 }
01235 domGeometryRef pdomgeom = WriteGeometry(bodyInfo,(*curShapeInfoSeq)[tsi.shapeIndex], transformMatrix, geomid);
01236 domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
01237 pinstgeom->setUrl((string("#")+geomid).c_str());
01238
01239 domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
01240 domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
01241 domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
01242 pinstmat->setTarget(xsAnyURI(*pinstmat, string("#")+geomid+string("_mat")));
01243 pinstmat->setSymbol("mat0");
01244 }
01245
01246
01247 int igeomid = plink.shapeIndices.length();
01248 for(size_t isensor = 0; isensor < plink.sensors.length(); ++isensor) {
01249 SensorInfo& sensor = plink.sensors[isensor];
01250
01251 for(unsigned int igeom = 0; igeom < sensor.shapeIndices.length(); ++igeom) {
01252 string geomid = _GetGeometryId(bodyInfo, ilink, igeomid++);
01253
01254 const TransformedShapeIndex& tsi = sensor.shapeIndices[igeom];
01255 DblArray12 sensorMatrix, transformMatrix,sensorgeomMatrix;
01256 PoseFromAxisAngleTranslation(sensorMatrix,sensor.rotation, sensor.translation);
01257 for(int i = 0; i < 12; ++i) {
01258 sensorgeomMatrix[i] = tsi.transformMatrix[i];
01259 }
01260 PoseMult(transformMatrix, sensorMatrix, sensorgeomMatrix);
01261
01262 domGeometryRef pdomgeom = WriteGeometry(bodyInfo,(*bodyInfo->shapes())[tsi.shapeIndex], transformMatrix, geomid);
01263 domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
01264 pinstgeom->setUrl((string("#")+geomid).c_str());
01265
01266 domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
01267 domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
01268 domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
01269 pinstmat->setTarget(xsAnyURI(*pinstmat, string("#")+geomid+string("_mat")));
01270 pinstmat->setSymbol("mat0");
01271 }
01272 }
01273
01274
01275 for(unsigned int _ichild = 0; _ichild < plink.childIndices.length(); ++_ichild) {
01276 int ichild = plink.childIndices[_ichild];
01277 LinkInfo& childlink = (*bodyInfo->links())[ichild];
01278 domLink::domAttachment_fullRef pattfull = daeSafeCast<domLink::domAttachment_full>(pdomlink->add(COLLADA_TYPE_ATTACHMENT_FULL));
01279
01280 string jointid = str(boost::format("%s/jointsid%d")%strModelUri%mapjointnames[string(childlink.name)]);
01281 pattfull->setJoint(jointid.c_str());
01282
01283 LINKOUTPUT childinfo = _WriteLink(bodyInfo, ichild, pattfull, pnode, strModelUri, mapjointnames);
01284 out.listusedlinks.insert(out.listusedlinks.end(),childinfo.listusedlinks.begin(),childinfo.listusedlinks.end());
01285
01286 _WriteTransformation(pattfull, childlink.rotation, childlink.translation);
01287
01288
01289
01290 string jointnodesid = _GetJointNodeSid(ichild,0);
01291 string jointType(CORBA::String_var(childlink.jointType));
01292 if( jointType == "rotate" || jointType == "fixed" ) {
01293 domRotateRef protate = daeSafeCast<domRotate>(childinfo.pnode->add(COLLADA_ELEMENT_ROTATE,0));
01294 protate->setSid(jointnodesid.c_str());
01295 protate->getValue().setCount(4);
01296 protate->getValue()[0] = childlink.jointAxis[0];
01297 protate->getValue()[1] = childlink.jointAxis[1];
01298 protate->getValue()[2] = childlink.jointAxis[2];
01299 protate->getValue()[3] = 0;
01300 }
01301 else if( jointType == "slide" ) {
01302 domTranslateRef ptrans = daeSafeCast<domTranslate>(childinfo.pnode->add(COLLADA_ELEMENT_TRANSLATE,0));
01303 ptrans->setSid(jointnodesid.c_str());
01304 ptrans->getValue().setCount(3);
01305 ptrans->getValue()[0] = 0;
01306 ptrans->getValue()[1] = 0;
01307 ptrans->getValue()[2] = 0;
01308 }
01309 else {
01310 COLLADALOG_WARN(str(boost::format("unsupported joint type specified %s")%jointType));
01311 }
01312 _WriteTransformation(childinfo.pnode, childlink.rotation, childlink.translation);
01313 }
01314
01315 out.listusedlinks.push_back(make_pair(ilink,linksid));
01316 out.plink = pdomlink;
01317 out.pnode = pnode;
01318 return out;
01319 }
01320
01321 void _SetRotate(domTargetable_float4Ref prot, const DblArray4& rotation)
01322 {
01323 prot->getValue().setCount(4);
01324 prot->getValue()[0] = rotation[0];
01325 prot->getValue()[1] = rotation[1];
01326 prot->getValue()[2] = rotation[2];
01327 prot->getValue()[3] = rotation[3]*(180.0/M_PI);
01328 }
01329
01333 void _WriteTransformation(daeElementRef pelt, const DblArray4& rotation, const DblArray3& translation)
01334 {
01335 _SetRotate(daeSafeCast<domRotate>(pelt->add(COLLADA_ELEMENT_ROTATE,0)), rotation);
01336 _SetVector3(daeSafeCast<domTranslate>(pelt->add(COLLADA_ELEMENT_TRANSLATE,0))->getValue(),translation);
01337 }
01338
01339
01340 void _WriteBindingsInstance_kinematics_scene(domInstance_kinematics_sceneRef ikscene, BodyInfo_impl* bodyInfo, const std::vector<axis_sids>& vaxissids, const std::vector<std::pair<std::string,std::string> >& vkinematicsbindings)
01341 {
01342 for(std::vector<std::pair<std::string,std::string> >::const_iterator it = vkinematicsbindings.begin(); it != vkinematicsbindings.end(); ++it) {
01343 domBind_kinematics_modelRef pmodelbind = daeSafeCast<domBind_kinematics_model>(ikscene->add(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL));
01344 pmodelbind->setNode(it->second.c_str());
01345 daeSafeCast<domCommon_param>(pmodelbind->add(COLLADA_ELEMENT_PARAM))->setValue(it->first.c_str());
01346 }
01347 for(std::vector<axis_sids>::const_iterator it = vaxissids.begin(); it != vaxissids.end(); ++it) {
01348 domBind_joint_axisRef pjointbind = daeSafeCast<domBind_joint_axis>(ikscene->add(COLLADA_ELEMENT_BIND_JOINT_AXIS));
01349 pjointbind->setTarget(it->jointnodesid.c_str());
01350 daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_AXIS)->add(COLLADA_TYPE_PARAM))->setValue(it->axissid.c_str());
01351 daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_VALUE)->add(COLLADA_TYPE_PARAM))->setValue(it->valuesid.c_str());
01352 }
01353 }
01354
01356 template <typename T> static void _SetVector3(T& t, const DblArray3& v) {
01357 t.setCount(3);
01358 t[0] = v[0];
01359 t[1] = v[1];
01360 t[2] = v[2];
01361 }
01362
01363 virtual void _AddKinematics_model(BodyInfo_impl* bodyInfo, boost::shared_ptr<kinematics_model_output> kmout) {
01364 string xmlfilename;
01365 if( !!bodyInfo->url() ) {
01366 xmlfilename = bodyInfo->url();
01367 }
01368 for(std::list<kinbody_models>::iterator it = _listkinbodies.begin(); it != _listkinbodies.end(); ++it) {
01369 if( it->xmlfilename == xmlfilename ) {
01370 BOOST_ASSERT(!it->kmout);
01371 it->kmout = kmout;
01372 return;
01373 }
01374 }
01375 kinbody_models cache;
01376 cache.xmlfilename = xmlfilename;
01377 cache.kinematicsgeometryhash = "";
01378 cache.kmout = kmout;
01379 _listkinbodies.push_back(cache);
01380 }
01381
01382 virtual boost::shared_ptr<kinematics_model_output> _GetKinematics_model(BodyInfo_impl* bodyInfo) {
01383 for(std::list<kinbody_models>::iterator it = _listkinbodies.begin(); it != _listkinbodies.end(); ++it) {
01384 if( !bodyInfo->url() || it->xmlfilename == bodyInfo->url() ) {
01385 return it->kmout;
01386 }
01387 }
01388 return boost::shared_ptr<kinematics_model_output>();
01389 }
01390
01391 virtual void _AddPhysics_model(BodyInfo_impl* bodyInfo, boost::shared_ptr<physics_model_output> pmout) {
01392 string xmlfilename;
01393 if( !!bodyInfo->url() ) {
01394 xmlfilename = bodyInfo->url();
01395 }
01396 for(std::list<kinbody_models>::iterator it = _listkinbodies.begin(); it != _listkinbodies.end(); ++it) {
01397 if( it->xmlfilename == xmlfilename ) {
01398 BOOST_ASSERT(!it->pmout);
01399 it->pmout = pmout;
01400 return;
01401 }
01402 }
01403 kinbody_models cache;
01404 cache.xmlfilename = xmlfilename;
01405 cache.kinematicsgeometryhash = "";
01406 cache.pmout = pmout;
01407 _listkinbodies.push_back(cache);
01408 }
01409
01410 virtual boost::shared_ptr<physics_model_output> _GetPhysics_model(BodyInfo_impl* bodyInfo) {
01411 for(std::list<kinbody_models>::iterator it = _listkinbodies.begin(); it != _listkinbodies.end(); ++it) {
01412 if( !bodyInfo->url() || it->xmlfilename == bodyInfo->url() ) {
01413 return it->pmout;
01414 }
01415 }
01416 return boost::shared_ptr<physics_model_output>();
01417 }
01418
01419 virtual int _GetRobotId(BodyInfo_impl* bodyInfo) {
01420 return 1;
01421 }
01422 virtual std::string _GetNodeId(BodyInfo_impl* bodyInfo) {
01423 return str(boost::format("visual%d")%_GetRobotId(bodyInfo));
01424 }
01425 virtual std::string _GetNodeId(BodyInfo_impl* bodyInfo, int ilink) {
01426 return str(boost::format("v%d.node%d")%_GetRobotId(bodyInfo)%ilink);
01427 }
01428
01429 virtual std::string _GetLinkSid(int ilink) {
01430 return str(boost::format("link%d")%ilink);
01431 }
01432
01433 virtual std::string _GetGeometryId(BodyInfo_impl* bodyInfo, int ilink, int igeom) {
01434 return str(boost::format("g%d_%d_geom%d")%_GetRobotId(bodyInfo)%ilink%igeom);
01435 }
01436 virtual std::string _GetJointNodeSid(int ijoint, int iaxis) {
01437 return str(boost::format("node_joint%d_axis%d")%ijoint%iaxis);
01438 }
01439
01440 virtual void handleError( daeString msg )
01441 {
01442 cerr << "COLLADA error: " << msg << endl;
01443 }
01444
01445 virtual void handleWarning( daeString msg )
01446 {
01447 cout << "COLLADA warning: " << msg << endl;
01448 }
01449
01450 boost::shared_ptr<DAE> _collada;
01451 domCOLLADA* _dom;
01452 daeDocument* _doc;
01453 domCOLLADA::domSceneRef _globalscene;
01454 domLibrary_visual_scenesRef _visualScenesLib;
01455 domLibrary_kinematics_scenesRef _kinematicsScenesLib;
01456 domLibrary_kinematics_modelsRef _kinematicsModelsLib;
01457 domLibrary_articulated_systemsRef _articulatedSystemsLib;
01458 domLibrary_physics_scenesRef _physicsScenesLib;
01459 domLibrary_physics_modelsRef _physicsModelsLib;
01460 domLibrary_materialsRef _materialsLib;
01461 domLibrary_effectsRef _effectsLib;
01462 domLibrary_imagesRef _imagesLib;
01463 domLibrary_geometriesRef _geometriesLib;
01464 domTechniqueRef _sensorsLib;
01465 domTechniqueRef _actuatorsLib;
01466 int _nextsensorid, _nextactuatorid;
01467 SCENE _scene;
01468 std::list<kinbody_models> _listkinbodies;
01469 std::list<ManipulatorInfo> _listmanipulators;
01470 };
01471
01472 #endif