bool GRobotControllerItem::onPlaybackInitialized(double time)
{
if(controller->isPlayingMotion()){
controller->stopMotion();
}
playbackConnections.disconnect();
if(bodyItem){
BodyMotionItemPtr motionItem =
ItemTreeView::mainInstance()->selectedSubItem<BodyMotionItem>(bodyItem);
if(motionItem){
MultiValueSeqPtr qseq = motionItem->jointPosSeq();
if(qseq->numFrames() > 0 && qseq->numParts() == controller->numJoints()){
if(controller->setMotion(&qseq->at(0, 0), qseq->numFrames(), qseq->getTimeStep())){
TimeBar* timeBar = TimeBar::instance();
playbackConnections.add(
timeBar->sigPlaybackStarted().connect(
bind(&GRobotControllerItem::onPlaybackStarted, this, _1)));
playbackConnections.add(
timeBar->sigPlaybackStopped().connect(
bind(&GRobotControllerItem::onPlaybackStopped, this)));
}
}
}
}
return true;
}
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);
}
