void WorldItemImpl::updateCollisions(bool forceUpdate) { for(BodyItemInfoMap::iterator p = bodyItemInfoMap.begin(); p != bodyItemInfoMap.end(); ++p){ BodyItemInfo& info = p->second; BodyItem* bodyItem = p->first; bodyItem->clearCollisions(); if(info.kinematicStateChanged || forceUpdate){ const BodyPtr& body = bodyItem->body(); for(int i=0; i < body->numLinks(); ++i){ collisionDetector->updatePosition(info.geometryId + i, body->link(i)->T()); } } info.kinematicStateChanged = false; } collisions->clear(); collisionDetector->detectCollisions(std::bind(&WorldItemImpl::extractCollisions, this, _1)); sceneCollision->setDirty(); for(BodyItemInfoMap::iterator p = bodyItemInfoMap.begin(); p != bodyItemInfoMap.end(); ++p){ BodyItem* bodyItem = p->first; BodyItemInfo& info = p->second; bodyItem->notifyCollisionUpdate(); } sigCollisionsUpdated(); }
void WorldRosItem::publishModelStates() { gazebo_msgs::ModelStates model_states; Item* item = world->childItem(); while(item) { BodyItem* body = boost::dynamic_pointer_cast<BodyItem>(item); if (body) { model_states.name.push_back(body->name()); Link* link = body->body()->rootLink(); Vector3 pos = link->translation(); Quat rot = Quat(link->rotation()); geometry_msgs::Pose pose; pose.position.x = pos(0); pose.position.y = pos(1); pose.position.z = pos(2); pose.orientation.w = rot.w(); pose.orientation.x = rot.x(); pose.orientation.y = rot.y(); pose.orientation.z = rot.z(); model_states.pose.push_back(pose); /* twist.linear.x = linear_vel.x; twist.linear.y = linear_vel.y; twist.linear.z = linear_vel.z; twist.angular.x = angular_vel.x; twist.angular.y = angular_vel.y; twist.angular.z = angular_vel.z; model_states.twist.push_back(twist); */ } item = item->nextItem(); } pub_model_states_.publish(model_states); }
bool EditableSceneBodyImpl::storeProperties(Archive& archive) { ListingPtr states = new Listing(); ItemList<BodyItem> bodyItems; bodyItems.extractChildItems(RootItem::instance()); for(size_t i=0; i < bodyItems.size(); ++i){ BodyItem* bodyItem = bodyItems[i]; EditableSceneBody* sceneBody = bodyItem->existingSceneBody(); if(sceneBody){ ValueNodePtr id = archive.getItemId(bodyItem); if(id){ EditableSceneBodyImpl* impl = sceneBody->impl; MappingPtr state = new Mapping(); state->insert("bodyItem", id); state->write("showCenterOfMass", impl->isCmVisible); state->write("showZmp", impl->isZmpVisible); states->append(state); } } } if(!states->empty()){ archive.insert("editableSceneBodies", states); return true; } return false; }
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; } } } }
/** \todo reduce the number of calling this function when the project is loaded. */ void WorldItemImpl::updateCollisionDetector(bool forceUpdate) { if(TRACE_FUNCTIONS){ os << "WorldItemImpl::updateCollisionDetector()" << endl; } if(!isCollisionDetectionEnabled){ return; } if(forceUpdate){ updateColdetBodyInfos(coldetBodyInfos); } else { vector<ColdetBodyInfo> infos; updateColdetBodyInfos(infos); if(infos.size() == coldetBodyInfos.size()){ if(std::equal(infos.begin(), infos.end(), coldetBodyInfos.begin(), [](ColdetBodyInfo& info1, ColdetBodyInfo& info2){ return (info1.bodyItem == info2.bodyItem && info1.isSelfCollisionDetectionEnabled == info2.isSelfCollisionDetectionEnabled); })){ return; // not changed } } coldetBodyInfos = infos; } clearCollisionDetector(); for(auto& bodyInfo : coldetBodyInfos){ BodyItem* bodyItem = bodyInfo.bodyItem; bodyCollisionDetector.addBody( bodyItem->body(), bodyInfo.isSelfCollisionDetectionEnabled, [&bodyInfo](Link* link, GeometryHandle geometry){ ColdetLinkInfo* linkInfo = new ColdetLinkInfo(bodyInfo.bodyItem, link, geometry); bodyInfo.linkInfos.push_back(linkInfo); return linkInfo; }); ColdetBodyInfo* pBodyInfo = &bodyInfo; sigKinematicStateChangedConnections.add( bodyItem->sigKinematicStateChanged().connect( [&, pBodyInfo](){ pBodyInfo->kinematicStateChanged = true; updateCollisionsLater.setPriority(kinematicsBar->collisionDetectionPriority()); updateCollisionsLater(); })); } bodyCollisionDetector.makeReady(); updateCollisions(true); }
void EditableSceneBodyImpl::restoreProperties(const Archive& archive) { Listing& states = *archive["editableSceneBodies"].toListing(); for(int i=0; i < states.size(); ++i){ Mapping* state = states[i].toMapping(); BodyItem* bodyItem = archive.findItem<BodyItem>(state->find("bodyItem")); if(bodyItem){ EditableSceneBodyImpl* impl = bodyItem->sceneBody()->impl; impl->showCenterOfMass(state->get("showCenterOfMass", impl->isCmVisible)); impl->showZmp(state->get("showZmp", impl->isZmpVisible)); } } }
void BodyItemImpl::createPenetrationBlocker(Link* link, bool excludeSelfCollisions, PenetrationBlockerPtr& blocker) { WorldItem* worldItem = self->findOwnerItem<WorldItem>(); if(worldItem){ blocker = boost::make_shared<PenetrationBlocker>(worldItem->collisionDetector()->clone(), link); const ItemList<BodyItem>& bodyItems = worldItem->bodyItems(); for(int i=0; i < bodyItems.size(); ++i){ BodyItem* bodyItem = bodyItems.get(i); if(bodyItem != self && bodyItem->body()->isStaticModel()){ blocker->addOpponentLink(bodyItem->body()->rootLink()); } } blocker->setDepth(kinematicsBar->penetrationBlockDepth()); blocker->start(); } }
void LinkPropertyViewImpl::updateProperties() { clear(); BodyItem* bodyItem = linkSelectionView->currentBodyItem(); if(bodyItem){ Body* body = bodyItem->body(); int linkIndex = linkSelectionView->selectedLinkIndex(); if(linkIndex >= 0){ Link* link = body->link(linkIndex); if(link){ updateLinkProperties(link); } } } }
/** \todo reduce the number of calling this function when the project is loaded. */ void WorldItemImpl::updateCollisionDetector(bool forceUpdate) { if(TRACE_FUNCTIONS){ os << "WorldItemImpl::updateCollisionDetector()" << endl; } if(!isCollisionDetectionEnabled){ return; } if(!forceUpdate){ ItemList<BodyItem> prevBodyItems = collisionBodyItems; boost::dynamic_bitset<> prevSelfCollisionFlags = collisionBodyItemsSelfCollisionFlags; updateCollisionBodyItems(); if(collisionBodyItems == prevBodyItems && collisionBodyItemsSelfCollisionFlags == prevSelfCollisionFlags){ return; } } else { updateCollisionBodyItems(); } clearCollisionDetector(); for(size_t i=0; i < collisionBodyItems.size(); ++i){ BodyItem* bodyItem = collisionBodyItems.get(i); const BodyPtr& body = bodyItem->body(); const int numLinks = body->numLinks(); pair<BodyItemInfoMap::iterator, bool> inserted = bodyItemInfoMap.insert(make_pair(bodyItem, BodyItemInfo())); BodyItemInfo& info = inserted.first->second; info.geometryId = addBodyToCollisionDetector( *body, *collisionDetector, bodyItem->isSelfCollisionDetectionEnabled()); geometryIdToBodyInfoMap.resize(collisionDetector->numGeometries(), inserted.first); sigKinematicStateChangedConnections.add( bodyItem->sigKinematicStateChanged().connect( std::bind(&WorldItemImpl::onBodyKinematicStateChanged, this, bodyItem))); } collisionDetector->makeReady(); updateCollisions(true); }
BodyInfo(BodyItem* bodyItem){ this->bodyItem = bodyItem; if(bodyItem){ body = bodyItem->body(); deviceInfos.resize(body->numDevices()); } else { body = 0; } }
void LeggedBodyBarImpl::moveCM(BodyItem::PositionType position) { const ItemList<BodyItem>& targetBodyItems = bodyBar->targetBodyItems(); for(size_t i=0; i < targetBodyItems.size(); ++i){ BodyItem* bodyItem = targetBodyItems[i]; Vector3 c = bodyItem->centerOfMass(); boost::optional<Vector3> p = bodyItem->getParticularPosition(position); if(p){ c[0] = (*p)[0]; c[1] = (*p)[1]; } if(!bodyItem->doLegIkToMoveCm(c, true)){ MessageView::instance()->notify( fmt::format(_("The center of mass of {} cannt be moved to the target position\n"), bodyItem->name())); } } }
void WorldItemImpl::extractCollisions(const CollisionPair& collisionPair) { CollisionLinkPairPtr collisionLinkPair = std::make_shared<CollisionLinkPair>(); collisionLinkPair->collisions = collisionPair.collisions(); BodyItem* bodyItem = 0; for(int i=0; i < 2; ++i){ ColdetLinkInfo* linkInfo = static_cast<ColdetLinkInfo*>(collisionPair.object(i)); if(linkInfo->bodyItem != bodyItem){ bodyItem = linkInfo->bodyItem; bodyItem->collisions().push_back(collisionLinkPair); } collisionLinkPair->body[i] = bodyItem->body(); Link* link = linkInfo->link; collisionLinkPair->link[i] = link; bodyItem->collisionsOfLink(link->index()).push_back(collisionLinkPair); bodyItem->collisionLinkBitSet().set(link->index()); } collisions->push_back(collisionLinkPair); }
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 WorldItemImpl::extractCollisions(const CollisionPair& collisionPair) { CollisionLinkPairPtr collisionLinkPair = std::make_shared<CollisionLinkPair>(); collisionLinkPair->collisions = collisionPair.collisions; BodyItem* bodyItem = 0; for(int i=0; i < 2; ++i){ const int geometryId = collisionPair.geometryId[i]; BodyItemInfoMap::const_iterator p = geometryIdToBodyInfoMap[geometryId]; const BodyItemInfo& info = p->second; const int linkIndex = geometryId - info.geometryId; if(p->first != bodyItem){ bodyItem = p->first; bodyItem->collisions().push_back(collisionLinkPair); } const BodyPtr& body = bodyItem->body(); collisionLinkPair->body[i] = body; collisionLinkPair->link[i] = body->link(linkIndex); bodyItem->collisionsOfLink(linkIndex).push_back(collisionLinkPair); bodyItem->collisionLinkBitSet().set(linkIndex); } collisions->push_back(collisionLinkPair); }
void BodyItemImpl::doAssign(Item* srcItem) { BodyItem* srcBodyItem = dynamic_cast<BodyItem*>(srcItem); if(srcBodyItem){ // copy the base link property Link* baseLink = 0; Link* srcBaseLink = srcBodyItem->currentBaseLink(); if(srcBaseLink){ baseLink = body->link(srcBaseLink->name()); if(baseLink){ setCurrentBaseLink(baseLink); } } // copy the current kinematic state Body* srcBody = srcBodyItem->body(); for(int i=0; i < srcBody->numLinks(); ++i){ Link* srcLink = srcBody->link(i); Link* link = body->link(srcLink->name()); if(link){ link->q() = srcLink->q(); } } if(baseLink){ baseLink->p() = srcBaseLink->p(); baseLink->R() = srcBaseLink->R(); } else { body->rootLink()->p() = srcBody->rootLink()->p(); body->rootLink()->R() = srcBody->rootLink()->R(); } zmp = srcBodyItem->impl->zmp; initialState = srcBodyItem->impl->initialState; self->notifyKinematicStateChange(true); } }