void BodyItemImpl::getParticularPosition(BodyItem::PositionType position, boost::optional<Vector3>& pos) { if(position == BodyItem::ZERO_MOMENT_POINT){ pos = zmp; } else { if(position == BodyItem::CM_PROJECTION){ pos = self->centerOfMass(); } else { LeggedBodyHelperPtr legged = getLeggedBodyHelper(body); if(legged->isValid()){ if(position == BodyItem::HOME_COP){ pos = legged->homeCopOfSoles(); } else if(position == BodyItem::RIGHT_HOME_COP || position == BodyItem::LEFT_HOME_COP) { if(legged->numFeet() == 2){ pos = legged->homeCopOfSole((position == BodyItem::RIGHT_HOME_COP) ? 0 : 1); } } } } if(pos){ (*pos).z() = 0.0; } } }
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(); }