00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
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
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
00128 chain->Schedule();
00129 chain->CalcPosition();
00130 chain->CalcVelocity();
00131
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
00152
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
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
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
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
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
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
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
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
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
00632 characters.push_back(cinfo);
00633 }
00634
00635 Joint* World::rootJoint(int index)
00636 {
00637 return characters[index].root;
00638 }