DynamicsSimulator_impl.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
00002 /*
00003  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00004  * All rights reserved. This program is made available under the terms of the
00005  * Eclipse Public License v1.0 which accompanies this distribution, and is
00006  * available at http://www.eclipse.org/legal/epl-v10.html
00007  * Contributors:
00008  * The University of Tokyo
00009  * National Institute of Advanced Industrial Science and Technology (AIST)
00010  * General Robotix Inc. 
00011  */
00016 #include <vector>
00017 #include <map>
00018 
00019 #include "DynamicsSimulator_impl.h"
00020 #include "ModelLoaderUtil.h"
00021 #include "Sensor.h"
00022 #include <psim.h>
00023 
00024 using namespace OpenHRP;
00025 using namespace std;
00026 
00027 
00028 //#define INTEGRATOR_DEBUG
00029 static const bool enableTimeMeasure = false;
00030 
00031 //#include <fstream>
00032 //static std::ofstream logfile("impl.log");
00033 //static std::ofstream logfile;
00034 
00035 namespace {
00036 
00037         struct IdLabel {
00038                 int id;
00039                 const char* label;
00040         };
00041         typedef map<int, const char*> IdToLabelMap;
00042 
00043         IdToLabelMap commandLabelMaps[2];
00044 
00045         IdToLabelMap commandLabelMap;
00046 
00047         IdLabel commandLabels[] = {
00048 
00049                 { DynamicsSimulator::POSITION_GIVEN,    "Position Given (High Gain Mode)" },
00050                 { DynamicsSimulator::JOINT_VALUE,       "Joint Value" },
00051                 { DynamicsSimulator::JOINT_VELOCITY,    "Joint Velocity"},
00052                 { DynamicsSimulator::JOINT_ACCELERATION,"Joint Acceleration"},
00053                 { DynamicsSimulator::JOINT_TORQUE,              "Joint Torque"},
00054                 { DynamicsSimulator::ABS_TRANSFORM,             "Absolute Transform"},
00055                 { DynamicsSimulator::ABS_VELOCITY,              "Absolute Velocity"},
00056                 { DynamicsSimulator::EXTERNAL_FORCE,    "External Force"},
00057                 { -1, "" },
00058         };
00059 
00060         void initializeCommandLabelMaps()
00061         {
00062                 if(commandLabelMap.empty()){
00063                         int i = 0;
00064                         while(true){
00065                                 IdLabel& idLabel = commandLabels[i++];
00066                                 if(idLabel.id == -1){
00067                                         break;
00068                                 }
00069                                 commandLabelMap[idLabel.id] = idLabel.label;
00070                         }
00071                 }
00072         }
00073 
00074         const char* getLabelOfLinkDataType(DynamicsSimulator::LinkDataType type)
00075         {
00076                 IdToLabelMap::iterator p = commandLabelMap.find(type);
00077                 return (p != commandLabelMap.end()) ? p->second : "Requesting Unknown Data Type";
00078         }
00079 };
00080 
00081 
00082 template<typename X, typename X_ptr>
00083 X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
00084 {
00085         CosNaming::Name ncName;
00086         ncName.length(1);
00087         ncName[0].id = CORBA::string_dup(n.c_str());
00088         ncName[0].kind = CORBA::string_dup("");
00089         X_ptr srv = NULL;
00090         try {
00091                 srv = X::_narrow(cxt->resolve(ncName));
00092         } catch(const CosNaming::NamingContext::NotFound &exc) {
00093                 std::cerr << n << " not found: ";
00094                 switch(exc.why) {
00095                 case CosNaming::NamingContext::missing_node:
00096                         std::cerr << "Missing Node" << std::endl;
00097                 case CosNaming::NamingContext::not_context:
00098                         std::cerr << "Not Context" << std::endl;
00099                         break;
00100                 case CosNaming::NamingContext::not_object:
00101                         std::cerr << "Not Object" << std::endl;
00102                         break;
00103                 }
00104                 return (X_ptr)NULL;
00105         } catch(CosNaming::NamingContext::CannotProceed &exc) {
00106                 std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
00107         } catch(CosNaming::NamingContext::AlreadyBound &exc) {
00108                 std::cerr << "Resolve " << n << " InvalidName" << std::endl;
00109         }
00110         return srv;
00111 }
00112 
00113 
00114 DynamicsSimulator_impl::DynamicsSimulator_impl(CORBA::ORB_ptr orb)
00115         : orb_(CORBA::ORB::_duplicate(orb))
00116 {
00117         world.setCurrentTime(0.0);
00118         world.setRungeKuttaMethod();
00119 
00120     // resolve collisionchecker object
00121     CollisionDetectorFactory_var collisionDetectorFactory;
00122 
00123         CORBA::Object_var nS = orb->resolve_initial_references("NameService");
00124     CosNaming::NamingContext_var cxT;
00125         cxT = CosNaming::NamingContext::_narrow(nS);
00126         collisionDetectorFactory =
00127                 checkCorbaServer<CollisionDetectorFactory,
00128                 CollisionDetectorFactory_var>("CollisionDetectorFactory", cxT);
00129         if (CORBA::is_nil(collisionDetectorFactory)) {
00130                 std::cerr << "CollisionDetectorFactory not found" << std::endl;
00131         }
00132 
00133         collisionDetector = collisionDetectorFactory->create();
00134 
00135         collisions = new CollisionSequence;
00136         collidingLinkPairs = new LinkPairSequence;
00137         allCharacterPositions = new CharacterPositionSequence;
00138         allCharacterSensorStates = new SensorStateSequence;
00139 
00140         needToUpdatePositions = true;
00141         needToUpdateSensorStates = true;
00142 }
00143 
00144 
00145 DynamicsSimulator_impl::~DynamicsSimulator_impl()
00146 {
00147 }
00148 
00149 
00150 void DynamicsSimulator_impl::destroy()
00151 {
00152         PortableServer::POA_var poa_ = _default_POA();
00153         PortableServer::ObjectId_var id = poa_->servant_to_id(this);
00154         poa_->deactivate_object(id);
00155 }
00156 
00157 
00158 void DynamicsSimulator_impl::registerCharacter(
00159                 const char *name,
00160                 BodyInfo_ptr body)
00161 {
00162 //      logfile << "registerCharacter(" << name << ", " << body->name() << ")" << endl;
00163         loadBodyFromBodyInfo(&world, name, body);
00164         collisionDetector->registerCharacter(name, body);
00165 //      logfile << "total dof = " << world.Chain()->NumDOF() << endl;
00166 }
00167 
00168 
00169 void DynamicsSimulator_impl::init(
00170                 CORBA::Double _timestep,
00171                 OpenHRP::DynamicsSimulator::IntegrateMethod integrateOpt,
00172                 OpenHRP::DynamicsSimulator::SensorOption sensorOpt)
00173 {
00174 //      logfile << "init" << endl;
00175 
00176         world.setTimeStep(_timestep);
00177     world.setCurrentTime(0.0);
00178 
00179         if(integrateOpt == OpenHRP::DynamicsSimulator::EULER){
00180                 world.setEulerMethod();
00181         } else {
00182                 world.setRungeKuttaMethod();
00183         }
00184 
00185         world.enableSensors((sensorOpt == OpenHRP::DynamicsSimulator::ENABLE_SENSOR));
00186 
00187         _setupCharacterData();
00188 
00189         isFirstSimulationLoop = true;
00190 
00191 #ifdef MEASURE_TIME
00192         processTime = 0;
00193 #endif
00194 }
00195 
00196 
00197 static void joint_traverse_sub(Joint* cur, std::vector<Joint*>& jlist)
00198 {
00199         if(!cur) return;
00200 //      logfile << "joint_traverse_sub: " << cur->name << ", n_dof = " << cur->n_dof << endl;
00201         jlist.push_back(cur);
00202         joint_traverse_sub(cur->brother, jlist);
00203         joint_traverse_sub(cur->child, jlist);
00204 }
00205 
00206 static void joint_traverse(Joint* r, std::vector<Joint*>& jlist)
00207 {
00208         jlist.push_back(r);
00209         joint_traverse_sub(r->child, jlist);
00210 }
00211 
00212 
00213 void DynamicsSimulator_impl::registerCollisionCheckPair(
00214                 const char *charName1,
00215                 const char *linkName1,
00216                 const char *charName2,
00217                 const char *linkName2,
00218                 const CORBA::Double staticFriction,
00219                 const CORBA::Double slipFriction,
00220                 const DblSequence6 & K,
00221                 const DblSequence6 & C,
00222         const double culling_thresh,
00223                 const double restitution)
00224 {
00225         const double epsilon = 0.0;
00226 //      logfile << "registerCollisionCheckPair" << endl;
00227 
00228         std::string emptyString = "";
00229         std::vector<Joint*> joints1;
00230         std::vector<Joint*> joints2;
00231         pSim* chain = world.Chain();
00232 
00233         if(emptyString == linkName1)
00234         {
00235                 Joint* r = chain->FindCharacterRoot(charName1);
00236                 if(r) joint_traverse(r, joints1);
00237         }
00238         else
00239         {
00240                 Joint* jnt1 = chain->FindJoint(linkName1, charName1);
00241                 if(jnt1) joints1.push_back(jnt1);
00242         }
00243         if(emptyString == linkName2)
00244         {
00245                 Joint* r = chain->FindCharacterRoot(charName2);
00246                 if(r) joint_traverse(r, joints2);
00247         }
00248         else
00249         {
00250                 Joint* jnt2 = chain->FindJoint(linkName2, charName2);
00251                 if(jnt2) joints2.push_back(jnt2);
00252         }
00253 
00254         for(size_t i=0; i < joints1.size(); ++i)
00255         {
00256                 Joint* j1 = joints1[i];
00257                 for(size_t j=0; j < joints2.size(); ++j)
00258                 {
00259                         Joint* j2 = joints2[j];
00260                         if(j1 && j2 && j1 != j2)
00261                         {
00262 //                              logfile << "pair = " << j1->name << ", " << j2->name << endl;
00263                                 world.addCollisionCheckLinkPair(j1, j2, staticFriction, slipFriction, epsilon);
00264                                 LinkPair_var linkPair = new LinkPair();
00265                                 linkPair->charName1  = CORBA::string_dup(charName1);
00266                                 linkPair->linkName1 = CORBA::string_dup(j1->basename);
00267                                 linkPair->charName2  = CORBA::string_dup(charName2);
00268                                 linkPair->linkName2 = CORBA::string_dup(j2->basename);
00269                                 linkPair->tolerance = 0;
00270                                 collisionDetector->addCollisionPair(linkPair);
00271                         }
00272                 }
00273         }
00274 }
00275 
00276 void DynamicsSimulator_impl::registerIntersectionCheckPair(
00277                 const char *charName1,
00278                 const char *linkName1,
00279                 const char *charName2,
00280                 const char *linkName2,
00281                 const double tolerance)
00282 {
00283 //      logfile << "registerCollisionCheckPair" << endl;
00284 
00285         std::string emptyString = "";
00286         std::vector<Joint*> joints1;
00287         std::vector<Joint*> joints2;
00288         pSim* chain = world.Chain();
00289 
00290         if(emptyString == linkName1)
00291         {
00292                 Joint* r = chain->FindCharacterRoot(charName1);
00293                 if(r) joint_traverse(r, joints1);
00294         }
00295         else
00296         {
00297                 Joint* jnt1 = chain->FindJoint(linkName1, charName1);
00298                 if(jnt1) joints1.push_back(jnt1);
00299         }
00300         if(emptyString == linkName2)
00301         {
00302                 Joint* r = chain->FindCharacterRoot(charName2);
00303                 if(r) joint_traverse(r, joints2);
00304         }
00305         else
00306         {
00307                 Joint* jnt2 = chain->FindJoint(linkName2, charName2);
00308                 if(jnt2) joints2.push_back(jnt2);
00309         }
00310 
00311         for(size_t i=0; i < joints1.size(); ++i)
00312         {
00313                 Joint* j1 = joints1[i];
00314                 for(size_t j=0; j < joints2.size(); ++j)
00315                 {
00316                         Joint* j2 = joints2[j];
00317                         if(j1 && j2 && j1 != j2)
00318                         {
00319                                 LinkPair_var linkPair = new LinkPair();
00320                                 linkPair->charName1  = CORBA::string_dup(charName1);
00321                                 linkPair->linkName1 = CORBA::string_dup(j1->basename);
00322                                 linkPair->charName2  = CORBA::string_dup(charName2);
00323                                 linkPair->linkName2 = CORBA::string_dup(j2->basename);
00324                                 linkPair->tolerance = tolerance;
00325                                 collisionDetector->addCollisionPair(linkPair);
00326                         }
00327                 }
00328         }
00329 }
00330 
00331 void DynamicsSimulator_impl::registerExtraJoint
00332                 (
00333                         const char*     charName1,
00334                         const char*     linkName1,
00335                         const char*     charName2,
00336                         const char*     linkName2,
00337                         const DblSequence3&     link1LocalPos,
00338                         const DblSequence3&     link2LocalPos,
00339                         const ExtraJointType jointType,
00340                         const DblSequence3&     jointAxis,
00341                         const char*                     extraJointName)
00342 {
00343 }
00344 
00346 void DynamicsSimulator_impl::getExtraJointConstraintForce(
00347                 const char * characterName,
00348                 const char * extraJointName,
00349                 DblSequence6_out contactForce)
00350 {
00351 }
00352 
00353 
00354 static void vec3_to_seq(const fVec3& vec, DblSequence_out& seq, size_t offset = 0)
00355 {
00356         seq[CORBA::ULong(offset++)] = vec(0);
00357         seq[CORBA::ULong(offset++)] = vec(1);
00358         seq[CORBA::ULong(offset)] = vec(2);
00359 }
00360 
00361 static void seq_to_vec3(const DblSequence3& seq, fVec3& vec)
00362 {
00363         vec(0) = seq[0];
00364         vec(1) = seq[1];
00365         vec(2) = seq[2];
00366 }
00367 
00368 void DynamicsSimulator_impl::getCharacterSensorValues(
00369                 const char *characterName,
00370                 const char *sensorName,
00371                 DblSequence_out sensorOutput)
00372 {
00373         sensorOutput = new DblSequence;
00374         Sensor* sensor = world.findSensor(sensorName, characterName);
00375 
00376         if(sensor)
00377         {
00378                 switch(sensor->type)
00379                 {
00380                 case Sensor::FORCE:
00381                         {
00382                                 ForceSensor* forceSensor = static_cast<ForceSensor*>(sensor);
00383                 sensorOutput->length(6);
00384 #ifndef __WIN32__
00385                 vec3_to_seq(forceSensor->f, sensorOutput);
00386                                 vec3_to_seq(forceSensor->tau, sensorOutput, 3);
00387 #else
00388                                 sensorOutput[CORBA::ULong(0)] = forceSensor->f(0);
00389                                 sensorOutput[CORBA::ULong(1)] = forceSensor->f(1);
00390                                 sensorOutput[CORBA::ULong(2)] = forceSensor->f(2);
00391                                 sensorOutput[CORBA::ULong(3)] = forceSensor->tau(0);
00392                                 sensorOutput[CORBA::ULong(4)] = forceSensor->tau(1);
00393                                 sensorOutput[CORBA::ULong(5)] = forceSensor->tau(2);
00394 #endif
00395                         }
00396                         break;
00397 
00398                 case Sensor::RATE_GYRO:
00399                         {
00400                                 RateGyroSensor* gyro = static_cast<RateGyroSensor*>(sensor);
00401                 sensorOutput->length(3);
00402 #ifndef __WIN32__
00403                 vec3_to_seq(gyro->w, sensorOutput);
00404 #else   
00405                                 sensorOutput[CORBA::ULong(0)] = gyro->w(0);
00406                                 sensorOutput[CORBA::ULong(1)] = gyro->w(1);
00407                                 sensorOutput[CORBA::ULong(2)] = gyro->w(2);
00408 #endif
00409                         }
00410                         break;
00411 
00412                 case Sensor::ACCELERATION:
00413                         {
00414                                 AccelSensor* accelSensor = static_cast<AccelSensor*>(sensor);
00415                 sensorOutput->length(3);
00416 #ifndef __WIN32__
00417                 vec3_to_seq(accelSensor->dv, sensorOutput);
00418 #else
00419                                 sensorOutput[CORBA::ULong(0)] = accelSensor->dv(0);
00420                                 sensorOutput[CORBA::ULong(1)] = accelSensor->dv(1);
00421                                 sensorOutput[CORBA::ULong(2)] = accelSensor->dv(2);
00422 #endif
00423                         }
00424                         break;
00425 
00426                 default:
00427                         break;
00428                 }
00429         }
00430 }
00431 
00432 void DynamicsSimulator_impl::initSimulation()
00433 {
00434         world.initialize();
00435 
00436         _updateCharacterPositions();
00437         collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
00438 
00439         if(enableTimeMeasure){
00440                 timeMeasureFinished = false;
00441                 timeMeasure1.begin();
00442         }
00443 }
00444 
00445 void DynamicsSimulator_impl::stepSimulation()
00446 {
00447         if(enableTimeMeasure) timeMeasure2.begin();
00448         world.calcNextState(collisions);
00449         if(enableTimeMeasure) timeMeasure2.end();
00450 
00451         if(enableTimeMeasure){
00452                 cout << " " << timeMeasure2.time() << "\n";
00453         }
00454 
00455         needToUpdateSensorStates = true;
00456 
00457         _updateCharacterPositions();
00458 
00459         collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
00460 
00461         if(enableTimeMeasure)
00462         {
00463                 if(world.currentTime() > 10.0 && !timeMeasureFinished)
00464                 {
00465                         timeMeasureFinished = true;
00466                         timeMeasure1.end();
00467                         cout << "Total elapsed time = " << timeMeasure1.totalTime() << "\n";
00468                         cout << "Internal elapsed time = " << timeMeasure2.totalTime();
00469                         cout << ", the avarage = " << timeMeasure2.avarageTime() << endl;;
00470                         //cout << "Collision check time = " << timeMeasure3.totalTime() << endl;
00471                 }
00472         }
00473 }
00474 
00475 
00476 void DynamicsSimulator_impl::setCharacterLinkData(
00477                 const char* characterName,
00478                 const char* linkName,
00479                 OpenHRP::DynamicsSimulator::LinkDataType type,
00480                 const DblSequence& wdata)
00481 {
00482 //      logfile << "setCharacterLinkData(" << characterName << ", " << linkName << ")" << endl;
00483         Joint* joint = world.Chain()->FindJoint(linkName, characterName);
00484         if(!joint) return;
00485 
00486         switch(type) {
00487 
00488         case OpenHRP::DynamicsSimulator::POSITION_GIVEN:
00489                 joint->t_given = !(wdata[0] > 0.0);
00490                 break;
00491 
00492         case OpenHRP::DynamicsSimulator::JOINT_VALUE:
00493                 joint->SetJointValue(wdata[0]);
00494                 break;
00495 
00496         case OpenHRP::DynamicsSimulator::JOINT_VELOCITY:
00497                 joint->SetJointVel(wdata[0]);
00498                 break;
00499 
00500         case OpenHRP::DynamicsSimulator::JOINT_ACCELERATION:
00501                 joint->SetJointAcc(wdata[0]);
00502                 break;
00503 
00504         case OpenHRP::DynamicsSimulator::JOINT_TORQUE:
00505                 joint->SetJointForce(wdata[0]);
00506                 break;
00507 
00508         case OpenHRP::DynamicsSimulator::ABS_TRANSFORM:
00509         {
00510                 fVec3 abs_pos(wdata[0], wdata[1], wdata[2]);
00511                 fMat33 abs_att(wdata[3], wdata[4], wdata[5], wdata[6], wdata[7], wdata[8], wdata[9], wdata[10], wdata[11]);  // wdata is in row major order
00512                 joint->SetJointValue(abs_pos, abs_att);
00513                 break;
00514         }
00515         
00516         case OpenHRP::DynamicsSimulator::ABS_VELOCITY:
00517         {
00518                 fVec3 abs_lin_vel(wdata[0], wdata[1], wdata[2]);
00519                 fVec3 abs_ang_vel(wdata[3], wdata[4], wdata[5]);
00520                 joint->rel_lin_vel.mul(abs_lin_vel, joint->abs_att);
00521                 joint->rel_ang_vel.mul(abs_ang_vel, joint->abs_att);
00522                 break;
00523         }
00524 
00525         case OpenHRP::DynamicsSimulator::EXTERNAL_FORCE:
00526         {
00527                 // original: local frame?, around world origin
00528                 // new: local frame, around joint origin
00529                 joint->ext_force(0) = wdata[0];
00530                 joint->ext_force(1) = wdata[1];
00531                 joint->ext_force(2) = wdata[2];
00532                 fVec3 n0(wdata[3], wdata[4], wdata[5]);
00533                 fVec3 np;
00534                 np.cross(joint->abs_pos, joint->ext_force);
00535                 joint->ext_moment.sub(n0, np);
00536                 break;
00537         }
00538 
00539         default:
00540                 return;
00541         }
00542 
00543         needToUpdatePositions = true;
00544         needToUpdateSensorStates = true;
00545 }
00546 
00547 
00548 void DynamicsSimulator_impl::getCharacterLinkData(
00549                 const char * characterName,
00550                 const char * linkName,
00551                 OpenHRP::DynamicsSimulator::LinkDataType type,
00552                 DblSequence_out out_rdata)
00553 {
00554 //      logfile << "getCharacterLinkData" << endl;
00555         Joint* joint = world.Chain()->FindJoint(linkName, characterName);
00556         assert(joint);
00557 
00558         DblSequence_var rdata = new DblSequence;
00559 
00560         switch(type) {
00561 
00562         case OpenHRP::DynamicsSimulator::JOINT_VALUE:
00563                 rdata->length(1);
00564                 rdata[0] = joint->q;
00565                 break;
00566 
00567         case OpenHRP::DynamicsSimulator::JOINT_VELOCITY:
00568                 rdata->length(1);
00569                 rdata[0] = joint->qd;
00570                 break;
00571 
00572         case OpenHRP::DynamicsSimulator::JOINT_ACCELERATION:
00573                 rdata->length(1);
00574                 rdata[0] = joint->qdd;
00575                 break;
00576 
00577         case OpenHRP::DynamicsSimulator::JOINT_TORQUE:
00578                 rdata->length(1);
00579                 rdata[0] = joint->tau;
00580                 break;
00581 
00582         case OpenHRP::DynamicsSimulator::ABS_TRANSFORM:
00583         {
00584                 fEulerPara ep;
00585                 ep.set(joint->abs_att);
00586                 rdata->length(7);
00587                 rdata[0] = joint->abs_pos(0);
00588                 rdata[1] = joint->abs_pos(1);
00589                 rdata[2] = joint->abs_pos(2);
00590                 rdata[3] = ep.Ang();
00591                 rdata[4] = ep.Axis()(0);
00592                 rdata[5] = ep.Axis()(1);
00593                 rdata[6] = ep.Axis()(2);
00594                 break;
00595         }
00596 
00597         case OpenHRP::DynamicsSimulator::ABS_VELOCITY:
00598         {
00599                 fVec3 v0, w0;
00600                 v0.mul(joint->abs_att, joint->loc_lin_vel);
00601                 w0.mul(joint->abs_att, joint->loc_ang_vel);
00602                 rdata->length(6);
00603                 rdata[0] = v0(0);
00604                 rdata[1] = v0(1);
00605                 rdata[2] = v0(2);
00606                 rdata[3] = w0(0);
00607                 rdata[4] = w0(1);
00608                 rdata[5] = w0(2);
00609                 break;
00610         }
00611         
00612         case OpenHRP::DynamicsSimulator::EXTERNAL_FORCE:
00613         {
00614                 // original: local frame, around joint origin
00615                 // new: local frame?, around world origin
00616                 rdata->length(6);
00617                 rdata[0] = joint->ext_force(0);
00618                 rdata[1] = joint->ext_force(1);
00619                 rdata[2] = joint->ext_force(2);
00620                 fVec3 np, n0;
00621                 np.cross(joint->abs_pos, joint->ext_force);
00622                 n0.add(joint->ext_moment, np);
00623                 rdata[3] = n0(0);
00624                 rdata[4] = n0(1);
00625                 rdata[5] = n0(2);
00626                 break;
00627         }
00628         
00629         default:
00630                 break;
00631         }
00632 
00633         out_rdata = rdata._retn();
00634 }
00635 
00636 
00637 void DynamicsSimulator_impl::getCharacterAllLinkData(
00638                 const char * characterName,
00639                 OpenHRP::DynamicsSimulator::LinkDataType type,
00640                 DblSequence_out rdata)
00641 {
00642 //      logfile << "getCharacterAllLinkData" << endl;
00643         rdata = new DblSequence();
00644 
00645         world.getAllCharacterData(characterName, type, rdata);
00646 }
00647 
00648 
00649 void DynamicsSimulator_impl::setCharacterAllLinkData(
00650                 const char * characterName,
00651                 OpenHRP::DynamicsSimulator::LinkDataType type,
00652                 const DblSequence & wdata)
00653 {
00654 //      logfile << "setCharacterAllLinkData: " << getLabelOfLinkDataType(type) << endl;
00655         world.setAllCharacterData(characterName, type, wdata);
00656 }
00657 
00658 
00659 void DynamicsSimulator_impl::setGVector(
00660                 const DblSequence3& wdata)
00661 {
00662         assert(wdata.length() == 3);
00663 
00664 //      logfile << "setGVector" << endl;
00665     fVec3 g;
00666         seq_to_vec3(wdata, g);
00667         world.setGravityAcceleration(g);
00668 
00669 }
00670 
00671 
00672 void DynamicsSimulator_impl::getGVector(
00673                 DblSequence3_out wdata)
00674 {
00675         wdata->length(3);
00676         fVec3 g = world.getGravityAcceleration();
00677         (*wdata)[0] = g(0);
00678         (*wdata)[1] = g(1);
00679         (*wdata)[2] = g(2);
00680 }
00681 
00682 
00683 void DynamicsSimulator_impl::setCharacterAllJointModes(
00684                 const char * characterName,
00685                 OpenHRP::DynamicsSimulator::JointDriveMode jointMode)
00686 {
00687 //      logfile << "setCharacterAllJointModes" << endl;
00688         bool isTorqueMode = (jointMode != OpenHRP::DynamicsSimulator::HIGH_GAIN_MODE);
00689 
00690         world.Chain()->SetCharacterTorqueGiven(characterName, isTorqueMode);
00691 
00692 }
00693 
00694 
00695 CORBA::Boolean DynamicsSimulator_impl::calcCharacterInverseKinematics(
00696                 const char * characterName,
00697                 const char * fromLink, const char * toLink,
00698                 const LinkPosition& target)
00699 {
00700         bool solved = false;
00701 
00702         // TODO
00703 
00704         return solved;
00705 }
00706 
00707 
00708 void DynamicsSimulator_impl::calcCharacterForwardKinematics(
00709                 const char * characterName)
00710 {
00711 //      logfile << "calcCharacterForwardKinematics" << endl;
00712         world.Chain()->CalcPosition();
00713 
00714         needToUpdatePositions = true;
00715         needToUpdateSensorStates = true;
00716 }
00717 
00718 
00719 void DynamicsSimulator_impl::calcWorldForwardKinematics()
00720 {
00721 //      logfile << "calcWorldForwardKinematics" << endl;
00722         world.Chain()->CalcPosition();
00723 
00724         needToUpdatePositions = true;
00725         needToUpdateSensorStates = true;
00726 }
00727 
00728 
00729 void DynamicsSimulator_impl::getWorldState(WorldState_out wstate)
00730 {
00731 //      logfile << "getWorldState" << endl;
00732         if (needToUpdatePositions) _updateCharacterPositions();
00733 
00734         wstate = new WorldState;
00735 
00736         wstate->time = world.currentTime();
00737         wstate->characterPositions = allCharacterPositions;
00738         wstate->collisions = collisions;
00739 }
00740 
00741 
00742 void DynamicsSimulator_impl::getCharacterSensorState(const char* characterName, SensorState_out sstate)
00743 {
00744 //      logfile << "getCharacterSensorState(" << characterName << ")" << endl;
00745         int index = -1;
00746         int n_char = world.numCharacter();
00747         for(int i=0; i<n_char; i++)
00748         {
00749                 Joint* j = world.rootJoint(i);
00750                 if(!strcmp(j->CharName(), characterName))
00751                 {
00752                         index = i;
00753                         break;
00754                 }
00755         }
00756         if(index >= 0)
00757         {
00758                 if(needToUpdateSensorStates)
00759                 {
00760                         _updateSensorStates();
00761                 }
00762                 sstate = new SensorState(allCharacterSensorStates[index]);
00763         }
00764         else
00765         {
00766                 sstate = new SensorState;
00767         }
00768 }
00769 
00770 
00771 void DynamicsSimulator_impl::_setupCharacterData()
00772 {
00773         int nchar = world.numCharacter();
00774 
00775         allCharacterPositions->length(nchar);
00776         allCharacterSensorStates->length(nchar);
00777 
00778 //      logfile << "_setupCharacterData" << endl;
00779 //      logfile << "total DOF = " << chain->NumDOF() << endl;
00780         for(int i=0; i<nchar; i++)
00781         {
00782                 Joint* j = world.rootJoint(i);
00783 //              logfile << "root = " << j->name << endl;
00784                 CharacterPosition& characterPosition = allCharacterPositions[i];
00785                 characterPosition.characterName = CORBA::string_dup(j->CharName());
00786                 int n_links = world.numLinks(i);
00787 //              logfile << "n_links = " << n_links << endl;
00788                 LinkPositionSequence& linkPositions = characterPosition.linkPositions;
00789                 linkPositions.length(n_links);
00790                 SensorState& sensorState = allCharacterSensorStates[i];
00791                 int n_joints = world.numJoints(i);
00792 //              logfile << "n_joints = " << n_joints << endl;
00793                 sensorState.q.length(n_joints);
00794                 sensorState.dq.length(n_joints);
00795                 sensorState.u.length(n_joints);
00796                 sensorState.force.length(world.numSensors(Sensor::FORCE, j->CharName()));
00797                 sensorState.rateGyro.length(world.numSensors(Sensor::RATE_GYRO, j->CharName()));
00798                 sensorState.accel.length(world.numSensors(Sensor::ACCELERATION, j->CharName()));
00799         }
00800 }
00801 
00802 
00803 void DynamicsSimulator_impl::_updateCharacterPositions()
00804 {
00805         world.getAllCharacterPositions(allCharacterPositions.inout());
00806         needToUpdatePositions = false;
00807 
00808 #if 0
00809         int n_char = allCharacterPositions->length();
00810         for(int i=0; i<n_char; i++)
00811         {
00812                 CharacterPosition& cpos = allCharacterPositions[i];
00813                 int n_link = cpos.linkPositions.length();
00814                 for(int j=0; j<n_link; j++)
00815                 {
00816                         LinkPosition& lpos = cpos.linkPositions[j];
00817                 }
00818         }
00819 #endif
00820 }
00821 
00822 void DynamicsSimulator_impl::_updateSensorStates()
00823 {
00824         world.getAllSensorStates(allCharacterSensorStates);
00825         needToUpdateSensorStates = false;
00826 }
00827 
00828 
00833 CORBA::Boolean DynamicsSimulator_impl::getCharacterCollidingPairs(
00834                 const char *characterName,
00835                 LinkPairSequence_out pairs)
00836 {
00837         std::vector<unsigned int> locations;
00838 
00839         for(unsigned int i=0; i < collisions->length(); ++i) {
00840 
00841                 if(  (strcmp(characterName, collisions[i].pair.charName1) == 0)  ||
00842                          (strcmp(characterName, collisions[i].pair.charName2) == 0))
00843                         locations.push_back(i);
00844         }
00845 
00846         pairs->length(locations.size());
00847 
00848         unsigned long n=0;
00849         for(std::vector<unsigned int>::iterator iter = locations.begin();
00850                 iter != locations.end(); ++iter) {
00851 
00852                 strcpy(pairs[n].charName1, collisions[*iter].pair.charName1);
00853                 strcpy(pairs[n].charName2, collisions[*iter].pair.charName2);
00854                 strcpy(pairs[n].linkName1, collisions[*iter].pair.linkName1);
00855                 strcpy(pairs[n].linkName2, collisions[*iter].pair.linkName2);
00856         }
00857 
00858         return true;
00859 }
00860 
00861 
00862 void DynamicsSimulator_impl::calcCharacterJacobian(
00863                 const char *characterName,
00864                 const char *baseLink,
00865                 const char *targetLink,
00866                 DblSequence_out jacobian)
00867 {
00868         fMat J;
00869         world.calcCharacterJacobian(characterName, baseLink, targetLink, J);
00870 
00871         int height = J.row();
00872         int width = J.col();
00873 
00874         jacobian->length(height * width);
00875         int i = 0;
00876         for(int r=0; r < height; ++r){
00877                 for(int c=0; c < width; ++c){
00878                         (*jacobian)[i++] = J(r, c);
00879                 }
00880         }
00881 }
00882 
00883 bool DynamicsSimulator_impl::checkCollision(bool checkAll) 
00884 {
00885         calcWorldForwardKinematics();   
00886     _updateCharacterPositions();
00887         if (checkAll){
00888                 return collisionDetector->queryContactDeterminationForDefinedPairs(allCharacterPositions.in(), collisions.out());
00889         }else{
00890                 return collisionDetector->queryIntersectionForDefinedPairs(checkAll, allCharacterPositions.in(), collidingLinkPairs.out());
00891         }
00892 }
00893 
00894 DistanceSequence *DynamicsSimulator_impl::checkDistance()
00895 {
00896     calcWorldForwardKinematics();
00897     _updateCharacterPositions();
00898         DistanceSequence_var distances = new DistanceSequence;
00899         collisionDetector->queryDistanceForDefinedPairs(allCharacterPositions.in(), distances);
00900         return distances._retn();
00901 }
00902 
00903 LinkPairSequence *DynamicsSimulator_impl::checkIntersection(CORBA::Boolean checkAll)
00904 {
00905     calcWorldForwardKinematics();
00906     _updateCharacterPositions();
00907         LinkPairSequence_var pairs = new LinkPairSequence;
00908         collisionDetector->queryIntersectionForDefinedPairs(checkAll, allCharacterPositions.in(), pairs);
00909         return pairs._retn();
00910 }
00911 
00916 DynamicsSimulatorFactory_impl::DynamicsSimulatorFactory_impl(CORBA::ORB_ptr     orb) :
00917         orb_(CORBA::ORB::_duplicate(orb))
00918 {
00919         initializeCommandLabelMaps();
00920 }
00921 
00922 
00923 DynamicsSimulatorFactory_impl::~DynamicsSimulatorFactory_impl()
00924 {
00925         PortableServer::POA_var poa = _default_POA();
00926         PortableServer::ObjectId_var id = poa -> servant_to_id(this);
00927         poa -> deactivate_object(id);
00928 }
00929 
00930 
00931 DynamicsSimulator_ptr DynamicsSimulatorFactory_impl::create()
00932 {
00933         DynamicsSimulator_impl* integratorImpl = new DynamicsSimulator_impl(orb_);
00934 
00935         PortableServer::ServantBase_var integratorrServant = integratorImpl;
00936         PortableServer::POA_var poa_ = _default_POA();
00937         PortableServer::ObjectId_var id =
00938                 poa_ -> activate_object(integratorImpl);
00939 
00940         return integratorImpl -> _this();
00941 }
00942 
00943 
00944 void DynamicsSimulatorFactory_impl::shutdown()
00945 {
00946         orb_->shutdown(false);
00947 }


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:53