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();// ロボットモデルは１個のみチェック
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);
}
}
}
