BodyInfo_impl.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
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     // URL文字列の' \' 区切り子を'/' に置き換え  Windows ファイルパス対応  
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     // build coldetModels 
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     //saveOriginalData();
00229     //originlinkShapeIndices_ = linkShapeIndices_;
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         //std::cout << "No climit type. climit was ignored." << std::endl;        
00377         linkInfo.climit.length((CORBA::ULong)0); // dummy
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     //  Mass = Σmass                 //
00404     //  C = (Σmass * T * c) / Mass   //
00405     //  I = Σ(R * I * Rt + G)       //
00406     //  R = Tの回転行列               //
00407     //  G = y*y+z*z, -x*y, -x*z, -y*x, z*z+x*x, -y*z, -z*x, -z*y, x*x+y*y    //
00408     //  (x, y, z ) = T * c - C        //
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         Matrix44 T;
00633         const DblArray4& r = sensorInfo.rotation;
00634         calcRodrigues(T, Vector3(r[0], r[1], r[2]), r[3]);
00635         for(int i=0; i < 3; ++i){
00636             T(i,3) = sensorInfo.translation[i];
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     // FIXME: should this be an assert?
00751     // pros for assert: can be turned off on release builds
00752     // cons for assert: can lead to unpredictable behavior in
00753     // release builds if the assertion is violated
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 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:52