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