00001
00002
00003
00004
00005
00006
00007
00008
00009
00015 #include "Body.h"
00016 #include "Link.h"
00017 #include "JointPath.h"
00018 #include "Sensor.h"
00019 #include "Light.h"
00020 #include "BodyCustomizerInterface.h"
00021 #include <hrpCollision/ColdetModel.h>
00022 #include <map>
00023 #include <cstdlib>
00024
00025 using namespace hrp;
00026 using namespace std;
00027
00028 static const bool PUT_DEBUG_MESSAGE = true;
00029
00030 static bool pluginsInDefaultDirectoriesLoaded = false;
00031
00032 #ifndef uint
00033 typedef unsigned int uint;
00034 #endif
00035
00036 namespace hrp {
00037
00038 class CustomizedJointPath : public JointPath
00039 {
00040 Body* body;
00041 int ikTypeId;
00042 bool isCustomizedIkPathReversed;
00043 virtual void onJointPathUpdated();
00044 public:
00045 CustomizedJointPath(Body* body, Link* baseLink, Link* targetLink);
00046 virtual ~CustomizedJointPath();
00047 virtual bool calcInverseKinematics(const Vector3& end_p, const Matrix33& end_R);
00048 virtual bool hasAnalyticalIK();
00049 };
00050 }
00051
00052
00053 Body::~Body()
00054 {
00055 if(customizerHandle){
00056 customizerInterface->destroy(customizerHandle);
00057 }
00058
00059 delete rootLink_;
00060
00061
00062 for(unsigned int sensorType =0; sensorType < numSensorTypes(); ++sensorType){
00063 int n = numSensors(sensorType);
00064 for(int i=0; i < n; ++i){
00065 Sensor* s = sensor(sensorType, i);
00066 if(s){
00067 Sensor::destroy(s);
00068 }
00069 }
00070 }
00071 }
00072
00073
00074 void Body::initialize()
00075 {
00076 rootLink_ = 0;
00077
00078 customizerHandle = 0;
00079 customizerInterface = 0;
00080 bodyHandleEntity.body = this;
00081 bodyHandle = &bodyHandleEntity;
00082 }
00083
00084
00085 Body::Body()
00086 : allSensors(Sensor::NUM_SENSOR_TYPES)
00087 {
00088 initialize();
00089
00090 rootLink_ = new Link;
00091 rootLink_->body = this;
00092
00093 defaultRootPosition.setZero();
00094 defaultRootAttitude.setIdentity();
00095 }
00096
00097
00098 Body::Body(const Body& org)
00099 : name_(org.name_),
00100 modelName_(org.modelName_),
00101 allSensors(Sensor::NUM_SENSOR_TYPES)
00102 {
00103 initialize();
00104
00105 setRootLink(new Link(*org.rootLink()));
00106
00107 defaultRootPosition = org.defaultRootPosition;
00108 defaultRootAttitude = org.defaultRootAttitude;
00109
00110
00111 std::map<Link*, int> linkToIndexMap;
00112
00113 for(unsigned int i=0; i < org.linkTraverse_.numLinks(); ++i){
00114 Link* lnk = org.linkTraverse_[i];
00115 linkToIndexMap[lnk] = i;
00116 }
00117
00118 int n = org.numSensorTypes();
00119 for(int sensorType = 0; sensorType < n; ++sensorType){
00120 for(unsigned int i=0; i < org.numSensors(sensorType); ++i){
00121 Sensor* orgSensor = org.sensor(sensorType, i);
00122 if (orgSensor){
00123 int linkIndex = linkToIndexMap[orgSensor->link];
00124 Link* newLink = linkTraverse_[linkIndex];
00125 Sensor* cloneSensor = createSensor(newLink, sensorType, orgSensor->id, orgSensor->name);
00126 *cloneSensor = *orgSensor;
00127 }
00128 }
00129 }
00130
00131
00132 for(size_t i=0; i < org.extraJoints.size(); ++i){
00133 const ExtraJoint& orgExtraJoint = org.extraJoints[i];
00134 ExtraJoint extraJoint(orgExtraJoint);
00135 for(int j=0; j < 2; ++j){
00136 extraJoint.link[j] = link(orgExtraJoint.link[j]->index);
00137 }
00138 }
00139
00140 if(org.customizerInterface){
00141 installCustomizer(org.customizerInterface);
00142 }
00143 }
00144
00145
00146 void Body::setRootLink(Link* link)
00147 {
00148 if(rootLink_){
00149 delete rootLink_;
00150 }
00151 rootLink_ = link;
00152
00153 updateLinkTree();
00154 }
00155
00156
00157 void Body::setDefaultRootPosition(const Vector3& p, const Matrix33& R)
00158 {
00159 defaultRootPosition = p;
00160 defaultRootAttitude = R;
00161 }
00162
00163
00164 void Body::getDefaultRootPosition(Vector3& out_p, Matrix33& out_R)
00165 {
00166 out_p = defaultRootPosition;
00167 out_R = defaultRootAttitude;
00168 }
00169
00170
00171 Link* Body::createEmptyJoint(int jointId)
00172 {
00173 Link* empty = new Link;
00174 empty->body = this;
00175 empty->jointId = jointId;
00176 empty->p.setZero();
00177 empty->R.setIdentity();
00178 empty->v.setZero();
00179 empty->w.setZero();
00180 empty->dv.setZero();
00181 empty->dw.setZero();
00182 empty->q = 0.0;
00183 empty->dq = 0.0;
00184 empty->ddq = 0.0;
00185 empty->u = 0.0;
00186 empty->a.setZero();
00187 empty->d.setZero();
00188 empty->b.setZero();
00189 empty->Rs.setIdentity();
00190 empty->m = 0.0;
00191 empty->I.setZero();
00192 empty->c.setZero();
00193 empty->wc.setZero();
00194 empty->vo.setZero();
00195 empty->dvo.setZero();
00196 empty->fext.setZero();
00197 empty->tauext.setZero();
00198 empty->Jm2 = 0.0;
00199 empty->ulimit = 0.0;
00200 empty->llimit = 0.0;
00201 empty->uvlimit = 0.0;
00202 empty->lvlimit = 0.0;
00203 empty->defaultJointValue = 0.0;
00204 empty->Ir = 0.0;
00205 empty->gearRatio = 1.0;
00206
00207 return empty;
00208 }
00209
00210
00211 void Body::updateLinkTree()
00212 {
00213 nameToLinkMap.clear();
00214 linkTraverse_.find(rootLink());
00215
00216 int n = linkTraverse_.numLinks();
00217 jointIdToLinkArray.clear();
00218 jointIdToLinkArray.resize(n, 0);
00219 int maxJointID = -1;
00220
00221 for(int i=0; i < n; ++i){
00222 Link* link = linkTraverse_[i];
00223 link->body = this;
00224 link->index = i;
00225 nameToLinkMap[link->name] = link;
00226
00227 int id = link->jointId;
00228 if(id >= 0){
00229 int size = jointIdToLinkArray.size();
00230 if(id >= size){
00231 jointIdToLinkArray.resize(id + 1, 0);
00232 }
00233 jointIdToLinkArray[id] = link;
00234 if(id > maxJointID){
00235 maxJointID = id;
00236 }
00237 }
00238 }
00239
00240 jointIdToLinkArray.resize(maxJointID + 1);
00241
00242 for(size_t i=0; i < jointIdToLinkArray.size(); ++i){
00243 if(!jointIdToLinkArray[i]){
00244 jointIdToLinkArray[i] = createEmptyJoint(i);
00245 std::cerr << "Warning: Model " << modelName_ <<
00246 " has empty joint ID in the valid IDs." << std::endl;
00247 }
00248 }
00249
00250 calcTotalMass();
00251
00252 isStaticModel_ = (rootLink_->jointType == Link::FIXED_JOINT && numJoints() == 0);
00253 }
00254
00255
00260 Link* Body::link(const std::string& name) const
00261 {
00262 NameToLinkMap::const_iterator p = nameToLinkMap.find(name);
00263 return (p != nameToLinkMap.end()) ? p->second : 0;
00264 }
00265
00266
00267 void Body::initializeConfiguration()
00268 {
00269 rootLink_->p = defaultRootPosition;
00270 rootLink_->setAttitude(defaultRootAttitude);
00271
00272 rootLink_->v.setZero();
00273 rootLink_->dv.setZero();
00274 rootLink_->w.setZero();
00275 rootLink_->dw.setZero();
00276 rootLink_->vo.setZero();
00277 rootLink_->dvo.setZero();
00278 rootLink_->constraintForces.clear();
00279
00280 int n = linkTraverse_.numLinks();
00281 for(int i=0; i < n; ++i){
00282 Link* link = linkTraverse_[i];
00283 link->u = 0.0;
00284 link->q = 0.0;
00285 link->dq = 0.0;
00286 link->ddq = 0.0;
00287 link->constraintForces.clear();
00288 }
00289
00290 calcForwardKinematics(true, true);
00291
00292 clearExternalForces();
00293 }
00294
00295
00296
00297 double Body::calcTotalMass()
00298 {
00299 totalMass_ = 0.0;
00300
00301 int n = linkTraverse_.numLinks();
00302 for(int i=0; i < n; ++i){
00303 totalMass_ += linkTraverse_[i]->m;
00304 }
00305
00306 return totalMass_;
00307 }
00308
00309
00310 Vector3 Body::calcCM()
00311 {
00312 totalMass_ = 0.0;
00313
00314 Vector3 mc(Vector3::Zero());
00315
00316 int n = linkTraverse_.numLinks();
00317 for(int i=0; i < n; i++){
00318 Link* link = linkTraverse_[i];
00319 link->wc = link->p + link->R * link->c;
00320 mc += link->m * link->wc;
00321 totalMass_ += link->m;
00322 }
00323
00324 return mc / totalMass_;
00325 }
00326
00327
00338 void Body::calcMassMatrix(dmatrix& out_M)
00339 {
00340
00341 dmatrix b1;
00342 dvector ddqorg;
00343 dvector uorg;
00344 Vector3 dvoorg;
00345 Vector3 dworg;
00346 Vector3 root_w_x_v;
00347 Vector3 g(0, 0, 9.8);
00348
00349 uint nJ = numJoints();
00350 int totaldof = nJ;
00351 if( !isStaticModel_ ) totaldof += 6;
00352
00353 out_M.resize(totaldof,totaldof);
00354 b1.resize(totaldof, 1);
00355
00356
00357 ddqorg.resize(nJ);
00358 uorg.resize(nJ);
00359 for(uint i = 0; i < nJ; ++i){
00360 Link* ptr = joint(i);
00361 ddqorg[i] = ptr->ddq;
00362 uorg [i] = ptr->u;
00363 ptr->ddq = 0.0;
00364 }
00365
00366
00367 dvoorg = rootLink_->dvo;
00368 dworg = rootLink_->dw;
00369 root_w_x_v = rootLink_->w.cross(rootLink_->vo + rootLink_->w.cross(rootLink_->p));
00370 rootLink_->dvo = g - root_w_x_v;
00371 rootLink_->dw.setZero();
00372
00373 setColumnOfMassMatrix(b1, 0);
00374
00375 if( !isStaticModel_ ){
00376 for(int i=0; i < 3; ++i){
00377 rootLink_->dvo[i] += 1.0;
00378 setColumnOfMassMatrix(out_M, i);
00379 rootLink_->dvo[i] -= 1.0;
00380 }
00381 for(int i=0; i < 3; ++i){
00382 rootLink_->dw[i] = 1.0;
00383 Vector3 dw_x_p = rootLink_->dw.cross(rootLink_->p);
00384 rootLink_->dvo -= dw_x_p;
00385 setColumnOfMassMatrix(out_M, i + 3);
00386 rootLink_->dvo += dw_x_p;
00387 rootLink_->dw[i] = 0.0;
00388 }
00389 }
00390
00391 for(uint i = 0; i < nJ; ++i){
00392 Link* ptr = joint(i);
00393 ptr->ddq = 1.0;
00394 int j = i + 6;
00395 setColumnOfMassMatrix(out_M, j);
00396 out_M(j, j) += ptr->Jm2;
00397 ptr->ddq = 0.0;
00398 }
00399
00400
00401 for(int i = 0; i < out_M.cols(); ++i){
00402 out_M.col(i) -= b1;
00403 }
00404
00405
00406 for(uint i = 0; i < nJ; ++i){
00407 Link* ptr = joint(i);
00408 ptr->ddq = ddqorg[i];
00409 ptr->u = uorg [i];
00410 }
00411 rootLink_->dvo = dvoorg;
00412 rootLink_->dw = dworg;
00413 }
00414
00415
00416 void Body::setColumnOfMassMatrix(dmatrix& out_M, int column)
00417 {
00418 Vector3 f;
00419 Vector3 tau;
00420
00421 calcInverseDynamics(rootLink_, f, tau);
00422
00423 if( !isStaticModel_ ){
00424 tau -= rootLink_->p.cross(f);
00425 out_M.block<6,1>(0, column) << f, tau;
00426 }
00427
00428 int n = numJoints();
00429 for(int i = 0; i < n; ++i){
00430 Link* ptr = joint(i);
00431 out_M(i + 6, column) = ptr->u;
00432 }
00433 }
00434
00435
00436
00437
00438
00439 void Body::calcInverseDynamics(Link* ptr, Vector3& out_f, Vector3& out_tau)
00440 {
00441 Link* parent = ptr->parent;
00442 if(parent){
00443 Vector3 dsv,dsw,sv,sw;
00444
00445 if(ptr->jointType != Link::FIXED_JOINT){
00446 sw.noalias() = parent->R * ptr->a;
00447 sv = ptr->p.cross(sw);
00448 }else{
00449 sw.setZero();
00450 sv.setZero();
00451 }
00452 dsv = parent->w.cross(sv) + parent->vo.cross(sw);
00453 dsw = parent->w.cross(sw);
00454
00455 ptr->dw = parent->dw + dsw * ptr->dq + sw * ptr->ddq;
00456 ptr->dvo = parent->dvo + dsv * ptr->dq + sv * ptr->ddq;
00457
00458 ptr->sw = sw;
00459 ptr->sv = sv;
00460 }
00461
00462 Vector3 c,P,L;
00463 Matrix33 I,c_hat;
00464
00465 c = ptr->R * ptr->c + ptr->p;
00466 I.noalias() = ptr->R * ptr->I * ptr->R.transpose();
00467 c_hat = hat(c);
00468 I.noalias() += ptr->m * c_hat * c_hat.transpose();
00469 P.noalias() = ptr->m * (ptr->vo + ptr->w.cross(c));
00470 L = ptr->m * c.cross(ptr->vo) + I * ptr->w;
00471
00472 out_f = ptr->m * (ptr->dvo + ptr->dw.cross(c)) + ptr->w.cross(P);
00473 out_tau = ptr->m * c.cross(ptr->dvo) + I * ptr->dw + ptr->vo.cross(P) + ptr->w.cross(L);
00474
00475 if(ptr->child){
00476 Vector3 f_c;
00477 Vector3 tau_c;
00478 calcInverseDynamics(ptr->child, f_c, tau_c);
00479 out_f += f_c;
00480 out_tau += tau_c;
00481 }
00482
00483 ptr->u = ptr->sv.dot(out_f) + ptr->sw.dot(out_tau);
00484
00485 if(ptr->sibling){
00486 Vector3 f_s;
00487 Vector3 tau_s;
00488 calcInverseDynamics(ptr->sibling, f_s, tau_s);
00489 out_f += f_s;
00490 out_tau += tau_s;
00491 }
00492 }
00493
00494
00499 void Body::calcTotalMomentum(Vector3& out_P, Vector3& out_L)
00500 {
00501 out_P.setZero();
00502 out_L.setZero();
00503
00504 Vector3 dwc;
00505 Vector3 P;
00506 Vector3 L;
00507 Vector3 Llocal;
00508
00509 int n = linkTraverse_.numLinks();
00510 for(int i=0; i < n; i++){
00511 Link* link = linkTraverse_[i];
00512
00513 dwc = link->v + link->w.cross(link->R * link->c);
00514
00515 P = link->m * dwc;
00516
00517
00518 Llocal.noalias() = link->I * link->R.transpose()*link->w;
00519 L = link->wc.cross(P) + link->R * Llocal;
00520
00521 out_P += P;
00522 out_L += L;
00523 }
00524 }
00525
00526 void Body::calcTotalMomentumFromJacobian(Vector3& out_P, Vector3& out_L)
00527 {
00528 out_P.setZero();
00529 out_L.setZero();
00530
00531 dmatrix J,H;
00532 calcCMJacobian(NULL,J);
00533 calcAngularMomentumJacobian(NULL,H);
00534
00535 dvector dq;
00536 int n = numJoints();
00537 dq.resize(n);
00538 for(int i=0; i < n; i++){
00539 Link* link = joint(i);
00540 dq[i] = link->dq;
00541 }
00542 dvector v;
00543 v.resize(n+3+3);
00544 v << dq, rootLink_->v, rootLink_->w;
00545
00546 out_P = totalMass_ * J * v;
00547 out_L = H * v;
00548 }
00549
00550 void Body::calcForwardKinematics(bool calcVelocity, bool calcAcceleration)
00551 {
00552 linkTraverse_.calcForwardKinematics(calcVelocity, calcAcceleration);
00553 }
00554
00555
00556 Light* Body::createLight(Link* link, int lightType, const std::string& name)
00557 {
00558 Light *light = new Light(link, lightType, name);
00559 nameToLightMap[name] = light;
00560 return light;
00561 }
00562
00563 Sensor* Body::createSensor(Link* link, int sensorType, int id, const std::string& name)
00564 {
00565 Sensor* sensor = 0;
00566
00567 if(sensorType < Sensor::NUM_SENSOR_TYPES && id >= 0){
00568
00569 SensorArray& sensors = allSensors[sensorType];
00570 int n = sensors.size();
00571 if(id >= n){
00572 sensors.resize(id + 1, 0);
00573 }
00574 sensor = sensors[id];
00575 if(sensor){
00576 std::cerr << "duplicated sensor Id is specified(id = "
00577 << id << ", name = " << name << ")" << std::endl;
00578
00579 nameToSensorMap.erase(sensor->name);
00580 } else {
00581 sensor = Sensor::create(sensorType);
00582 }
00583 if(sensor){
00584 sensor->id = id;
00585 sensors[id] = sensor;
00586 sensor->link = link;
00587 sensor->name = name;
00588 nameToSensorMap[name] = sensor;
00589 link->sensors.push_back(sensor);
00590 }
00591 }
00592
00593 return sensor;
00594 }
00595
00596 void Body::addSensor(Sensor* sensor, int sensorType, int id ){
00597 if(sensorType < Sensor::NUM_SENSOR_TYPES && id >= 0){
00598 SensorArray& sensors = allSensors[sensorType];
00599 int n = sensors.size();
00600 if(id >= n){
00601 sensors.resize(id + 1, 0);
00602 }
00603 Sensor* sameId = sensors[id];
00604 if(sameId){
00605 std::cerr << "duplicated sensor Id is specified(id = "
00606 << id << ", name = " << sensor->name << ")" << std::endl;
00607
00608 nameToSensorMap.erase(sameId->name);
00609 }
00610 sensors[id] = sensor;
00611 nameToSensorMap[sensor->name] = sensor;
00612 }
00613 }
00614
00615
00616 void Body::clearSensorValues()
00617 {
00618 for(unsigned int i=0; i < numSensorTypes(); ++i){
00619 for(unsigned int j=0; j < numSensors(i); ++j){
00620 if(sensor(i,j))
00621 sensor(i, j)->clear();
00622 }
00623 }
00624 }
00625
00626
00627 JointPathPtr Body::getJointPath(Link* baseLink, Link* targetLink)
00628 {
00629 if(customizerInterface && customizerInterface->initializeAnalyticIk){
00630 return JointPathPtr(new CustomizedJointPath(this, baseLink, targetLink));
00631 } else {
00632 return JointPathPtr(new JointPath(baseLink, targetLink));
00633 }
00634 }
00635
00636
00637 void Body::setVirtualJointForcesSub()
00638 {
00639 if(customizerInterface->setVirtualJointForces){
00640 customizerInterface->setVirtualJointForces(customizerHandle);
00641 }
00642 }
00643
00644
00645 void Body::clearExternalForces()
00646 {
00647 int n = linkTraverse_.numLinks();
00648 for(int i=0; i < n; ++i){
00649 Link* link = linkTraverse_[i];
00650 link->fext.setZero();
00651 link->tauext.setZero();
00652 }
00653 }
00654
00655
00656 void Body::updateLinkColdetModelPositions()
00657 {
00658 const int n = linkTraverse_.numLinks();
00659 for(int i=0; i < n; ++i){
00660 Link* link = linkTraverse_[i];
00661 if(link->coldetModel){
00662 link->coldetModel->setPosition(link->segmentAttitude(), link->p);
00663 }
00664 }
00665 }
00666
00667
00668 void Body::putInformation(std::ostream &out)
00669 {
00670 out << "Body: model name = " << modelName_ << " name = " << name_ << "\n\n";
00671
00672 int n = numLinks();
00673 for(int i=0; i < n; ++i){
00674 out << *link(i);
00675 }
00676 out << std::endl;
00677 }
00678
00679
00683 bool Body::installCustomizer()
00684 {
00685 if(!pluginsInDefaultDirectoriesLoaded){
00686 loadBodyCustomizers(bodyInterface());
00687 pluginsInDefaultDirectoriesLoaded = true;
00688 }
00689
00690 BodyCustomizerInterface* interface = findBodyCustomizer(modelName_);
00691
00692 return interface ? installCustomizer(interface) : false;
00693 }
00694
00695
00696 bool Body::installCustomizer(BodyCustomizerInterface * customizerInterface)
00697 {
00698 if(this->customizerInterface){
00699 if(customizerHandle){
00700 this->customizerInterface->destroy(customizerHandle);
00701 customizerHandle = 0;
00702 }
00703 this->customizerInterface = 0;
00704 }
00705
00706 if(customizerInterface){
00707 customizerHandle = customizerInterface->create(bodyHandle, modelName_.c_str());
00708 if(customizerHandle){
00709 this->customizerInterface = customizerInterface;
00710 }
00711 }
00712
00713 return (customizerHandle != 0);
00714 }
00715
00716
00717 std::ostream& operator<< (std::ostream& out, Body& body)
00718 {
00719 body.putInformation(out);
00720 return out;
00721 }
00722
00723
00724 static inline Link* extractLink(BodyHandle bodyHandle, int linkIndex)
00725 {
00726 return static_cast<BodyHandleEntity*>(bodyHandle)->body->link(linkIndex);
00727 }
00728
00729
00730 static int getLinkIndexFromName(BodyHandle bodyHandle, const char* linkName)
00731 {
00732 Body* body = static_cast<BodyHandleEntity*>(bodyHandle)->body;
00733 Link* link = body->link(linkName);
00734 return (link ? link->index : -1);
00735 }
00736
00737
00738 static const char* getLinkName(BodyHandle bodyHandle, int linkIndex)
00739 {
00740 return extractLink(bodyHandle, linkIndex)->name.c_str();
00741 }
00742
00743
00744 static double* getJointValuePtr(BodyHandle bodyHandle, int linkIndex)
00745 {
00746 return &(extractLink(bodyHandle,linkIndex)->q);
00747 }
00748
00749
00750 static double* getJointVelocityPtr(BodyHandle bodyHandle, int linkIndex)
00751 {
00752 return &(extractLink(bodyHandle, linkIndex)->dq);
00753 }
00754
00755
00756 static double* getJointTorqueForcePtr(BodyHandle bodyHandle, int linkIndex)
00757 {
00758 return &(extractLink(bodyHandle, linkIndex)->u);
00759 }
00760
00761
00762 BodyInterface* Body::bodyInterface()
00763 {
00764 static BodyInterface interface = {
00765 hrp::BODY_INTERFACE_VERSION,
00766 getLinkIndexFromName,
00767 getLinkName,
00768 getJointValuePtr,
00769 getJointVelocityPtr,
00770 getJointTorqueForcePtr,
00771 };
00772
00773 return &interface;
00774 }
00775
00776
00777 CustomizedJointPath::CustomizedJointPath(Body* body, Link* baseLink, Link* targetLink) :
00778 JointPath(baseLink, targetLink),
00779 body(body)
00780 {
00781 onJointPathUpdated();
00782 }
00783
00784
00785 CustomizedJointPath::~CustomizedJointPath()
00786 {
00787
00788 }
00789
00790
00791 void CustomizedJointPath::onJointPathUpdated()
00792 {
00793 ikTypeId = body->customizerInterface->initializeAnalyticIk(
00794 body->customizerHandle, baseLink()->index, endLink()->index);
00795 if(ikTypeId){
00796 isCustomizedIkPathReversed = false;
00797 } else {
00798
00799 ikTypeId = body->customizerInterface->initializeAnalyticIk(
00800 body->customizerHandle, endLink()->index, baseLink()->index);
00801 if(ikTypeId){
00802 isCustomizedIkPathReversed = true;
00803 }
00804 }
00805 }
00806
00807
00808 bool CustomizedJointPath::calcInverseKinematics(const Vector3& end_p, const Matrix33& end_R)
00809 {
00810 bool solved;
00811
00812 if(ikTypeId == 0 || isBestEffortIKMode){
00813
00814 solved = JointPath::calcInverseKinematics(end_p, end_R);
00815
00816 } else {
00817
00818 std::vector<double> qorg(numJoints());
00819
00820 for(unsigned int i=0; i < numJoints(); ++i){
00821 qorg[i] = joint(i)->q;
00822 }
00823
00824 const Link* targetLink = endLink();
00825 const Link* baseLink_ = baseLink();
00826
00827 Vector3 p_relative;
00828 Matrix33 R_relative;
00829 if(!isCustomizedIkPathReversed){
00830 p_relative.noalias() = baseLink_->R.transpose() * (end_p - baseLink_->p);
00831 R_relative.noalias() = baseLink_->R.transpose() * end_R;
00832 } else {
00833 p_relative.noalias() = end_R.transpose() * (baseLink_->p - end_p);
00834 R_relative.noalias() = end_R.transpose() * baseLink_->R;
00835 }
00836 solved = body->customizerInterface->
00837 calcAnalyticIk(body->customizerHandle, ikTypeId, p_relative, R_relative);
00838
00839 if(solved){
00840
00841 calcForwardKinematics();
00842
00843 Vector3 dp(end_p - targetLink->p);
00844 Vector3 omega(omegaFromRot((targetLink->R*targetLink->Rs).transpose() * end_R));
00845
00846 double errsqr = dp.dot(dp) + omega.dot(omega);
00847
00848 if(errsqr < maxIKErrorSqr){
00849 solved = true;
00850 } else {
00851 solved = false;
00852 for(unsigned int i=0; i < numJoints(); ++i){
00853 joint(i)->q = qorg[i];
00854 }
00855 calcForwardKinematics();
00856 }
00857 }
00858 }
00859
00860 return solved;
00861 }
00862
00863
00864 bool CustomizedJointPath::hasAnalyticalIK()
00865 {
00866 return (ikTypeId != 0);
00867 }
00868
00869 void Body::calcCMJacobian(Link *base, dmatrix &J)
00870 {
00871
00872 JointPathPtr jp;
00873 if (base){
00874 jp = getJointPath(rootLink(), base);
00875 Link *skip = jp->joint(0);
00876 skip->subm = rootLink()->m;
00877 skip->submwc = rootLink()->m*rootLink()->wc;
00878 Link *l = rootLink()->child;
00879 if (l){
00880 if (l != skip) {
00881 l->calcSubMassCM();
00882 skip->subm += l->subm;
00883 skip->submwc += l->submwc;
00884 }
00885 l = l->sibling;
00886 while(l){
00887 if (l != skip){
00888 l->calcSubMassCM();
00889 skip->subm += l->subm;
00890 skip->submwc += l->submwc;
00891 }
00892 l = l->sibling;
00893 }
00894 }
00895
00896
00897 for (unsigned int i=1; i<jp->numJoints(); i++){
00898 l = jp->joint(i);
00899 l->subm = l->parent->m + l->parent->subm;
00900 l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
00901 }
00902
00903 J.resize(3, numJoints());
00904 }else{
00905 rootLink()->calcSubMassCM();
00906 J.resize(3, numJoints()+6);
00907 }
00908
00909
00910 std::vector<int> sgn(numJoints(), 1);
00911 if (jp) {
00912 for (unsigned int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
00913 }
00914
00915 for (unsigned int i=0; i<numJoints(); i++){
00916 Link *j = joint(i);
00917 switch(j->jointType){
00918 case Link::ROTATIONAL_JOINT:
00919 {
00920 Vector3 omega(sgn[j->jointId]*j->R*j->a);
00921 Vector3 arm((j->submwc-j->subm*j->p)/totalMass_);
00922 Vector3 dp(omega.cross(arm));
00923 J.col(j->jointId) = dp;
00924 break;
00925 }
00926 case Link::SLIDE_JOINT:
00927 {
00928 Vector3 dp((j->subm/totalMass_)*sgn[j->jointId]*j->R*j->d);
00929 J.col(j->jointId) = dp;
00930 break;
00931 }
00932 default:
00933 std::cerr << "calcCMJacobian() : unsupported jointType("
00934 << j->jointType << ")" << std::endl;
00935 }
00936 }
00937 if (!base){
00938 int c = numJoints();
00939 J(0, c ) = 1.0; J(0, c+1) = 0.0; J(0, c+2) = 0.0;
00940 J(1, c ) = 0.0; J(1, c+1) = 1.0; J(1, c+2) = 0.0;
00941 J(2, c ) = 0.0; J(2, c+1) = 0.0; J(2, c+2) = 1.0;
00942
00943 Vector3 dp(rootLink()->submwc/totalMass_ - rootLink()->p);
00944 J(0, c+3) = 0.0; J(0, c+4) = dp(2); J(0, c+5) = -dp(1);
00945 J(1, c+3) = -dp(2); J(1, c+4) = 0.0; J(1, c+5) = dp(0);
00946 J(2, c+3) = dp(1); J(2, c+4) = -dp(0); J(2, c+5) = 0.0;
00947 }
00948 }
00949
00950 void Body::calcAngularMomentumJacobian(Link *base, dmatrix &H)
00951 {
00952
00953 JointPathPtr jp;
00954
00955 dmatrix M;
00956 calcCMJacobian(base, M);
00957 M.conservativeResize(3, numJoints());
00958 M *= totalMass();
00959
00960 if (base){
00961 jp = getJointPath(rootLink(), base);
00962 Link *skip = jp->joint(0);
00963 skip->subm = rootLink()->m;
00964 skip->submwc = rootLink()->m*rootLink()->wc;
00965 Link *l = rootLink()->child;
00966 if (l){
00967 if (l != skip) {
00968 l->calcSubMassCM();
00969 skip->subm += l->subm;
00970 skip->submwc += l->submwc;
00971 }
00972 l = l->sibling;
00973 while(l){
00974 if (l != skip){
00975 l->calcSubMassCM();
00976 skip->subm += l->subm;
00977 skip->submwc += l->submwc;
00978 }
00979 l = l->sibling;
00980 }
00981 }
00982
00983
00984 for (unsigned int i=1; i<jp->numJoints(); i++){
00985 l = jp->joint(i);
00986 l->subm = l->parent->m + l->parent->subm;
00987 l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
00988 }
00989
00990 H.resize(3, numJoints());
00991 }else{
00992 rootLink()->calcSubMassCM();
00993 H.resize(3, numJoints()+6);
00994 }
00995
00996
00997 std::vector<int> sgn(numJoints(), 1);
00998 if (jp) {
00999 for (unsigned int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
01000 }
01001
01002 for (unsigned int i=0; i<numJoints(); i++){
01003 Link *j = joint(i);
01004 switch(j->jointType){
01005 case Link::ROTATIONAL_JOINT:
01006 {
01007 Vector3 omega(sgn[j->jointId]*j->R*j->a);
01008 Vector3 Mcol = M.col(j->jointId);
01009 Matrix33 jsubIw;
01010 j->calcSubMassInertia(jsubIw);
01011 Vector3 dp = jsubIw*omega;
01012 if (j->subm!=0) dp += (j->submwc/j->subm).cross(Mcol);
01013 H.col(j->jointId) = dp;
01014 break;
01015 }
01016 case Link::SLIDE_JOINT:
01017 {
01018 if(j->subm!=0){
01019 Vector3 Mcol =M.col(j->jointId);
01020 Vector3 dp = (j->submwc/j->subm).cross(Mcol);
01021 H.col(j->jointId) = dp;
01022 }
01023 break;
01024 }
01025 default:
01026 std::cerr << "calcCMJacobian() : unsupported jointType("
01027 << j->jointType << ")" << std::endl;
01028 }
01029 }
01030 if (!base){
01031 int c = numJoints();
01032 H.block(0, c, 3, 3).setZero();
01033 Matrix33 Iw;
01034 rootLink_->calcSubMassInertia(Iw);
01035 H.block(0, c+3, 3, 3) = Iw;
01036 Vector3 cm = calcCM();
01037 Matrix33 cm_cross;
01038 cm_cross <<
01039 0.0, -cm(2), cm(1),
01040 cm(2), 0.0, -cm(0),
01041 -cm(1), cm(0), 0.0;
01042 H.block(0,0,3,c) -= cm_cross * M;
01043 }
01044 }