void PoseSeqItem::onPositionChanged() { if(!sigInterpolationParametersChangedConnection.connected()){ sigInterpolationParametersChangedConnection = generationBar->sigInterpolationParametersChanged().connect( std::bind(&PoseSeqItem::updateInterpolationParameters, this)); updateInterpolationParameters(); } BodyItemPtr prevBodyItem = ownerBodyItem; ownerBodyItem = findOwnerItem<BodyItem>(); if(ownerBodyItem == prevBodyItem){ return; } if(!ownerBodyItem){ interpolator_->setBody(0); } else { Body* body = ownerBodyItem->body(); if(seq->targetBodyName().empty()){ seq->setTargetBodyName(body->name()); } else if(prevBodyItem && (seq->targetBodyName() != body->name())){ convert(prevBodyItem->body()); } interpolator_->setBody(body); const Listing& linearInterpolationJoints = *ownerBodyItem->body()->info()->findListing("linearInterpolationJoints"); if(linearInterpolationJoints.isValid()){ for(int i=0; i < linearInterpolationJoints.size(); ++i){ Link* link = body->link(linearInterpolationJoints[i].toString()); if(link){ interpolator_->setLinearInterpolationJoint(link->jointId()); } } } LeggedBodyHelperPtr legged = getLeggedBodyHelper(ownerBodyItem->body()); if(legged->isValid()){ for(int i=0; i < legged->numFeet(); ++i){ interpolator_->addFootLink(legged->footLink(i)->index(), legged->centerOfSoleLocal(i)); } } interpolator_->setLipSyncShapes(*ownerBodyItem->body()->info()->findMapping("lipSyncShapes")); bodyMotionItem_->motion()->setNumJoints(interpolator_->body()->numJoints()); if(generationBar->isAutoGenerationForNewBodyEnabled()){ updateTrajectory(true); } else { bodyMotionItem_->notifyUpdate(); } } }
static bool bodyMotionItemPreFilter(BodyMotionItem* protoItem, Item* parentItem) { BodyItemPtr bodyItem = dynamic_cast<BodyItem*>(parentItem); if(!bodyItem){ bodyItem = parentItem->findOwnerItem<BodyItem>(); } if(bodyItem){ int prevNumJoints = protoItem->jointPosSeq()->numParts(); int numJoints = bodyItem->body()->numJoints(); if(numJoints != prevNumJoints){ protoItem->jointPosSeq()->setNumParts(numJoints, true); } } return true; }
void OnlineViewerServerImpl::registerBodyItem(BodyItemPtr bodyItem) { BodyItemInfo info; info.bodyItem = bodyItem; info.needToSelectLogItem = true; info.bodyItemConnections.add( bodyItem->sigNameChanged().connect( boost::bind(&OnlineViewerServerImpl::onBodyItemNameChanged, this, bodyItem.get(), _1))); info.bodyItemConnections.add( bodyItem->sigDisconnectedFromRoot().connect( boost::bind(&OnlineViewerServerImpl::onBodyItemDetachedFromRoot, this, bodyItem.get()))); bodyItemInfoMap.insert(make_pair(bodyItem->name(), info)); }
void OnlineViewerServerImpl::load(string name, string url) { string filepath; QRegExp filePattern("(\\w+)://(.+)"); if(filePattern.exactMatch(url.c_str())){ string protocol = filePattern.cap(1).toStdString(); if(protocol == "file"){ filepath = filePattern.cap(2).toStdString(); } else { mv->putln( fmt(_("OnlineViewer: The model file at \"%1%\" cannot be read. %2% protocol is not supported.")) % url % protocol); return; } } else { filepath = url; } // search for registered body items BodyItemInfo* info = findInfo(name); if(info && info->bodyItem->filePath() == filepath){ info->needToSelectLogItem = true; // mv->putln(fmt(_("OnlineViewer: \"%1%\" at \"%2%\" has already been loaded.")) % name % url); return; } // search for existing body items RootItem* rootItem = RootItem::instance(); ItemList<BodyItem> bodyItems; bodyItems.extractChildItems(rootItem); for(int i=0; i < bodyItems.size(); ++i){ BodyItemPtr bodyItem = bodyItems[i]; if(bodyItem->name() == name && bodyItem->filePath() == filepath){ registerBodyItem(bodyItem); return; } } // load a new body item BodyItemPtr bodyItem = new BodyItem(); mv->putln(fmt(_("OnlineViewer: Loading \"%1%\" at \"%2%\".")) % name % url); mv->flush(); if(!bodyItem->load(filepath)){ mv->putln(fmt(_("OnlineViewer: Loading \"%1%\" failed.")) % name); } else { bodyItem->setName(name); ItemList<WorldItem> worldItems; if(worldItems.extractChildItems(rootItem)){ worldItems.front()->addChildItem(bodyItem); } else { rootItem->addChildItem(bodyItem); } ItemTreeView::instance()->checkItem(bodyItem, true); registerBodyItem(bodyItem); } }
bool WorldRosItem::spawnModel(gazebo_msgs::SpawnModel::Request &req, gazebo_msgs::SpawnModel::Response &res) { std::string model_name = req.model_name; std::string model_xml = req.model_xml; cnoid::Vector3 trans; cnoid::Quat R; trans(0) = req.initial_pose.position.x; trans(1) = req.initial_pose.position.y; trans(2) = req.initial_pose.position.z; R.w() = req.initial_pose.orientation.w; R.x() = req.initial_pose.orientation.x; R.y() = req.initial_pose.orientation.y; R.z() = req.initial_pose.orientation.z; BodyItemPtr body = new BodyItem(); body->setName(req.model_name); const char *fname = tmpnam(NULL); std::ofstream ofs(fname); ofs << model_xml << std::endl; ofs.close(); body->loadModelFile(fname); remove(fname); body->body()->rootLink()->setTranslation(trans); body->body()->rootLink()->setRotation(R.matrix()); world->addChildItem(body); BodyRosItemPtr bodyros = new BodyRosItem(); bodyros->setName(req.model_name + "_ROS"); body->addChildItem(bodyros); ItemTreeView::instance()->checkItem(body); ItemTreeView::instance()->checkItem(bodyros); return true; }
EditableSceneBody::EditableSceneBody(BodyItemPtr bodyItem) : SceneBody(bodyItem->body(), createEditableSceneLink) { setName(body()->name()); impl = new EditableSceneBodyImpl(this, bodyItem); }
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); }
bool isEditable() { return bodyItem->isEditable() && (!bodyItem->body()->isStaticModel() || enableStaticModelEditCheck->isChecked()); }