00001
00002
00003
00004
00005
00006
00007
00008
00009
00016 #include "BodyInfo_impl.h"
00017
00018 #include <map>
00019 #include <vector>
00020 #include <iostream>
00021 #include <boost/bind.hpp>
00022 #include <boost/format.hpp>
00023
00024 #include <hrpCorba/ViewSimulator.hh>
00025 #include <hrpUtil/EasyScanner.h>
00026 #include <hrpUtil/VrmlNodes.h>
00027 #include <hrpUtil/VrmlParser.h>
00028 #include <hrpUtil/ImageConverter.h>
00029
00030 #include "VrmlUtil.h"
00031
00032
00033
00034 using namespace std;
00035 using namespace boost;
00036
00037 namespace {
00038 typedef map<string, string> SensorTypeMap;
00039 SensorTypeMap sensorTypeMap;
00040 }
00041
00042
00043 BodyInfo_impl::BodyInfo_impl(PortableServer::POA_ptr poa) :
00044 ShapeSetInfo_impl(poa)
00045 {
00046 lastUpdate_ = 0;
00047 }
00048
00049
00050 BodyInfo_impl::~BodyInfo_impl()
00051 {
00052
00053 }
00054
00055
00056 const std::string& BodyInfo_impl::topUrl()
00057 {
00058 return url_;
00059 }
00060
00061
00062 char* BodyInfo_impl::name()
00063 {
00064 return CORBA::string_dup(name_.c_str());
00065 }
00066
00067
00068 char* BodyInfo_impl::url()
00069 {
00070 return CORBA::string_dup(url_.c_str());
00071 }
00072
00073
00074 StringSequence* BodyInfo_impl::info()
00075 {
00076 return new StringSequence(info_);
00077 }
00078
00079
00080 LinkInfoSequence* BodyInfo_impl::links()
00081 {
00082 return new LinkInfoSequence(links_);
00083 }
00084
00085
00086 AllLinkShapeIndexSequence* BodyInfo_impl::linkShapeIndices()
00087 {
00088 return new AllLinkShapeIndexSequence(linkShapeIndices_);
00089 }
00090
00091 ExtraJointInfoSequence* BodyInfo_impl::extraJoints()
00092 {
00093 return new ExtraJointInfoSequence(extraJoints_);
00094 }
00095
00104 void BodyInfo_impl::loadModelFile(const std::string& url)
00105 {
00106 string filename( deleteURLScheme( url ) );
00107
00108
00109 string url2;
00110 url2 = filename;
00111 replace( url2, string("\\"), string("/") );
00112 filename = url2;
00113 url_ = CORBA::string_dup(url2.c_str());
00114
00115
00116 ModelNodeSet modelNodeSet;
00117 modelNodeSet.sigMessage.connect(boost::bind(&putMessage, _1));
00118
00119 bool result = false;
00120
00121 try {
00122 result = modelNodeSet.loadModelFile( filename );
00123
00124 if(result){
00125 applyTriangleMeshShaper(modelNodeSet.humanoidNode());
00126 }
00127 cout.flush();
00128 }
00129 catch(const ModelNodeSet::Exception& ex) {
00130 cout << ex.what() << endl;
00131 cout << "Retrying to load the file as a standard VRML file" << endl;
00132 try {
00133 VrmlParser parser;
00134 parser.load(filename);
00135
00136 links_.length(1);
00137 LinkInfo &linfo = links_[0];
00138 linfo.name = CORBA::string_dup("root");
00139 linfo.parentIndex = -1;
00140 linfo.jointId = -1;
00141 linfo.jointType = CORBA::string_dup("fixed");
00142 linfo.jointValue = 0;
00143 for (int i=0; i<3; i++){
00144 linfo.jointAxis[i] = 0;
00145 linfo.translation[i] = 0;
00146 linfo.rotation[i] = 0;
00147 linfo.centerOfMass[i] = 0;
00148 }
00149 linfo.jointAxis[2] = 1;
00150 linfo.rotation[2] = 1; linfo.rotation[3] = 0;
00151 linfo.mass = 0;
00152 for (int i=0; i<9; i++) linfo.inertia[i] = 0;
00153
00154
00155 Matrix44 E(Matrix44::Identity());
00156
00157 while(VrmlNodePtr node = parser.readNode()){
00158 if(!node->isCategoryOf(PROTO_DEF_NODE)){
00159 applyTriangleMeshShaper(node);
00160 traverseShapeNodes(node.get(), E, linfo.shapeIndices, linfo.inlinedShapeTransformMatrices, &topUrl());
00161 }
00162 }
00163 return;
00164 } catch(EasyScanner::Exception& ex){
00165 cout << ex.getFullMessage() << endl;
00166 throw ModelLoader::ModelLoaderException(ex.getFullMessage().c_str());
00167 }
00168 }
00169
00170 if(!result){
00171 throw ModelLoader::ModelLoaderException("The model file cannot be loaded.");
00172 }
00173
00174 const string& humanoidName = modelNodeSet.humanoidNode()->defName;
00175 name_ = humanoidName.c_str();
00176 const MFString& info = modelNodeSet.humanoidNode()->fields["info"].mfString();
00177 info_.length(info.size());
00178 for (unsigned int i=0; i<info_.length(); i++){
00179 info_[i] = CORBA::string_dup(info[i].c_str());
00180 }
00181
00182 int numJointNodes = modelNodeSet.numJointNodes();
00183
00184 links_.length(numJointNodes);
00185 if( 0 < numJointNodes ) {
00186 int currentIndex = 0;
00187 JointNodeSetPtr rootJointNodeSet = modelNodeSet.rootJointNodeSet();
00188 readJointNodeSet(rootJointNodeSet, currentIndex, -1);
00189 }
00190
00191 linkShapeIndices_.length(numJointNodes);
00192 for(int i = 0 ; i < numJointNodes ; ++i) {
00193 linkShapeIndices_[i] = links_[i].shapeIndices;
00194 }
00195
00196
00197 linkColdetModels.resize(numJointNodes);
00198 for(int linkIndex = 0; linkIndex < numJointNodes ; ++linkIndex){
00199 ColdetModelPtr coldetModel(new ColdetModel());
00200 coldetModel->setName(std::string(links_[linkIndex].name));
00201 int vertexIndex = 0;
00202 int triangleIndex = 0;
00203
00204 Matrix44 E(Matrix44::Identity());
00205 const TransformedShapeIndexSequence& shapeIndices = linkShapeIndices_[linkIndex];
00206 setColdetModel(coldetModel, shapeIndices, E, vertexIndex, triangleIndex);
00207
00208 Matrix44 T(Matrix44::Identity());
00209 const SensorInfoSequence& sensors = links_[linkIndex].sensors;
00210 for (unsigned int i=0; i<sensors.length(); i++){
00211 const SensorInfo& sensor = sensors[i];
00212 calcRodrigues(T, Vector3(sensor.rotation[0], sensor.rotation[1],
00213 sensor.rotation[2]), sensor.rotation[3]);
00214 T(0,3) = sensor.translation[0];
00215 T(1,3) = sensor.translation[1];
00216 T(2,3) = sensor.translation[2];
00217 const TransformedShapeIndexSequence& sensorShapeIndices = sensor.shapeIndices;
00218 setColdetModel(coldetModel, sensorShapeIndices, T, vertexIndex, triangleIndex);
00219 }
00220
00221 if(triangleIndex>0)
00222 coldetModel->build();
00223
00224 linkColdetModels[linkIndex] = coldetModel;
00225 links_[linkIndex].AABBmaxDepth = coldetModel->getAABBTreeDepth();
00226 links_[linkIndex].AABBmaxNum = coldetModel->getAABBmaxNum();
00227 }
00228
00229
00230
00231 int n = modelNodeSet.numExtraJointNodes();
00232 extraJoints_.length(n);
00233 for(int i=0; i < n; ++i){
00234
00235 TProtoFieldMap& f = modelNodeSet.extraJointNode(i)->fields;
00236 ExtraJointInfo_var extraJointInfo(new ExtraJointInfo());
00237 extraJointInfo->name = CORBA::string_dup( modelNodeSet.extraJointNode(i)->defName.c_str() );
00238
00239 string link1Name, link2Name;
00240 copyVrmlField( f, "link1Name", link1Name );
00241 copyVrmlField( f, "link2Name", link2Name );
00242 extraJointInfo->link[0] = CORBA::string_dup(link1Name.c_str());
00243 extraJointInfo->link[1] = CORBA::string_dup(link2Name.c_str());
00244
00245 string jointType;
00246 copyVrmlField( f, "jointType", jointType);
00247 if(jointType == "xy"){
00248 extraJointInfo->jointType = EJ_XY;
00249 } else if(jointType == "xyz"){
00250 extraJointInfo->jointType = EJ_XYZ;
00251 } else if(jointType == "z"){
00252 extraJointInfo->jointType = EJ_Z;
00253 }else {
00254 throw ModelNodeSet::Exception(str(format("JointType \"%1%\" is not supported.") % jointType));
00255 }
00256 copyVrmlField( f, "jointAxis", extraJointInfo->axis );
00257 copyVrmlField( f, "link1LocalPos", extraJointInfo->point[0] );
00258 copyVrmlField( f, "link2LocalPos", extraJointInfo->point[1] );
00259
00260 extraJoints_[i] = extraJointInfo;
00261 }
00262 }
00263
00264
00265 int BodyInfo_impl::readJointNodeSet(JointNodeSetPtr jointNodeSet, int& currentIndex, int parentIndex)
00266 {
00267 int index = currentIndex;
00268 currentIndex++;
00269
00270 LinkInfo_var linkInfo(new LinkInfo());
00271 linkInfo->parentIndex = parentIndex;
00272
00273 size_t numChildren = jointNodeSet->childJointNodeSets.size();
00274
00275 for( size_t i = 0 ; i < numChildren ; ++i ){
00276 JointNodeSetPtr childJointNodeSet = jointNodeSet->childJointNodeSets[i];
00277 int childIndex = readJointNodeSet(childJointNodeSet, currentIndex, index);
00278
00279 long childIndicesLength = linkInfo->childIndices.length();
00280 linkInfo->childIndices.length( childIndicesLength + 1 );
00281 linkInfo->childIndices[childIndicesLength] = childIndex;
00282 }
00283
00284 links_[index] = linkInfo;
00285 try {
00286 vector<VrmlProtoInstancePtr>& segmentNodes = jointNodeSet->segmentNodes;
00287 int numSegment = segmentNodes.size();
00288 links_[index].segments.length(numSegment);
00289 for(int i = 0 ; i < numSegment ; ++i){
00290 SegmentInfo_var segmentInfo(new SegmentInfo());
00291 Matrix44 T = jointNodeSet->transforms.at(i);
00292 long s = links_[index].shapeIndices.length();
00293 int p = 0;
00294 for(int row=0; row < 3; ++row){
00295 for(int col=0; col < 4; ++col){
00296 segmentInfo->transformMatrix[p++] = T(row, col);
00297 }
00298 }
00299 traverseShapeNodes(segmentNodes[i].get(), T, links_[index].shapeIndices, links_[index].inlinedShapeTransformMatrices, &topUrl());
00300 long e =links_[index].shapeIndices.length();
00301 segmentInfo->shapeIndices.length(e-s);
00302 for(int j=0, k=s; k<e; k++)
00303 segmentInfo->shapeIndices[j++] = k;
00304 links_[index].segments[i] = segmentInfo;
00305 }
00306 setJointParameters(index, jointNodeSet->jointNode);
00307 setSegmentParameters(index, jointNodeSet);
00308 setSensors(index, jointNodeSet);
00309 setHwcs(index, jointNodeSet);
00310 setLights(index, jointNodeSet);
00311 }
00312 catch( ModelLoader::ModelLoaderException& ex ) {
00313 string name(linkInfo->name);
00314 string error = name.empty() ? "Unnamed JointNode" : name;
00315 error += ": ";
00316 error += ex.description;
00317 throw ModelLoader::ModelLoaderException( error.c_str() );
00318 }
00319
00320 return index;
00321 }
00322
00323
00324 void BodyInfo_impl::setJointParameters(int linkInfoIndex, VrmlProtoInstancePtr jointNode)
00325 {
00326 LinkInfo& linkInfo = links_[linkInfoIndex];
00327
00328 linkInfo.name = CORBA::string_dup( jointNode->defName.c_str() );
00329
00330 TProtoFieldMap& fmap = jointNode->fields;
00331
00332 CORBA::Long jointId;
00333 copyVrmlField( fmap, "jointId", jointId );
00334 linkInfo.jointId = (CORBA::Short)jointId;
00335
00336 linkInfo.jointAxis[0] = 0.0;
00337 linkInfo.jointAxis[1] = 0.0;
00338 linkInfo.jointAxis[2] = 0.0;
00339
00340 VrmlVariantField& fJointAxis = fmap["jointAxis"];
00341
00342 switch( fJointAxis.typeId() ) {
00343
00344 case SFSTRING:
00345 {
00346 SFString& axisLabel = fJointAxis.sfString();
00347 if( axisLabel == "X" ) { linkInfo.jointAxis[0] = 1.0; }
00348 else if( axisLabel == "Y" ) { linkInfo.jointAxis[1] = 1.0; }
00349 else if( axisLabel == "Z" ) { linkInfo.jointAxis[2] = 1.0; }
00350 }
00351 break;
00352
00353 case SFVEC3F:
00354 copyVrmlField( fmap, "jointAxis", linkInfo.jointAxis );
00355 break;
00356
00357 default:
00358 break;
00359 }
00360
00361 std::string jointType;
00362 copyVrmlField( fmap, "jointType", jointType );
00363 linkInfo.jointType = CORBA::string_dup( jointType.c_str() );
00364
00365 copyVrmlField( fmap, "translation", linkInfo.translation );
00366 copyVrmlRotationFieldToDblArray4( fmap, "rotation", linkInfo.rotation );
00367
00368 copyVrmlField( fmap, "ulimit", linkInfo.ulimit );
00369 copyVrmlField( fmap, "llimit", linkInfo.llimit );
00370 copyVrmlField( fmap, "uvlimit", linkInfo.uvlimit );
00371 copyVrmlField( fmap, "lvlimit", linkInfo.lvlimit );
00372
00373 if(fmap["climit"].typeId() != UNDETERMINED_FIELD_TYPE){
00374 copyVrmlField( fmap, "climit", linkInfo.climit );
00375 }else{
00376
00377 linkInfo.climit.length((CORBA::ULong)0);
00378 }
00379
00380 copyVrmlField( fmap, "gearRatio", linkInfo.gearRatio );
00381 copyVrmlField( fmap, "rotorInertia", linkInfo.rotorInertia );
00382 copyVrmlField( fmap, "rotorResistor", linkInfo.rotorResistor );
00383 copyVrmlField( fmap, "torqueConst", linkInfo.torqueConst );
00384 copyVrmlField( fmap, "encoderPulse", linkInfo.encoderPulse );
00385 copyVrmlField( fmap, "jointValue", linkInfo.jointValue );
00386 }
00387
00388 void BodyInfo_impl::setSegmentParameters(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
00389 {
00390 LinkInfo& linkInfo = links_[linkInfoIndex];
00391
00392 vector<VrmlProtoInstancePtr>& segmentNodes = jointNodeSet->segmentNodes;
00393 int numSegment = segmentNodes.size();
00394
00395 linkInfo.mass = 0.0;
00396 for( int i = 0 ; i < 3 ; ++i ) {
00397 linkInfo.centerOfMass[i] = 0.0;
00398 for( int j = 0 ; j < 3 ; ++j ) {
00399 linkInfo.inertia[i*3 + j] = 0.0;
00400 }
00401 }
00402
00403
00404
00405
00406
00407
00408
00409 std::vector<Vector4, Eigen::aligned_allocator<Vector4> > centerOfMassArray;
00410 std::vector<double> massArray;
00411 for(int i = 0 ; i < numSegment ; ++i){
00412 SegmentInfo& segmentInfo = linkInfo.segments[i];
00413 Matrix44 T = jointNodeSet->transforms.at(i);
00414 DblArray3& centerOfMass = segmentInfo.centerOfMass;
00415 CORBA::Double& mass =segmentInfo.mass;
00416 DblArray9& inertia = segmentInfo.inertia;
00417 TProtoFieldMap& fmap = segmentNodes[i]->fields;
00418 copyVrmlField( fmap, "centerOfMass", centerOfMass );
00419 copyVrmlField( fmap, "mass", mass );
00420 copyVrmlField( fmap, "momentsOfInertia", inertia );
00421 Vector4 c0(centerOfMass[0], centerOfMass[1], centerOfMass[2], 1.0);
00422 Vector4 c1(T * c0);
00423 centerOfMassArray.push_back(c1);
00424 massArray.push_back(mass);
00425 for(int j=0; j<3; j++){
00426 linkInfo.centerOfMass[j] = c1(j) * mass + linkInfo.centerOfMass[j] * linkInfo.mass;
00427 }
00428 linkInfo.mass += mass;
00429 if(linkInfo.mass > 0.0){
00430 for(int j=0; j<3; j++){
00431 linkInfo.centerOfMass[j] /= linkInfo.mass;
00432 }
00433 }
00434 Matrix33 I;
00435 I << inertia[0], inertia[1], inertia[2], inertia[3], inertia[4], inertia[5], inertia[6], inertia[7], inertia[8];
00436 Matrix33 R;
00437 R << T(0,0), T(0,1), T(0,2), T(1,0), T(1,1), T(1,2), T(2,0), T(2,1), T(2,2);
00438 Matrix33 I1(R * I * R.transpose());
00439 for(int j=0; j<3; j++){
00440 for(int k=0; k<3; k++)
00441 linkInfo.inertia[j*3+k] += I1(j,k);
00442 }
00443 segmentInfo.name = CORBA::string_dup( segmentNodes[i]->defName.c_str() );
00444 }
00445 if(linkInfo.mass <=0.0 )
00446 std::cerr << "Warning: Mass is zero. <Model>" << name_ << " <Link>" << linkInfo.name << std::endl;
00447
00448 for(int i = 0 ; i < numSegment ; ++i){
00449 Vector4 c( centerOfMassArray.at(i) );
00450 double x = c(0) - linkInfo.centerOfMass[0];
00451 double y = c(1) - linkInfo.centerOfMass[1];
00452 double z = c(2) - linkInfo.centerOfMass[2];
00453 double m = massArray.at(i);
00454
00455 linkInfo.inertia[0] += m * (y*y + z*z);
00456 linkInfo.inertia[1] += -m * x * y;
00457 linkInfo.inertia[2] += -m * x * z;
00458 linkInfo.inertia[3] += -m * y * x;
00459 linkInfo.inertia[4] += m * (z*z + x*x);
00460 linkInfo.inertia[5] += -m * y * z;
00461 linkInfo.inertia[6] += -m * z * x;
00462 linkInfo.inertia[7] += -m * z * y;
00463 linkInfo.inertia[8] += m * (x*x + y*y);
00464 }
00465 }
00466
00467
00468 void BodyInfo_impl::setSensors(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
00469 {
00470 LinkInfo& linkInfo = links_[linkInfoIndex];
00471
00472 vector<VrmlProtoInstancePtr>& sensorNodes = jointNodeSet->sensorNodes;
00473
00474 int numSensors = sensorNodes.size();
00475 linkInfo.sensors.length(numSensors);
00476
00477 for(int i = 0 ; i < numSensors ; ++i) {
00478 SensorInfo_var sensorInfo( new SensorInfo() );
00479 readSensorNode( linkInfoIndex, sensorInfo, sensorNodes[i] );
00480 linkInfo.sensors[i] = sensorInfo;
00481 }
00482 }
00483
00484
00485 void BodyInfo_impl::setHwcs(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
00486 {
00487 LinkInfo& linkInfo = links_[linkInfoIndex];
00488
00489 vector<VrmlProtoInstancePtr>& hwcNodes = jointNodeSet->hwcNodes;
00490
00491 int numHwcs = hwcNodes.size();
00492 linkInfo.hwcs.length(numHwcs);
00493
00494 for(int i = 0 ; i < numHwcs ; ++i) {
00495 HwcInfo_var hwcInfo( new HwcInfo() );
00496 readHwcNode( linkInfoIndex, hwcInfo, hwcNodes[i] );
00497 linkInfo.hwcs[i] = hwcInfo;
00498 }
00499 }
00500
00501 void BodyInfo_impl::setLights(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
00502 {
00503 LinkInfo& linkInfo = links_[linkInfoIndex];
00504
00505 vector<pair<Matrix44, VrmlNodePtr>,
00506 Eigen::aligned_allocator<pair<Matrix44, VrmlNodePtr> > >& lightNodes = jointNodeSet->lightNodes;
00507
00508 int numLights = lightNodes.size();
00509 linkInfo.lights.length(numLights);
00510
00511 for(int i = 0 ; i < numLights ; ++i) {
00512 LightInfo_var lightInfo( new LightInfo() );
00513 readLightNode( linkInfoIndex, lightInfo, lightNodes[i] );
00514 linkInfo.lights[i] = lightInfo;
00515 }
00516 }
00517
00518
00519 void BodyInfo_impl::readSensorNode(int linkInfoIndex, SensorInfo& sensorInfo, VrmlProtoInstancePtr sensorNode)
00520 {
00521 if(sensorTypeMap.empty()) {
00522 sensorTypeMap["ForceSensor"] = "Force";
00523 sensorTypeMap["Gyro"] = "RateGyro";
00524 sensorTypeMap["AccelerationSensor"] = "Acceleration";
00525 sensorTypeMap["PressureSensor"] = "";
00526 sensorTypeMap["PhotoInterrupter"] = "";
00527 sensorTypeMap["VisionSensor"] = "Vision";
00528 sensorTypeMap["TorqueSensor"] = "";
00529 sensorTypeMap["RangeSensor"] = "Range";
00530 }
00531
00532 try {
00533 sensorInfo.name = CORBA::string_dup( sensorNode->defName.c_str() );
00534
00535 TProtoFieldMap& fmap = sensorNode->fields;
00536
00537 copyVrmlField(fmap, "sensorId", sensorInfo.id );
00538 copyVrmlField(fmap, "translation", sensorInfo.translation );
00539 copyVrmlRotationFieldToDblArray4( fmap, "rotation", sensorInfo.rotation );
00540
00541 SensorTypeMap::iterator p = sensorTypeMap.find( sensorNode->proto->protoName );
00542 std::string sensorType;
00543 if(p != sensorTypeMap.end()){
00544 sensorType = p->second;
00545 sensorInfo.type = CORBA::string_dup( sensorType.c_str() );
00546 } else {
00547 throw ModelLoader::ModelLoaderException("Unknown Sensor Node");
00548 }
00549
00550 if(sensorType == "Force") {
00551 sensorInfo.specValues.length( CORBA::ULong(6) );
00552 DblArray3 maxForce, maxTorque;
00553 copyVrmlField(fmap, "maxForce", maxForce );
00554 copyVrmlField(fmap, "maxTorque", maxTorque );
00555 sensorInfo.specValues[0] = maxForce[0];
00556 sensorInfo.specValues[1] = maxForce[1];
00557 sensorInfo.specValues[2] = maxForce[2];
00558 sensorInfo.specValues[3] = maxTorque[0];
00559 sensorInfo.specValues[4] = maxTorque[1];
00560 sensorInfo.specValues[5] = maxTorque[2];
00561
00562 } else if(sensorType == "RateGyro") {
00563 sensorInfo.specValues.length( CORBA::ULong(3) );
00564 DblArray3 maxAngularVelocity;
00565 copyVrmlField(fmap, "maxAngularVelocity", maxAngularVelocity);
00566 sensorInfo.specValues[0] = maxAngularVelocity[0];
00567 sensorInfo.specValues[1] = maxAngularVelocity[1];
00568 sensorInfo.specValues[2] = maxAngularVelocity[2];
00569
00570 } else if( sensorType == "Acceleration" ){
00571 sensorInfo.specValues.length( CORBA::ULong(3) );
00572 DblArray3 maxAcceleration;
00573 copyVrmlField(fmap, "maxAcceleration", maxAcceleration);
00574 sensorInfo.specValues[0] = maxAcceleration[0];
00575 sensorInfo.specValues[1] = maxAcceleration[1];
00576 sensorInfo.specValues[2] = maxAcceleration[2];
00577
00578 } else if( sensorType == "Vision" ){
00579 sensorInfo.specValues.length( CORBA::ULong(7) );
00580
00581 CORBA::Double specValues[3];
00582 copyVrmlField(fmap, "frontClipDistance", specValues[0] );
00583 copyVrmlField(fmap, "backClipDistance", specValues[1] );
00584 copyVrmlField(fmap, "fieldOfView", specValues[2] );
00585 sensorInfo.specValues[0] = specValues[0];
00586 sensorInfo.specValues[1] = specValues[1];
00587 sensorInfo.specValues[2] = specValues[2];
00588
00589 std::string sensorTypeString;
00590 copyVrmlField(fmap, "type", sensorTypeString );
00591
00592 if(sensorTypeString=="NONE" ) {
00593 sensorInfo.specValues[3] = Camera::NONE;
00594 } else if(sensorTypeString=="COLOR") {
00595 sensorInfo.specValues[3] = Camera::COLOR;
00596 } else if(sensorTypeString=="MONO") {
00597 sensorInfo.specValues[3] = Camera::MONO;
00598 } else if(sensorTypeString=="DEPTH") {
00599 sensorInfo.specValues[3] = Camera::DEPTH;
00600 } else if(sensorTypeString=="COLOR_DEPTH") {
00601 sensorInfo.specValues[3] = Camera::COLOR_DEPTH;
00602 } else if(sensorTypeString=="MONO_DEPTH") {
00603 sensorInfo.specValues[3] = Camera::MONO_DEPTH;
00604 } else {
00605 throw ModelLoader::ModelLoaderException("Sensor node has unkown type string");
00606 }
00607
00608 CORBA::Long width, height;
00609 copyVrmlField(fmap, "width", width);
00610 copyVrmlField(fmap, "height", height);
00611
00612 sensorInfo.specValues[4] = static_cast<CORBA::Double>(width);
00613 sensorInfo.specValues[5] = static_cast<CORBA::Double>(height);
00614
00615 double frameRate;
00616 copyVrmlField(fmap, "frameRate", frameRate);
00617 sensorInfo.specValues[6] = frameRate;
00618 } else if( sensorType == "Range" ){
00619 sensorInfo.specValues.length( CORBA::ULong(4) );
00620 CORBA::Double v;
00621 copyVrmlField(fmap, "scanAngle", v);
00622 sensorInfo.specValues[0] = v;
00623 copyVrmlField(fmap, "scanStep", v);
00624 sensorInfo.specValues[1] = v;
00625 copyVrmlField(fmap, "scanRate", v);
00626 sensorInfo.specValues[2] = v;
00627 copyVrmlField(fmap, "maxDistance", v);
00628 sensorInfo.specValues[3] = v;
00629 }
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640 Matrix44 E(Matrix44::Identity());
00641 VrmlVariantField *field = sensorNode->getField("children");
00642 if (field){
00643 MFNode &children = field->mfNode();
00644 for (unsigned int i=0; i<children.size(); i++){
00645 traverseShapeNodes(children[i].get(), E,
00646 sensorInfo.shapeIndices, sensorInfo.inlinedShapeTransformMatrices, &topUrl());
00647 }
00648 }
00649 } catch(ModelLoader::ModelLoaderException& ex) {
00650 string error = name_.empty() ? "Unnamed sensor node" : name_;
00651 error += ": ";
00652 error += ex.description;
00653 throw ModelLoader::ModelLoaderException( error.c_str() );
00654 }
00655 }
00656
00657 void BodyInfo_impl::readHwcNode(int linkInfoIndex, HwcInfo& hwcInfo, VrmlProtoInstancePtr hwcNode )
00658 {
00659 try {
00660 hwcInfo.name = CORBA::string_dup( hwcNode->defName.c_str() );
00661
00662 TProtoFieldMap& fmap = hwcNode->fields;
00663
00664 copyVrmlField(fmap, "id", hwcInfo.id );
00665 copyVrmlField(fmap, "translation", hwcInfo.translation );
00666 copyVrmlRotationFieldToDblArray4( fmap, "rotation", hwcInfo.rotation );
00667 std::string url;
00668 copyVrmlField( fmap, "url", url );
00669 hwcInfo.url = CORBA::string_dup( url.c_str() );
00670
00671 Matrix44 E(Matrix44::Identity());
00672 VrmlVariantField *field = hwcNode->getField("children");
00673 if (field){
00674 MFNode &children = field->mfNode();
00675 for (unsigned int i=0; i<children.size(); i++){
00676 traverseShapeNodes(children[i].get(), E,
00677 hwcInfo.shapeIndices, hwcInfo.inlinedShapeTransformMatrices, &topUrl());
00678 }
00679 }
00680 } catch(ModelLoader::ModelLoaderException& ex) {
00681 throw ModelLoader::ModelLoaderException( ex.description );
00682 }
00683 }
00684
00685 void BodyInfo_impl::readLightNode(int linkInfoIndex, LightInfo& lightInfo,
00686 std::pair<Matrix44, VrmlNodePtr> &transformedLight )
00687 {
00688 VrmlNode *lightNode = transformedLight.second.get();
00689 Matrix44 &T = transformedLight.first;
00690 for (int i=0; i<3; i++){
00691 for (int j=0; j<4; j++){
00692 lightInfo.transformMatrix[i*4+j] = T(i,j);
00693 }
00694 }
00695 try {
00696 lightInfo.name = CORBA::string_dup( lightNode->defName.c_str() );
00697 VrmlPointLight *plight = dynamic_cast<VrmlPointLight *>(lightNode);
00698 VrmlDirectionalLight *dlight = dynamic_cast<VrmlDirectionalLight *>(lightNode);
00699 VrmlSpotLight *slight = dynamic_cast<VrmlSpotLight *>(lightNode);
00700 if (plight){
00701 lightInfo.type = OpenHRP::POINT;
00702 lightInfo.ambientIntensity = plight->ambientIntensity;
00703 lightInfo.intensity = plight->intensity;
00704 lightInfo.on = plight->on;
00705 lightInfo.radius = plight->radius;
00706 for (int i=0; i<3; i++){
00707 lightInfo.attenuation[i] = plight->attenuation[i];
00708 lightInfo.color[i] = plight->color[i];
00709 lightInfo.location[i] = plight->location[i];
00710 }
00711 }else if(dlight){
00712 lightInfo.type = OpenHRP::DIRECTIONAL;
00713 lightInfo.ambientIntensity = dlight->ambientIntensity;
00714 lightInfo.intensity = dlight->intensity;
00715 lightInfo.on = dlight->on;
00716 for (int i=0; i<3; i++){
00717 lightInfo.color[i] = dlight->color[i];
00718 lightInfo.direction[i] = dlight->direction[i];
00719 }
00720 }else if(slight){
00721 lightInfo.type = OpenHRP::SPOT;
00722 lightInfo.ambientIntensity = slight->ambientIntensity;
00723 lightInfo.intensity = slight->intensity;
00724 lightInfo.on = slight->on;
00725 lightInfo.radius = slight->radius;
00726 lightInfo.beamWidth = slight->beamWidth;
00727 lightInfo.cutOffAngle = slight->cutOffAngle;
00728 for (int i=0; i<3; i++){
00729 lightInfo.attenuation[i] = slight->attenuation[i];
00730 lightInfo.color[i] = slight->color[i];
00731 lightInfo.location[i] = slight->location[i];
00732 lightInfo.direction[i] = slight->direction[i];
00733 }
00734 }else{
00735 throw ModelLoader::ModelLoaderException("unknown light type");
00736 }
00737 } catch(ModelLoader::ModelLoaderException& ex) {
00738 throw ModelLoader::ModelLoaderException( ex.description );
00739 }
00740 }
00741
00742 void BodyInfo_impl::setParam(std::string param, bool value){
00743 if(param == "readImage")
00744 readImage = value;
00745 else
00746 ;
00747 }
00748
00749 bool BodyInfo_impl::getParam(std::string param){
00750
00751
00752
00753
00754 if(param == "readImage")
00755 return readImage;
00756 else
00757 abort ();
00758 }
00759
00760 void BodyInfo_impl::setParam(std::string param, int value){
00761 if(param == "AABBType")
00762 AABBdataType_ = (OpenHRP::ModelLoader::AABBdataType)value;
00763 else
00764 ;
00765 }
00766
00767 void BodyInfo_impl::changetoBoundingBox(unsigned int* inputData){
00768 const double EPS = 1.0e-6;
00769 createAppearanceInfo();
00770 std::vector<Vector3> boxSizeMap;
00771 std::vector<Vector3> boundingBoxData;
00772
00773 for(unsigned int i=0; i<links_.length(); i++){
00774 int _depth;
00775 if( AABBdataType_ == OpenHRP::ModelLoader::AABB_NUM )
00776 _depth = linkColdetModels[i]->numofBBtoDepth(inputData[i]);
00777 else
00778 _depth = inputData[i];
00779 if( _depth >= links_[i].AABBmaxDepth)
00780 _depth = links_[i].AABBmaxDepth-1;
00781 if(_depth >= 0 ){
00782 linkColdetModels[i]->getBoundingBoxData(_depth, boundingBoxData);
00783 std::vector<TransformedShapeIndex> tsiMap;
00784 links_[i].shapeIndices.length(0);
00785 SensorInfoSequence& sensors = links_[i].sensors;
00786 for (unsigned int j=0; j<sensors.length(); j++){
00787 SensorInfo& sensor = sensors[j];
00788 sensor.shapeIndices.length(0);
00789 }
00790
00791 for(unsigned int j=0; j<boundingBoxData.size()/2; j++){
00792
00793 bool flg=false;
00794 unsigned int k=0;
00795 for( ; k<boxSizeMap.size(); k++)
00796 if((boxSizeMap[k] - boundingBoxData[j*2+1]).norm() < EPS)
00797 break;
00798 if( k<boxSizeMap.size() )
00799 flg=true;
00800 else{
00801 boxSizeMap.push_back(boundingBoxData[j*2+1]);
00802 setBoundingBoxData(boundingBoxData[j*2+1],k);
00803 }
00804
00805 if(flg){
00806 unsigned int l=0;
00807 for( ; l<tsiMap.size(); l++){
00808 Vector3 p(tsiMap[l].transformMatrix[3],tsiMap[l].transformMatrix[7],tsiMap[l].transformMatrix[11]);
00809 if((p - boundingBoxData[j*2]).norm() < EPS && tsiMap[l].shapeIndex == (int)k)
00810 break;
00811 }
00812 if( l==tsiMap.size() )
00813 flg=false;
00814 }
00815
00816 if(!flg){
00817 int num = links_[i].shapeIndices.length();
00818 links_[i].shapeIndices.length(num+1);
00819 TransformedShapeIndex& tsi = links_[i].shapeIndices[num];
00820 tsi.inlinedShapeTransformMatrixIndex = -1;
00821 tsi.shapeIndex = k;
00822 Matrix44 T(Matrix44::Identity());
00823 for(int p = 0,row=0; row < 3; ++row)
00824 for(int col=0; col < 4; ++col)
00825 if(col==3){
00826 switch(row){
00827 case 0:
00828 tsi.transformMatrix[p++] = boundingBoxData[j*2][0];
00829 break;
00830 case 1:
00831 tsi.transformMatrix[p++] = boundingBoxData[j*2][1];
00832 break;
00833 case 2:
00834 tsi.transformMatrix[p++] = boundingBoxData[j*2][2];
00835 break;
00836 default:
00837 ;
00838 }
00839 }else
00840 tsi.transformMatrix[p++] = T(row, col);
00841
00842 tsiMap.push_back(tsi);
00843 }
00844 }
00845 }
00846 linkShapeIndices_[i] = links_[i].shapeIndices;
00847 }
00848 }
00849
00850 void BodyInfo_impl::changetoOriginData(){
00851 linkShapeIndices_ = originlinkShapeIndices_;
00852 for(size_t i = 0 ; i < links_.length() ; ++i) {
00853 links_[i].shapeIndices = linkShapeIndices_[i];
00854 }
00855 restoreOriginalData();
00856 }