void BodyMotionPoseProvider::initialize(BodyPtr body__, BodyMotionPtr motion) { body_ = body__->clone(); this->motion = motion; footLinkPositions.reset(new MultiAffine3Seq()); footLinks.clear(); ikPaths.clear(); LeggedBodyHelperPtr legged = getLeggedBodyHelper(body_); if(legged->isValid()){ for(int i=0; i < legged->numFeet(); ++i){ Link* link = legged->footLink(i); JointPathPtr ikPath = getCustomJointPath(body_, body_->rootLink(), link); if(ikPath){ if(ikPath->hasAnalyticalIK() || ikPath->numJoints() == 6){ footLinks.push_back(link); ikPaths.push_back(ikPath); } } } ZMP_ = legged->homeCopOfSoles(); } else { ZMP_.setZero(); } updateMotion(); }
bool CompositeIK::addBaseLink(Link* baseLink) { if(baseLink && targetLink_){ JointPathPtr path = getCustomJointPath(body_, targetLink_, baseLink); if(path){ hasAnalyticalIK_ = paths.empty() ? path->hasAnalyticalIK() : (hasAnalyticalIK_ && path->hasAnalyticalIK()); paths.push_back(path); return true; } } return false; }
bool CompositeIK::addBaseLink(Link* baseLink) { if(baseLink && targetLink_){ JointPathPtr path = getCustomJointPath(body_, targetLink_, baseLink); if(path){ isAnalytical_ = pathList.empty() ? path->hasAnalyticalIK() : (isAnalytical_ && path->hasAnalyticalIK()); PathInfo info; info.path = path; info.endLink = baseLink; pathList.push_back(info); return true; } } return false; }
void BodyMotionPoseProvider::initialize(BodyPtr body__, BodyMotionPtr motion) { body_ = body__->duplicate(); this->motion = motion; footLinkPositions.reset(new MultiAffine3Seq()); footLinks.clear(); ikPaths.clear(); YamlSequence& footLinkInfos = *body_->info()->findSequence("footLinks"); if(footLinkInfos.isValid()){ for(int i=0; i < footLinkInfos.size(); ++i){ const YamlMapping& footLinkInfo = *footLinkInfos[i].toMapping(); Link* link = body_->link(footLinkInfo["link"].toString()); JointPathPtr ikPath = body_->getJointPath(body_->rootLink(), link); if(ikPath && ikPath->hasAnalyticalIK()){ footLinks.push_back(link); ikPaths.push_back(ikPath); } } } updateMotion(); }
bool LeggedBodyHelper::doLegIkToMoveCm(const Vector3& c, bool onlyProjectionToFloor) { if(!isValid_){ return false; } static const int MAX_ITERATION = 100; static const double ERROR_THRESH_SQR = 1.0e-6 * 1.0e-6; Link* baseFoot = footInfos[0].link; Link* waist = body_->rootLink(); LinkTraverse fkTraverse(waist); Vector3 c0 = body_->calcCenterOfMass(); bool failed = false; int loopCounter = 0; while(true){ Vector3 e = c - c0; if(onlyProjectionToFloor){ e.z() = 0.0; } if(e.squaredNorm() < ERROR_THRESH_SQR){ break; } size_t numDone = 0; JointPathPtr baseToWaist = getCustomJointPath(body_, baseFoot, waist); if(baseToWaist && baseToWaist->calcInverseKinematics(waist->p() + e, waist->R())){ numDone++; for(size_t j=1; j < footInfos.size(); ++j){ Link* foot = footInfos[j].link; JointPathPtr waistToFoot = getCustomJointPath(body_, waist, foot); if(waistToFoot){ bool ikDone; if(waistToFoot->hasAnalyticalIK()){ ikDone = waistToFoot->calcInverseKinematics(foot->p(), foot->R()); } else { Vector3 p0 = foot->p(); Matrix3 R0 = foot->R(); waistToFoot->calcForwardKinematics(); ikDone = waistToFoot->calcInverseKinematics(p0, R0); } if(ikDone){ numDone++; } else { break; } } } } if(numDone < footInfos.size()){ failed = true; break; } if(++loopCounter < MAX_ITERATION){ fkTraverse.calcForwardKinematics(); c0 = body_->calcCenterOfMass(); } else { break; } } return !failed; }