bool BodyMotionGenerationBar::shapeBodyMotionWithSimpleInterpolation (BodyPtr& body, PoseProvider* provider, BodyMotionItemPtr motionItem) { if(setup->onlyTimeBarRangeCheck.isChecked()){ poseProviderToBodyMotionConverter->setTimeRange(timeBar->minTime(), timeBar->maxTime()); } else { poseProviderToBodyMotionConverter->setFullTimeRange(); } poseProviderToBodyMotionConverter->setAllLinkPositionOutput(setup->se3Check.isChecked()); BodyMotionPtr motion = motionItem->motion(); motion->setFrameRate(timeBar->frameRate()); bool result = poseProviderToBodyMotionConverter->convert(body, provider, *motion); if(result){ motionItem->updateChildItemLineup(); motionItem->notifyUpdate(); } return result; }
bool BodyUnit::initialize(BodyItemPtr bodyItem) { if(TRACE_FUNCTIONS){ cout << "BodyUnit::initialize()" << endl; } if(bodyItem->body()->isStaticModel()){ return false; } frame = 0; body = bodyItem->body()->duplicate(); baseLink = 0; ItemList<BodyMotionItem> motionItems = bodyItem->getSubItems<BodyMotionItem>(); if(!motionItems.empty()){ BodyMotionItemPtr motionItem; // prefer the first checked item for(size_t i=0; i < motionItems.size(); ++i){ if(ItemTreeView::mainInstance()->isItemChecked(motionItems[i])){ motionItem = motionItems[i]; break; } } if(!motionItem){ motionItem = motionItems[0]; } frameRate = motionItem->motion()->frameRate(); qseqRef = motionItem->jointPosSeq(); if(qseqResult){ qseqResultBuf.reset(new MultiValueSeq(qseqResult->numParts(), 0, frameRate)); } if(motionItem->hasRelativeZmpSeqItem()){ zmpSeq = motionItem->relativeZmpSeq(); } rootResultItem = motionItem->linkPosSeqItem(); rootResultBuf.reset(new MultiAffine3Seq(1, 0, frameRate)); rootResult = motionItem->linkPosSeq(); rootResult->setFrameRate(frameRate); rootResult->setDimension(1, 1); } const YamlMapping& info = *bodyItem->body()->info(); const YamlSequence& footLinkInfos = *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()); Vector3 soleCenter; if(link && read(footLinkInfo, "soleCenter", soleCenter)){ footLinks.push_back(link); footLinkOriginHeights.push_back(-soleCenter[2]); } } } if(!footLinks.empty()){ if(zmpSeq){ setBaseLinkByZmp(0); } else { if(bodyItem->currentBaseLink()){ int currentBaseLinkIndex = bodyItem->currentBaseLink()->index; for(size_t i=0; i < footLinks.size(); ++i){ Link* footLink = footLinks[i]; if(footLink->index == currentBaseLinkIndex){ setBaseLink(footLink, footLinkOriginHeights[i]); } } } else{ setBaseLink(footLinks[0], footLinkOriginHeights[0]); } } } if(!baseLink){ if(bodyItem->currentBaseLink()){ baseLink = body->link(bodyItem->currentBaseLink()->index); } else { baseLink = body->rootLink(); } fkTraverse.find(baseLink, true, true); } if(rootResult){ Link* rootLink = body->rootLink(); Affine3& initialPos = rootResult->at(0, 0); initialPos.translation() = rootLink->p; initialPos.linear() = rootLink->R; } return (qseqRef != 0); }