00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00016 #include <hrpCorba/OpenHRPCommon.hh>
00017 #include <stack>
00018 #include "ModelLoaderUtil.h"
00019 #include "Sensor.h"
00020
00021 #include "World.h"
00022 #include "psim.h"
00023
00024 using namespace OpenHRP;
00025 using namespace std;
00026
00027
00028
00029
00030
00031 static void array_to_mat33(const DblArray4& a, fMat33& mat)
00032 {
00033 const double sth = sin(a[3]);
00034 const double vth = 1.0 - cos(a[3]);
00035
00036 double ax = a[0];
00037 double ay = a[1];
00038 double az = a[2];
00039
00040 const double axx = ax*ax*vth;
00041 const double ayy = ay*ay*vth;
00042 const double azz = az*az*vth;
00043 const double axy = ax*ay*vth;
00044 const double ayz = ay*az*vth;
00045 const double azx = az*ax*vth;
00046
00047 ax *= sth;
00048 ay *= sth;
00049 az *= sth;
00050
00051 mat(0,0) = 1.0 - azz - ayy;
00052 mat(0,1) = -az + axy;
00053 mat(0,2) = ay + azx;
00054 mat(1,0) = az + axy;
00055 mat(1,1) = 1.0 - azz - axx;
00056 mat(1,2) = -ax + ayz;
00057 mat(2,0) = -ay + azx;
00058 mat(2,1) = ax + ayz;
00059 mat(2,2) = 1.0 - ayy - axx;
00060 }
00061
00062 static void array_to_mat33(const DblArray9& a, fMat33& mat)
00063 {
00064 mat(0,0) = a[0];
00065 mat(1,0) = a[1];
00066 mat(2,0) = a[2];
00067 mat(0,1) = a[3];
00068 mat(1,1) = a[4];
00069 mat(2,1) = a[5];
00070 mat(0,2) = a[6];
00071 mat(1,2) = a[7];
00072 mat(2,2) = a[8];
00073 }
00074
00075 static void array_to_vec3(const DblArray3& a, fVec3& vec)
00076 {
00077 vec(0) = a[0];
00078 vec(1) = a[1];
00079 vec(2) = a[2];
00080 }
00081
00082 static void createSensors(::World* world, Joint* jnt, SensorInfoSequence iSensors)
00083 {
00084 int numSensors = iSensors.length();
00085
00086 for(int i=0; i < numSensors; ++i)
00087 {
00088 SensorInfo iSensor = iSensors[i];
00089
00090
00091 int id = iSensor.id;
00092
00093 if(id < 0){
00094 std::cerr << "Warning: sensor ID is not given to sensor " << iSensor.name
00095 << "of character " << jnt->CharName() << "." << std::endl;
00096 } else {
00097
00098 int sensorType = Sensor::COMMON;
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110 CORBA::String_var type0 = iSensor.type;
00111 string type(type0);
00112
00113 if( type == "Force" ) { sensorType = Sensor::FORCE; }
00114 else if( type == "RateGyro" ) { sensorType = Sensor::RATE_GYRO; }
00115 else if( type == "Acceleration" ) { sensorType = Sensor::ACCELERATION; }
00116 else if( type == "Vision" ) { sensorType = Sensor::VISION; }
00117
00118
00119 CORBA::String_var name0 = iSensor.name;
00120 string name(name0);
00121 const DblArray3& p = iSensor.translation;
00122 static fVec3 localPos;
00123 static fMat33 localR;
00124 array_to_vec3(p, localPos);
00125 const DblArray4& rot = iSensor.rotation;
00126 array_to_mat33(rot, localR);
00127 world->addSensor(jnt, sensorType, id, name, localPos, localR);
00128 }
00129 }
00130 }
00131
00132 static inline double getLimitValue(DblSequence_var limitseq, double defaultValue)
00133 {
00134 return (limitseq->length() == 0) ? defaultValue : limitseq[0];
00135 }
00136
00137
00138 static Joint* createLink(::World* world, const char* charname, int index, LinkInfoSequence_var iLinks, Joint* pjoint)
00139 {
00140 Chain* _chain = (Chain*)world->Chain();
00141 LinkInfo iLink = iLinks[index];
00142
00143
00144 CORBA::String_var name = iLink.name;
00145 std::string myname;
00146 char sep[2];
00147 sep[0] = charname_separator;
00148 sep[1] = '\0';
00149 myname = std::string(name) + std::string(sep) + std::string(charname);
00150 Joint* jnt = new Joint(myname.c_str());
00151
00152 _chain->AddJoint(jnt, pjoint);
00153
00154 int jointId = iLink.jointId;
00155 jnt->i_joint = jointId;
00156
00157 CORBA::String_var jointType = iLink.jointType;
00158 const std::string jt(jointType);
00159
00160 if(jt == "fixed")
00161 {
00162 jnt->j_type = ::JFIXED;
00163 }
00164 else if(jt == "free")
00165 {
00166 jnt->j_type = ::JFREE;
00167 }
00168 else if(jt == "rotate")
00169 {
00170 jnt->j_type = ::JROTATE;
00171 }
00172 else if(jt == "slide")
00173 {
00174 jnt->j_type = ::JSLIDE;
00175 }
00176 else
00177 {
00178 jnt->j_type = ::JFREE;
00179 }
00180
00181 if(jointId < 0)
00182 {
00183 if(jnt->j_type == ::JROTATE || jnt->j_type == ::JSLIDE)
00184 {
00185 std::cerr << "Warning: Joint ID is not given to joint " << jnt->name
00186 << " of character " << charname << "." << std::endl;
00187 }
00188 }
00189
00190 const DblArray3& t =iLink.translation;
00191 static fVec3 rel_pos;
00192 array_to_vec3(t, rel_pos);
00193
00194 const DblArray4& r = iLink.rotation;
00195 static fMat33 rel_att;
00196 array_to_mat33(r, rel_att);
00197
00198
00199
00200 if(jnt->j_type == ::JROTATE || jnt->j_type == ::JSLIDE)
00201 {
00202 const DblArray3& a = iLink.jointAxis;
00203 static fVec3 loc_axis;
00204 array_to_vec3(a, loc_axis);
00205
00206
00207
00208 #if 0
00209 static fMat33 init_att;
00210 static fVec3 p_axis;
00211 p_axis.mul(rel_att, loc_axis);
00212 static fVec3 x, y;
00213 x.set(1.0, 0.0, 0.0);
00214 y.set(0.0, 1.0, 0.0);
00215 double zx = p_axis*x;
00216 x -= zx * p_axis;
00217 double xlen = x.length();
00218 if(xlen > 1e-8)
00219 {
00220 x /= xlen;
00221 y.cross(p_axis, x);
00222 }
00223 else
00224 {
00225 double yz = y*p_axis;
00226 y -= yz * p_axis;
00227 double ylen = y.length();
00228 y /= ylen;
00229 x.cross(y, p_axis);
00230 }
00231 init_att(0,0) = x(0); init_att(1,0) = x(1); init_att(2,0) = x(2);
00232 init_att(0,1) = y(0); init_att(1,1) = y(1); init_att(2,1) = y(2);
00233 init_att(0,2) = p_axis(0); init_att(1,2) = p_axis(1); init_att(2,2) = p_axis(2);
00234 if(jnt->j_type == JROTATE)
00235 jnt->SetRotateJointType(rel_pos, init_att, AXIS_Z);
00236 else if(jnt->j_type == JSLIDE)
00237 jnt->SetSlideJointType(rel_pos, init_att, AXIS_Z);
00238
00239 #else
00240 AxisIndex axis = AXIS_NULL;
00241 if(loc_axis(0) > 0.95) axis = AXIS_X;
00242 else if(loc_axis(1) > 0.95) axis = AXIS_Y;
00243 else if(loc_axis(2) > 0.95) axis = AXIS_Z;
00244 else{
00245 std::cerr << "unsupported joint axis for " << myname
00246 << ", only X, Y and Z axes are supported" << std::endl;
00247 }
00248 assert(axis != AXIS_NULL);
00249 if(jnt->j_type == JROTATE)
00250 jnt->SetRotateJointType(rel_pos, rel_att, axis);
00251 else if(jnt->j_type == JSLIDE)
00252 jnt->SetSlideJointType(rel_pos, rel_att, axis);
00253 #endif
00254
00255 }
00256 else if(jnt->j_type == ::JSPHERE)
00257 {
00258 jnt->SetSphereJointType(rel_pos, rel_att);
00259 }
00260 else if(jnt->j_type == ::JFIXED)
00261 {
00262 jnt->SetFixedJointType(rel_pos, rel_att);
00263 }
00264 else if(jnt->j_type == ::JFREE)
00265 {
00266
00267
00268 jnt->SetFreeJointType(rel_pos, rel_att);
00269 }
00270
00271 jnt->mass = iLink.mass;
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302 const DblArray3& rc = iLink.centerOfMass;
00303 static fVec3 com;
00304 array_to_vec3(rc, com);
00305 jnt->loc_com.set(com);
00306
00307 const DblArray9& I = iLink.inertia;
00308 static fMat33 inertia;
00309 array_to_mat33(I, inertia);
00310 jnt->inertia.set(inertia);
00311
00312
00313
00314
00315 for( unsigned int i = 0 ; i < iLinks[index].childIndices.length() ; i++ )
00316 {
00317 if( 0 <= iLinks[index].childIndices[i] )
00318 {
00319 createLink(world, charname, iLinks[index].childIndices[i], iLinks, jnt);
00320 }
00321 }
00322
00323 createSensors(world, jnt, iLink.sensors);
00324
00325 return jnt;
00326 }
00327
00328
00329 int loadBodyFromBodyInfo(::World* world, const char* _name, BodyInfo_ptr bodyInfo)
00330 {
00331
00332 pSim* _chain = world->Chain();
00333 _chain->BeginCreateChain(true);
00334
00335 if(!_chain->Root())
00336 {
00337 _chain->AddRoot("space");
00338 }
00339
00340 CORBA::String_var name = _name;
00341
00342 int n = bodyInfo->links()->length();
00343 LinkInfoSequence_var iLinks = bodyInfo->links();
00344 int failed = true;
00345
00346 for(int i=0; i < n; ++i)
00347 {
00348 if(iLinks[i].parentIndex < 0)
00349 {
00350 static fMat33 Rs;
00351 Rs.identity();
00352 Joint* r = createLink(world, name, i, iLinks, _chain->Root());
00353 world->addCharacter(r, _name, bodyInfo->links());
00354 failed = false;
00355 break;
00356 }
00357 }
00358 #if 0
00359 int rootIndex = -1;
00360 if(body){
00361 matrix33 Rs(tvmet::identity<matrix33>());
00362 Link* rootLink = createLink(body, rootIndex, iLinks, Rs, importLinkShape);
00363 body->setRootLink(rootLink);
00364
00365 DblArray3_var p = iLinks[rootIndex]->translation();
00366 vector3 pos(p[0u], p[1u], p[2u]);
00367 const DblArray4 &R = iLinks[rootIndex]->rotation();
00368 matrix33 att;
00369 getMatrix33FromRowMajorArray(att, R);
00370 body->setDefaultRootPosition(pos, att);
00371
00372 body->installCustomizer();
00373
00374 body->initializeConfiguration();
00375 }
00376 #endif
00377 _chain->EndCreateChain();
00378
00379
00380 return failed;
00381 }
00382
00383
00384 int loadBodyFromModelLoader(::World* world, const char* name, const char *url, CosNaming::NamingContext_var cxt)
00385 {
00386 CosNaming::Name ncName;
00387 ncName.length(1);
00388 ncName[0].id = CORBA::string_dup("ModelLoader");
00389 ncName[0].kind = CORBA::string_dup("");
00390 ModelLoader_var modelLoader = NULL;
00391 try {
00392 modelLoader = ModelLoader::_narrow(cxt->resolve(ncName));
00393 } catch(const CosNaming::NamingContext::NotFound &exc) {
00394 std::cerr << "ModelLoader not found: ";
00395 switch(exc.why) {
00396 case CosNaming::NamingContext::missing_node:
00397 std::cerr << "Missing Node" << std::endl;
00398 case CosNaming::NamingContext::not_context:
00399 std::cerr << "Not Context" << std::endl;
00400 break;
00401 case CosNaming::NamingContext::not_object:
00402 std::cerr << "Not Object" << std::endl;
00403 break;
00404 }
00405 return 0;
00406 } catch(CosNaming::NamingContext::CannotProceed &exc) {
00407 std::cerr << "Resolve ModelLoader CannotProceed" << std::endl;
00408 } catch(CosNaming::NamingContext::AlreadyBound &exc) {
00409 std::cerr << "Resolve ModelLoader InvalidName" << std::endl;
00410 }
00411
00412 BodyInfo_var bodyInfo;
00413 try {
00414 bodyInfo = modelLoader->getBodyInfo(url);
00415 } catch(CORBA::SystemException& ex) {
00416 std::cerr << "CORBA::SystemException raised by ModelLoader: " << ex._rep_id() << std::endl;
00417 return 0;
00418 } catch(ModelLoader::ModelLoaderException& ex){
00419 std::cerr << "ModelLoaderException ( " << ex.description << ") : " << ex.description << std::endl;
00420 }
00421
00422 if(CORBA::is_nil(bodyInfo)){
00423 return 0;
00424 }
00425
00426 return loadBodyFromBodyInfo(world, name, bodyInfo);
00427 }
00428
00429
00430 int loadBodyFromModelLoader(::World* world, const char* name, const char *url, CORBA_ORB_var orb)
00431 {
00432 CosNaming::NamingContext_var cxt;
00433 try {
00434 CORBA::Object_var nS = orb->resolve_initial_references("NameService");
00435 cxt = CosNaming::NamingContext::_narrow(nS);
00436 } catch(CORBA::SystemException& ex) {
00437 std::cerr << "NameService doesn't exist" << std::endl;
00438 return 0;
00439 }
00440
00441 return loadBodyFromModelLoader(world, name, url, cxt);
00442 }
00443
00444
00445 int loadBodyFromModelLoader(::World* world, const char* name, const char *url, int argc, char *argv[])
00446 {
00447 CORBA::ORB_var orb = CORBA::ORB_init(argc, argv);
00448 return loadBodyFromModelLoader(world, name, url, orb);
00449 }
00450
00451
00452 int loadBodyFromModelLoader(::World* world, const char* name, const char *URL, istringstream &strm)
00453 {
00454 vector<string> argvec;
00455 while (!strm.eof()){
00456 string arg;
00457 strm >> arg;
00458 argvec.push_back(arg);
00459 }
00460 int argc = argvec.size();
00461 char **argv = new char *[argc];
00462 for (int i=0; i<argc; i++){
00463 argv[i] = (char *)argvec[i].c_str();
00464 }
00465
00466 int ret = loadBodyFromModelLoader(world, name, URL, argc, argv);
00467
00468 delete [] argv;
00469
00470 return ret;
00471 }