World.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 <string>
00017 
00018 #include "World.h"
00019 #include "psim.h"
00020 #include "Sensor.h"
00021 
00022 static const double DEFAULT_GRAVITY_ACCELERATION = 9.80665;
00023 
00024 static const bool debugMode = false;
00025 
00026 #include <fstream>
00027 //static std::ofstream logfile("world.log");
00028 static std::ofstream logfile;
00029 
00030 #include <sdcontact.h>
00031 
00032 #if 0
00033 bool World::LinkPairKey::operator<(const LinkPairKey& pair2) const
00034 {
00035         if((body1 == pair2.body1 && body2 == pair2.body2) ||
00036            (body2 == pair2.body1 && body1 == pair2.body2 )){
00037                 if(link1 < link2){
00038                         if(pair2.link1 < pair2.link2){
00039                                 return (link1 < pair2.link1) ? true : (link2 < pair2.link2);
00040                         } else {
00041                                 return (link1 < pair2.link2) ? true : (link2 < pair2.link1);
00042                         }
00043                 } else {
00044                         if(pair2.link1 < pair2.link2){
00045                                 return (link2 < pair2.link1) ? true : (link1 < pair2.link2);
00046                         } else {
00047                                 return (link2 < pair2.link2) ? true : (link1 < pair2.link1);
00048                         }
00049                 }
00050         } else {
00051                 if(body1 < body2){
00052                         if(pair2.body1 < pair2.body2){
00053                                 return (body1 < pair2.body1) ? true : (body2 < pair2.body2);
00054                         } else {
00055                                 return (body1 < pair2.body2) ? true : (body2 < pair2.body1);
00056                         }
00057                 } else {
00058                         if(pair2.body1 < pair2.body2){
00059                                 return (body2 < pair2.body1) ? true : (body1 < pair2.body2);
00060                         } else {
00061                                 return (body2 < pair2.body2) ? true : (body1 < pair2.body1);
00062                         }
00063                 }
00064         }
00065 }
00066 #endif
00067 
00068 World::World(): g(0.0, 0.0, DEFAULT_GRAVITY_ACCELERATION)
00069 {
00070     currentTime_ = 0.0;
00071     timeStep_ = 0.005;
00072 
00073     isEulerMethod = false;
00074         sensorsAreEnabled = false;
00075         numRegisteredLinkPairs = 0;
00076 
00077         chain = new pSim;
00078 }
00079 
00080 
00081 World::~World()
00082 {
00083         if(chain) delete chain;
00084         int n_sensors = sensors.size();
00085         for(int i=0; i<n_sensors; i++)
00086         {
00087                 Sensor::destroy(sensors[i]);
00088         }
00089         sensors.clear();
00090         int n_pairs = contact_pairs.size();
00091         for(int i=0; i<n_pairs; i++)
00092         {
00093                 delete contact_pairs[i];
00094         }
00095         contact_pairs.clear();
00096 }
00097 
00098 
00099 void World::setTimeStep(double ts)
00100 {
00101     timeStep_ = ts;
00102 }
00103 
00104 
00105 void World::setCurrentTime(double time)
00106 {
00107     currentTime_ = time;
00108 }
00109 
00110 
00111 void World::setGravityAcceleration(const fVec3& _g)
00112 {
00113     g.set(_g);
00114         if(chain->Root())
00115                 chain->Root()->loc_lin_acc.set(_g);
00116 }
00117 
00118 
00119 void World::enableSensors(bool on)
00120 {
00121         sensorsAreEnabled = on;
00122 }
00123 
00124 
00125 void World::initialize()
00126 {
00127 //      logfile << "initialize" << endl;
00128         chain->Schedule();
00129         chain->CalcPosition();
00130         chain->CalcVelocity();
00131 //      logfile << "initialize end" << endl;
00132 #if 0
00133         if(isEulerMethod){
00134                 pForwardDynamics->setEulerMethod();
00135         } else {
00136                 pForwardDynamics->setRungeKuttaMethod();
00137         }
00138         pForwardDynamics->setGravityAcceleration(g);
00139         pForwardDynamics->setTimeStep(timeStep_);
00140         pForwardDynamics->enableSensors(sensorsAreEnabled);
00141         pForwardDynamics->initialize();
00142 #endif
00143 }
00144 
00145 
00146 void World::calcNextState(OpenHRP::CollisionSequence& corbaCollisionSequence)
00147 {
00148         if(debugMode){
00149                 cout << "World current time = " << currentTime_ << endl;
00150         }
00151 //      logfile << "calcNextState" << endl;
00152         // corbaCollisionSequence->contact_pairs
00153         int n_pair = contact_pairs.size();
00154         for(int i=0; i<n_pair; i++)
00155         {
00156                 contact_pairs[i]->Clear();
00157                 OpenHRP::Collision& col = corbaCollisionSequence[i];
00158                 int n_point = col.points.length();
00159                 if(n_point == 0) continue;
00160                 Joint* joint0 = contact_pairs[i]->GetJoint(0);
00161                 static fVec3 pos0, norm0, pos, norm;
00162                 for(int j=0; j<n_point; j++)
00163                 {
00164                         OpenHRP::CollisionPoint& point = col.points[j];
00165                         // inertia frame -> joint0 frame
00166                         pos0(0) = point.position[0];
00167                         pos0(1) = point.position[1];
00168                         pos0(2) = point.position[2];
00169                         norm0(0) = point.normal[0];
00170                         norm0(1) = point.normal[1];
00171                         norm0(2) = point.normal[2];
00172                         pos0 -= joint0->abs_pos;
00173                         pos.mul(pos0, joint0->abs_att);
00174                         norm.mul(norm0, joint0->abs_att);
00175                         contact_pairs[i]->AddPoint(pos.data(), norm.data(), point.idepth);
00176                 }
00177         }
00178         //
00179         if(isEulerMethod)
00180         {
00181                 chain->Update(timeStep_, contact_pairs);
00182                 chain->Integrate(timeStep_);
00183                 chain->CalcPosition();
00184                 chain->CalcVelocity();
00185         }
00186         else
00187         {
00188                 chain->Update(timeStep_, contact_pairs);
00189                 chain->IntegrateRK4(timeStep_, 0);
00190                 chain->CalcPosition();
00191                 chain->CalcVelocity();
00192 
00193                 chain->Update();
00194                 chain->IntegrateRK4(timeStep_, 1);
00195                 chain->CalcPosition();
00196                 chain->CalcVelocity();
00197 
00198                 chain->Update();
00199                 chain->IntegrateRK4(timeStep_, 2);
00200                 chain->CalcPosition();
00201                 chain->CalcVelocity();
00202 
00203                 chain->Update();
00204                 chain->IntegrateRK4(timeStep_, 3);
00205                 chain->CalcPosition();
00206                 chain->CalcVelocity();
00207         }
00208         // update sensors
00209         logfile << "update sensors ->" << endl;
00210         int n;
00211         n = numSensors();
00212         for(int i=0; i<n; i++)
00213         {
00214                 if(sensors[i]->type == Sensor::FORCE)
00215                 {
00216                         ForceSensor* fs = (ForceSensor*)sensors[i];
00217                         update_force_sensor(fs);
00218                 }
00219                 else if(sensors[i]->type == Sensor::RATE_GYRO)
00220                 {
00221                         RateGyroSensor* rgs = (RateGyroSensor*)sensors[i];
00222                         update_rate_gyro_sensor(rgs);
00223                 }
00224                 else if(sensors[i]->type == Sensor::ACCELERATION)
00225                 {
00226                         AccelSensor* as = (AccelSensor*)sensors[i];
00227                         update_accel_sensor(as);
00228                 }
00229                 else
00230                 {
00231                 }
00232         }
00233         logfile << "<- update sensors" << endl;
00234     currentTime_ += timeStep_;
00235 }
00236 
00237 void World::update_force_sensor(ForceSensor* fs)
00238 {
00239         logfile << "force sensor: " << fs->name << endl;
00240         logfile << "joint = " << fs->joint->name << endl;
00241         logfile << "f = " << fs->joint->joint_f << endl;
00242         logfile << "n = " << fs->joint->joint_n << endl;
00243         logfile << "localR =  " << fs->localR << endl;
00244         logfile << "localPos = " << fs->localPos << endl;
00245         fVec3 local_f, local_n, n;
00246         local_f.mul(fs->joint->joint_f, fs->localR);
00247         n.cross(fs->joint->joint_f, fs->localPos);
00248         n += fs->joint->joint_n;
00249         local_n.mul(n, fs->localR);
00250         fs->f.neg(local_f);
00251         fs->tau.neg(local_n);
00252 }
00253 
00254 void World::update_rate_gyro_sensor(RateGyroSensor* rgs)
00255 {
00256         rgs->w.mul(rgs->joint->loc_ang_vel, rgs->localR);
00257 }
00258 
00259 void World::update_accel_sensor(AccelSensor* as)
00260 {
00261         chain->CalcAcceleration();
00262         fVec3 acc_j, tmp1;
00263         tmp1.cross(as->joint->loc_ang_vel, as->localPos);
00264         acc_j.cross(as->joint->loc_ang_vel, tmp1);
00265         tmp1.cross(as->joint->loc_ang_acc, as->localPos);
00266         acc_j += tmp1;
00267         acc_j += as->joint->loc_lin_acc;
00268         as->dv.mul(acc_j, as->localR);
00269 }
00270 
00271 void World::setEulerMethod()
00272 {
00273     isEulerMethod = true;
00274 }
00275 
00276 
00277 void World::setRungeKuttaMethod()
00278 {
00279     isEulerMethod = false;
00280 }
00281 
00282 
00283 #if 0
00284 std::pair<int,bool> World::getIndexOfLinkPairs(BodyPtr body1, Link* link1, BodyPtr body2, Link* link2)
00285 {
00286         int index = -1;
00287         int isRegistered = false;
00288 
00289     if(link1 != link2){
00290 
00291                 LinkPairKey linkPair;
00292                 linkPair.body1 = body1;
00293                 linkPair.link1 = link1;
00294                 linkPair.body2 = body2;
00295                 linkPair.link2 = link2;
00296 
00297                 LinkPairKeyToIndexMap::iterator p = linkPairKeyToIndexMap.find(linkPair);
00298 
00299                 if(p != linkPairKeyToIndexMap.end()){
00300                         index = p->second;
00301                         isRegistered = true;
00302                 } else {
00303                         index = numRegisteredLinkPairs++;
00304                         linkPairKeyToIndexMap[linkPair] = index;
00305                 }
00306         }
00307 
00308         return std::make_pair(index, isRegistered);
00309 }
00310 #endif
00311 
00312 int World::addSensor(Joint* _joint, int sensorType, int _id, const std::string _name, const fVec3& _localPos, const fMat33& _localR)
00313 {
00314         Sensor* sensor = Sensor::create(sensorType);
00315         sensor->joint = _joint;
00316         sensor->id = _id;
00317         sensor->name = _name;
00318         sensor->localPos.set(_localPos);
00319         sensor->localR.set(_localR);
00320         sensors.push_back(sensor);
00321         return 0;
00322 }
00323 
00324 
00325 Sensor* World::findSensor(const char* sensorName, const char* charName)
00326 {
00327         int n_sensors = sensors.size();
00328         for(int i=0; i<n_sensors; i++)
00329         {
00330                 if(!strcmp(sensors[i]->name.c_str(), sensorName) && sensors[i]->joint &&
00331                    !strcmp(sensors[i]->joint->CharName(), charName))
00332                 {
00333                         return sensors[i];
00334                 }
00335         }
00336         return 0;
00337 }
00338 
00339 int World::numSensors(int sensorType, const char* charName)
00340 {
00341         int count = 0;
00342         int n_sensors = sensors.size();
00343         for(int i=0; i<n_sensors; i++)
00344         {
00345                 if(sensors[i]->type == sensorType &&
00346                    (!charName || !strcmp(sensors[i]->joint->CharName(), charName)))
00347                 {
00348                         count++;
00349                 }
00350         }
00351         return count;
00352 }
00353 
00354 void World::getAllCharacterData(const char* name, OpenHRP::DynamicsSimulator::LinkDataType type, OpenHRP::DblSequence_out& rdata)
00355 {
00356         int index = -1, nchar = characters.size();
00357         for(int i=0; i<nchar; i++)
00358         {
00359                 if(!strcmp(name, characters[i].name.c_str()))
00360                 {
00361                         index = i;
00362                         break;
00363                 }
00364         }
00365         if(index < 0) return;
00366         CharacterInfo& cinfo = characters[index];
00367         int n_joints = cinfo.n_joints, n_links = cinfo.links.size();
00368         rdata->length(n_joints);
00369         for(int i=0; i<n_links; i++)
00370         {
00371                 if(cinfo.jointIDs[i] >= 0)
00372                 {
00373                         _get_all_character_data_sub(cinfo.links[i], cinfo.jointIDs[i], type, rdata);
00374                 }
00375         }
00376 }
00377 
00378 void World::_get_all_character_data_sub(Joint* cur, int index, OpenHRP::DynamicsSimulator::LinkDataType type, OpenHRP::DblSequence_out& rdata)
00379 {
00380         if(cur->j_type == ::JROTATE || cur->j_type == ::JSLIDE)
00381         {
00382                 switch(type) {
00383                 case OpenHRP::DynamicsSimulator::JOINT_VALUE:
00384                         (*rdata)[index] = cur->q;
00385                         break;
00386 
00387                 case OpenHRP::DynamicsSimulator::JOINT_VELOCITY:
00388                         (*rdata)[index] = cur->qd;
00389                         break;
00390 
00391                 case OpenHRP::DynamicsSimulator::JOINT_ACCELERATION:
00392                         (*rdata)[index] = cur->qdd;
00393                         break;
00394 
00395                 case OpenHRP::DynamicsSimulator::JOINT_TORQUE:
00396                         (*rdata)[index] = cur->tau;
00397                         break;
00398 
00399                 default:
00400 //                      cerr << "ERROR - Invalid type: " << getLabelOfGetLinkDataType(type) << endl;
00401                         break;
00402                 }
00403         }
00404 }
00405 
00406 void World::setAllCharacterData(const char* name, OpenHRP::DynamicsSimulator::LinkDataType type, const OpenHRP::DblSequence& wdata)
00407 {
00408         int index = -1, nchar = characters.size();
00409         for(int i=0; i<nchar; i++)
00410         {
00411                 if(!strcmp(name, characters[i].name.c_str()))
00412                 {
00413                         index = i;
00414                         break;
00415                 }
00416         }
00417         if(index < 0) return;
00418         CharacterInfo& cinfo = characters[index];
00419         int n_links = cinfo.links.size();
00420         for(int i=0; i<n_links; i++)
00421         {
00422                 if(cinfo.jointIDs[i] >= 0)
00423                 {
00424                         _set_all_character_data_sub(cinfo.links[i], cinfo.jointIDs[i], type, wdata);
00425                 }
00426         }
00427 }
00428 
00429 void World::_set_all_character_data_sub(Joint* cur, int index, OpenHRP::DynamicsSimulator::LinkDataType type, const OpenHRP::DblSequence& wdata)
00430 {
00431         if(index < (int)wdata.length() &&
00432            (cur->j_type == ::JROTATE || cur->j_type == ::JSLIDE))
00433         {
00434 //              logfile << "set[" << index << "]: " << cur->name << ": " << wdata[index] << endl;
00435                 switch(type) {
00436                 case OpenHRP::DynamicsSimulator::JOINT_VALUE:
00437                         cur->SetJointValue(wdata[index]);
00438                         break;
00439 
00440                 case OpenHRP::DynamicsSimulator::JOINT_VELOCITY:
00441                         cur->SetJointVel(wdata[index]);
00442                         break;
00443 
00444                 case OpenHRP::DynamicsSimulator::JOINT_ACCELERATION:
00445                         cur->SetJointAcc(wdata[index]);
00446                         break;
00447 
00448                 case OpenHRP::DynamicsSimulator::JOINT_TORQUE:
00449                         cur->SetJointForce(wdata[index]);
00450                         break;
00451 
00452                 default:
00453 //                      cerr << "ERROR - Invalid type: " << getLabelOfGetLinkDataType(type) << endl;
00454                         break;
00455                 }
00456         }
00457 }
00458 
00459 static void vec3_to_array(const fVec3& vec, double* a, int offset = 0)
00460 {
00461         a[offset++] = vec(0);
00462         a[offset++] = vec(1);
00463         a[offset] = vec(2);
00464 }
00465 
00466 static void mat33_to_array(const fMat33& mat, OpenHRP::DblArray9& a)
00467 {
00468         a[0] = mat(0,0);  a[1] = mat(0,1);  a[2] = mat(0,2);
00469         a[3] = mat(1,0);  a[4] = mat(1,1);  a[5] = mat(1,2);
00470         a[6] = mat(2,0);  a[7] = mat(2,1);  a[8] = mat(2,2);
00471 }
00472 
00473 void World::getAllCharacterPositions(OpenHRP::CharacterPositionSequence& all_char_pos)
00474 {
00475         int nchar = characters.size();
00476         for(int i=0; i<nchar; i++)
00477         {
00478                 CharacterInfo& cinfo = characters[i];
00479                 OpenHRP::CharacterPosition& char_pos = all_char_pos[i];
00480                 int nlink = cinfo.links.size();
00481                 for(int j=0; j<nlink; j++)
00482                 {
00483                         OpenHRP::LinkPosition& link_pos = char_pos.linkPositions[j];
00484                         Joint* cur = cinfo.links[j];
00485 //                      logfile << "[" << j << "] " << cur->name << ": " << cur->abs_pos << endl;
00486                         vec3_to_array(cur->abs_pos, link_pos.p);
00487                         mat33_to_array(cur->abs_att, link_pos.R);
00488                 }
00489         }
00490 }
00491 
00492 void World::getAllSensorStates(OpenHRP::SensorStateSequence& all_sensor_states)
00493 {
00494         int nchar = characters.size();
00495         for(int i=0; i<nchar; i++)
00496         {
00497                 OpenHRP::SensorState& state = all_sensor_states[i];
00498                 // joint angles and torques
00499                 CharacterInfo& cinfo = characters[i];
00500                 int n_links = cinfo.links.size();
00501                 for(int j=0; j<n_links; j++)
00502                 {
00503                         int index = cinfo.jointIDs[j];
00504                         if(index >= 0)
00505                         {
00506                                 Joint* joint = cinfo.links[j];
00507                                 if(joint->j_type == ::JROTATE || joint->j_type == ::JSLIDE){
00508                                         state.q[index] = joint->q;
00509                                         state.dq[index] = joint->qd;
00510                                         state.u[index] = joint->tau;
00511                                 }
00512                         }
00513                 }       
00514                 // other sensors
00515                 Joint* r = rootJoint(i);
00516                 int n_sensors = sensors.size();
00517                 for(int i=0; i<n_sensors; i++)
00518                 {
00519                         if(strcmp(sensors[i]->joint->CharName(), r->CharName())) continue;
00520                         switch(sensors[i]->type)
00521                         {
00522                         case Sensor::FORCE:
00523                         {
00524                                 ForceSensor* _fs = (ForceSensor*)sensors[i];
00525                                 vec3_to_array(_fs->f, state.force[sensors[i]->id], 0);
00526                                 vec3_to_array(_fs->tau, state.force[sensors[i]->id], 3);
00527                                 break;
00528                         }
00529 
00530                         case Sensor::RATE_GYRO:
00531                         {
00532                                 RateGyroSensor* _gs = (RateGyroSensor*)sensors[i];
00533                                 vec3_to_array(_gs->w, state.rateGyro[sensors[i]->id]);
00534                                 break;
00535                         }
00536 
00537                         case Sensor::ACCELERATION:
00538                         {
00539                                 AccelSensor* _as = (AccelSensor*)sensors[i];
00540                                 vec3_to_array(_as->dv, state.accel[sensors[i]->id]);
00541                                 break;
00542                         }
00543 
00544                         default:
00545                                 break;
00546                         }
00547                 }
00548         }
00549 }
00550 
00551 #if 0
00552 void World::_get_all_sensor_states_sub(Joint* cur, int& count, SensorState& state)
00553 {
00554         if(!cur || cur->real) return;
00555         if(cur->j_type == ::JROTATE || cur->j_type == ::JSLIDE)
00556         {
00557                 state.q[count] = cur->q;
00558                 state.u[count] = cur->tau;
00559                 count++;
00560         }
00561         _get_all_sensor_states_sub(cur->child, count, state);
00562         _get_all_sensor_states_sub(cur->brother, count, state);
00563 }
00564 #endif
00565 void World::calcCharacterJacobian(
00566                 const char* characterName,
00567                 const char* baseLink,
00568                 const char* targetLink,
00569                 fMat& J)
00570 {
00571         Joint* basej = chain->FindJoint(baseLink, characterName);
00572         if(!basej) return;
00573         Joint* targetj = chain->FindJoint(targetLink, characterName);
00574         if(!targetj) return;
00575 
00576         fMat tempJ(6, chain->NumDOF());
00577         tempJ.zero();
00578         targetj->CalcJacobian(tempJ);
00579 
00580         int n_dof = 0;
00581         for(Joint* j=targetj; j!=basej; j=j->parent)
00582         {
00583                 n_dof += j->n_dof;
00584         }
00585         
00586         J.resize(6, n_dof);
00587         J.zero();
00588         int count = 0;
00589         for(Joint* j=targetj; j!=basej; j=j->parent)
00590         {
00591                 for(int m=0; m<j->n_dof; m++)
00592                 {
00593                         J(0, count+m) = tempJ(0, j->i_dof+m);
00594                         J(1, count+m) = tempJ(1, j->i_dof+m);
00595                         J(2, count+m) = tempJ(2, j->i_dof+m);
00596                         J(3, count+m) = tempJ(3, j->i_dof+m);
00597                         J(4, count+m) = tempJ(4, j->i_dof+m);
00598                         J(5, count+m) = tempJ(5, j->i_dof+m);
00599                 }
00600                 count += j->n_dof;
00601         }
00602 }
00603 
00604 
00605 void World::addCollisionCheckLinkPair(Joint* jnt1, Joint* jnt2, double staticFriction, double slipFriction, double epsilon)
00606 {
00607         double default_spring = 1e5;
00608         double default_damper = 1e4;
00609         double default_slip_p = 2000.0;
00610         double default_slip_d = 700.0;
00611         double default_slip_func_coef_base = 0.1;
00612         SDContactPair* sd_pair = new SDContactPair(jnt1, jnt2, default_spring, default_damper, staticFriction, slipFriction, default_slip_p, default_slip_d, default_slip_func_coef_base);
00613         contact_pairs.push_back(sd_pair);
00614 }
00615 
00616 void World::addCharacter(Joint* rjoint, const std::string& name, OpenHRP::LinkInfoSequence_var links)
00617 {
00618         CharacterInfo cinfo(rjoint, name);
00619         int n_links = links->length();
00620         for(int i=0; i<n_links; i++)
00621         {
00622                 OpenHRP::LinkInfo linfo = links[i];
00623                 Joint* jnt = chain->FindJoint(linfo.name, name.c_str());
00624                 if(jnt)
00625                 {
00626                         cinfo.links.push_back(jnt);
00627                         cinfo.jointIDs.push_back(linfo.jointId);
00628                         if(linfo.jointId >= 0) cinfo.n_joints++;
00629                 }
00630         }
00631 //      logfile << "character: " << name << ", root = " << rjoint->name << ", n_joints = " << cinfo.n_joints << endl;
00632         characters.push_back(cinfo);
00633 }
00634 
00635 Joint* World::rootJoint(int index)
00636 {
00637         return characters[index].root;
00638 }


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