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();
}
