void OnlineViewerServerImpl::updateLog (BodyItemInfo* info, const LinkPositionSequence& links, int numLinks, double time) { BodyMotionItem* motionItem = info->logItem; if(!motionItem){ motionItem = info->bodyItem->findChildItem<BodyMotionItem>(info->logName); if(!motionItem){ motionItem = new BodyMotionItem(); motionItem->setName(info->logName); info->bodyItem->addChildItem(motionItem); } resetLogItem(info, motionItem); } if(info->needToSelectLogItem){ ItemTreeView::instance()->selectItem(motionItem, true); info->needToSelectLogItem = false; } MultiSE3Seq& seq = *motionItem->motion()->linkPosSeq(); int frame = seq.frameOfTime(time); int lastFrame = std::max(0, std::min(frame, seq.numFrames())); seq.setNumFrames(frame + 1); Body* body = info->bodyItem->body(); for(int i=lastFrame; i <= frame; ++i){ MultiSE3Seq::Frame positions = seq.frame(i); for(int j=0; j < numLinks; ++j){ SE3& se3 = positions[j]; se3.translation() = Eigen::Map<Vector3>(const_cast<double*>(links[j].p)); Matrix3 Rs = body->link(j)->Rs().transpose(); se3.rotation() = Eigen::Map<Matrix3>(const_cast<double*>(links[j].R)).transpose() * Rs; } } }
void KinematicFaultCheckerImpl::apply() { bool processed = false; ItemList<BodyMotionItem> items = ItemTreeView::mainInstance()->selectedItems<BodyMotionItem>(); if(items.empty()){ mes.notify(_("No BodyMotionItems are selected.")); } else { for(size_t i=0; i < items.size(); ++i){ BodyMotionItem* motionItem = items.get(i); BodyItem* bodyItem = motionItem->findOwnerItem<BodyItem>(); if(!bodyItem){ mes.notify(str(fmt(_("%1% is not owned by any BodyItem. Check skiped.")) % motionItem->name())); } else { mes.putln(); mes.notify(str(fmt(_("Applying the Kinematic Fault Checker to %1% ...")) % motionItem->headItem()->name())); dynamic_bitset<> linkSelection; if(selectedJointsRadio.isChecked()){ linkSelection = LinkSelectionView::mainInstance()->linkSelection(bodyItem); } else if(nonSelectedJointsRadio.isChecked()){ linkSelection = LinkSelectionView::mainInstance()->linkSelection(bodyItem); linkSelection.flip(); } else { linkSelection.resize(bodyItem->body()->numLinks(), true); } double beginningTime = 0.0; double endingTime = motionItem->motion()->getTimeLength(); std::numeric_limits<double>::max(); if(onlyTimeBarRangeCheck.isChecked()){ TimeBar* timeBar = TimeBar::instance(); beginningTime = timeBar->minTime(); endingTime = timeBar->maxTime(); } int n = checkFaults(bodyItem, motionItem, mes.cout(), positionCheck.isChecked(), velocityCheck.isChecked(), collisionCheck.isChecked(), linkSelection, beginningTime, endingTime); if(n > 0){ if(n == 1){ mes.notify(_("A fault has been detected.")); } else { mes.notify(str(fmt(_("%1% faults have been detected.")) % n)); } } else { mes.notify(_("No faults have been detected.")); } processed = true; } } } }
void HrpsysSequenceFileExportPlugin::HrpsysSequenceFileExport() { cout << "\x1b[31m" << "Start HrpsysSequenceFileExport" << "\x1b[m" << endl; // BodyItem,BodyPtr ItemList<BodyItem> bodyItems = ItemTreeView::mainInstance()->checkedItems<BodyItem>(); BodyPtr body = bodyItems[0]->body();// ロボットモデルは1個のみチェック LeggedBodyHelperPtr lgh = getLeggedBodyHelper(body); PoseSeqItem* poseSeqItem = ItemTreeView::mainInstance()->selectedItems<PoseSeqItem>()[0]; PoseSeqPtr poseSeq = poseSeqItem->poseSeq(); BodyMotionItem* bodyMotionItem = poseSeqItem->bodyMotionItem(); BodyMotionPtr motion = bodyMotionItem->motion(); string poseSeqPathString = poseSeqItem->filePath(); boost::filesystem::path poseSeqPath(poseSeqPathString); cout << " parent_path:" << poseSeqPath.parent_path().string() << " basename:" << getBasename(poseSeqPath) << endl; double dt = ((double) 1)/motion->frameRate(); int numFrames = motion->numFrames(); std::vector<string> extentionVec{"wrenches","optionaldata"}; for(auto ext : extentionVec){ MultiValueSeqItemPtr seqItem = bodyMotionItem->findSubItem<MultiValueSeqItem>(ext); if(!seqItem){ cout << ext << " is not found in PoseSeq: " << poseSeqItem->name() << endl; continue; } stringstream fnamess; fnamess << poseSeqItem->name() << "." << ext; ofstream ofs; ofs.open(((filesystem::path) poseSeqPath.parent_path() / fnamess.str()).string().c_str(), ios::out); MultiValueSeqPtr multiValueSeqPtr = seqItem->seq(); for(int i=0; i<numFrames; ++i){ ofs << i*dt; MultiValueSeq::Frame frame = multiValueSeqPtr->frame(i); for(int j=0; j<frame.size(); ++j){ ofs << " " << frame[j]; } ofs << endl; } ofs.close(); } cout << "\x1b[31m" << "Finished HrpsysSequenceFileExport" << "\x1b[m" << endl << endl; }
bool ZMPSeqItem::makeRootRelative(bool on) { BodyMotionItem* bodyMotionItem = dynamic_cast<BodyMotionItem*>(parentItem()); if(bodyMotionItem){ if(cnoid::makeRootRelative(*zmpseq_, *bodyMotionItem->motion(), on)){ mvout() << format(_("{0} of {1} has been converted to {2}."), name(), bodyMotionItem->name(), (on ? _("the root relative coordinate") : _("the global coordinate"))) << endl; return true; } } mvout() << format(_("{0}'s coordinate system cannot be changed " "because there is no root link motion associated with {0}."), name()) << endl; return false; }
void BodyMotionGenerationBar::onGenerationButtonClicked() { set<BodyMotionItem*> motionItems; // for avoiding overlap ItemList<Item> selectedItems = ItemTreeView::mainInstance()->selectedItems<Item>(); for(size_t i=0; i < selectedItems.size(); ++i){ PoseSeqItem* poseSeqItem = dynamic_cast<PoseSeqItem*>(selectedItems[i]); if(poseSeqItem){ motionItems.insert(poseSeqItem->bodyMotionItem()); } else { BodyMotionItem* motionItem = dynamic_cast<BodyMotionItem*>(selectedItems[i]); if(motionItem){ motionItems.insert(motionItem); } } } for(set<BodyMotionItem*>::iterator p = motionItems.begin(); p != motionItems.end(); ++p){ BodyMotionItem* motionItem = *p; BodyItem* bodyItem = motionItem->findOwnerItem<BodyItem>(true); if(bodyItem){ PoseProvider* provider = 0; PoseSeqItem* poseSeqItem = dynamic_cast<PoseSeqItem*>(motionItem->parentItem()); if(poseSeqItem){ provider = poseSeqItem->interpolator().get(); } else { bodyMotionPoseProvider->initialize(bodyItem->body(), motionItem->motion()); provider = bodyMotionPoseProvider; if(setup->newBodyItemCheck.isChecked()){ BodyMotionItem* newMotionItem = new BodyMotionItem(); newMotionItem->setName(motionItem->name() + "'"); motionItem->parentItem()->insertChildItem(newMotionItem, motionItem->nextItem()); motionItem = newMotionItem; } } shapeBodyMotion(bodyItem->body(), provider, motionItem, true); } } }