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);
}
}
}
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;
}
}
}
}
