Exemplo n.º 1
0
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();
}
Exemplo n.º 2
0
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);
}
Exemplo n.º 3
0
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;
            }
        }
    }
}
Exemplo n.º 5
0
/**
   \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);
}
Exemplo n.º 6
0
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));
        }
    }
}
Exemplo n.º 7
0
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();
    }
}
Exemplo n.º 8
0
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);
            }
        }
    }
}
Exemplo n.º 9
0
/**
   \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);
}
Exemplo n.º 10
0
 BodyInfo(BodyItem* bodyItem){
     this->bodyItem = bodyItem;
     if(bodyItem){
         body = bodyItem->body();
         deviceInfos.resize(body->numDevices());
     } else {
         body = 0;
     }
 }
Exemplo n.º 11
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()));
        }
    }
}
Exemplo n.º 12
0
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);
        }
    }
}
Exemplo n.º 14
0
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);
}
Exemplo n.º 15
0
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);
    }
}