Beispiel #1
0
void PoseSeqItem::onPositionChanged()
{
    if(!sigInterpolationParametersChangedConnection.connected()){
        sigInterpolationParametersChangedConnection =
            generationBar->sigInterpolationParametersChanged().connect(
                std::bind(&PoseSeqItem::updateInterpolationParameters, this));
        updateInterpolationParameters();
    }

    BodyItemPtr prevBodyItem = ownerBodyItem;
    ownerBodyItem = findOwnerItem<BodyItem>();
    if(ownerBodyItem == prevBodyItem){
        return;
    }
        
    if(!ownerBodyItem){
        interpolator_->setBody(0);

    } else {
        Body* body = ownerBodyItem->body();

        if(seq->targetBodyName().empty()){
            seq->setTargetBodyName(body->name());
        } else if(prevBodyItem && (seq->targetBodyName() != body->name())){
            convert(prevBodyItem->body());
        }

        interpolator_->setBody(body);

        const Listing& linearInterpolationJoints =
            *ownerBodyItem->body()->info()->findListing("linearInterpolationJoints");
        if(linearInterpolationJoints.isValid()){
            for(int i=0; i < linearInterpolationJoints.size(); ++i){
                Link* link = body->link(linearInterpolationJoints[i].toString());
                if(link){
                    interpolator_->setLinearInterpolationJoint(link->jointId());
                }
            }
        }

        LeggedBodyHelperPtr legged = getLeggedBodyHelper(ownerBodyItem->body());
        if(legged->isValid()){
            for(int i=0; i < legged->numFeet(); ++i){
                interpolator_->addFootLink(legged->footLink(i)->index(), legged->centerOfSoleLocal(i));
            }
        }

        interpolator_->setLipSyncShapes(*ownerBodyItem->body()->info()->findMapping("lipSyncShapes"));
        bodyMotionItem_->motion()->setNumJoints(interpolator_->body()->numJoints());

        if(generationBar->isAutoGenerationForNewBodyEnabled()){
            updateTrajectory(true);
        } else {
            bodyMotionItem_->notifyUpdate();
        }
    }
}
bool WorldRosItem::spawnModel(gazebo_msgs::SpawnModel::Request &req,
                              gazebo_msgs::SpawnModel::Response &res)
{
  std::string model_name = req.model_name;
  std::string model_xml = req.model_xml;

  cnoid::Vector3 trans;
  cnoid::Quat R;
  trans(0) = req.initial_pose.position.x;
  trans(1) = req.initial_pose.position.y;
  trans(2) = req.initial_pose.position.z;
  R.w() = req.initial_pose.orientation.w;
  R.x() = req.initial_pose.orientation.x;
  R.y() = req.initial_pose.orientation.y;
  R.z() = req.initial_pose.orientation.z;

  BodyItemPtr body = new BodyItem();
  body->setName(req.model_name);
  
  const char *fname = tmpnam(NULL);
  std::ofstream ofs(fname);
  ofs << model_xml << std::endl;
  ofs.close();
  body->loadModelFile(fname);
  remove(fname);
  
  body->body()->rootLink()->setTranslation(trans);
  body->body()->rootLink()->setRotation(R.matrix());
  world->addChildItem(body);

  BodyRosItemPtr bodyros = new BodyRosItem();
  bodyros->setName(req.model_name + "_ROS");
  body->addChildItem(bodyros);
  
  ItemTreeView::instance()->checkItem(body);
  ItemTreeView::instance()->checkItem(bodyros);
  
  return true;
}
static bool bodyMotionItemPreFilter(BodyMotionItem* protoItem, Item* parentItem)
{
    BodyItemPtr bodyItem = dynamic_cast<BodyItem*>(parentItem);
    if(!bodyItem){
        bodyItem = parentItem->findOwnerItem<BodyItem>();
    }
    if(bodyItem){
        int prevNumJoints = protoItem->jointPosSeq()->numParts();
        int numJoints = bodyItem->body()->numJoints();
        if(numJoints != prevNumJoints){
            protoItem->jointPosSeq()->setNumParts(numJoints, true);
        }
    }
    return true;
}
EditableSceneBody::EditableSceneBody(BodyItemPtr bodyItem)
    : SceneBody(bodyItem->body(), createEditableSceneLink)
{
    setName(body()->name());
    impl = new EditableSceneBodyImpl(this, bodyItem);
}
bool BodyUnit::initialize(BodyItemPtr bodyItem)
{
    if(TRACE_FUNCTIONS){
        cout << "BodyUnit::initialize()" << endl;
    }

    if(bodyItem->body()->isStaticModel()){
        return false;
    }
        
    frame = 0;
    body = bodyItem->body()->duplicate();
    baseLink = 0;

    ItemList<BodyMotionItem> motionItems = bodyItem->getSubItems<BodyMotionItem>();
    if(!motionItems.empty()){
        BodyMotionItemPtr motionItem;
        // prefer the first checked item
        for(size_t i=0; i < motionItems.size(); ++i){
            if(ItemTreeView::mainInstance()->isItemChecked(motionItems[i])){
                motionItem = motionItems[i];
                break;
            }
        }
        if(!motionItem){
            motionItem = motionItems[0];
        }
        
        frameRate = motionItem->motion()->frameRate();
        qseqRef = motionItem->jointPosSeq();
        if(qseqResult){
            qseqResultBuf.reset(new MultiValueSeq(qseqResult->numParts(), 0, frameRate));
        }

        if(motionItem->hasRelativeZmpSeqItem()){
            zmpSeq = motionItem->relativeZmpSeq();
        }

        rootResultItem = motionItem->linkPosSeqItem();
        rootResultBuf.reset(new MultiAffine3Seq(1, 0, frameRate));
        rootResult = motionItem->linkPosSeq();
        rootResult->setFrameRate(frameRate);
        rootResult->setDimension(1, 1);
    }

    const YamlMapping& info = *bodyItem->body()->info();
    const YamlSequence& footLinkInfos = *info.findSequence("footLinks");
    if(footLinkInfos.isValid()){
        for(int i=0; i < footLinkInfos.size(); ++i){
            const YamlMapping& footLinkInfo = *footLinkInfos[i].toMapping();
            Link* link = body->link(footLinkInfo["link"].toString());
            Vector3 soleCenter;
            if(link && read(footLinkInfo, "soleCenter", soleCenter)){
                footLinks.push_back(link);
                footLinkOriginHeights.push_back(-soleCenter[2]);
            }
        }
    }

    if(!footLinks.empty()){
        if(zmpSeq){
            setBaseLinkByZmp(0);
        } else {
            if(bodyItem->currentBaseLink()){
                int currentBaseLinkIndex = bodyItem->currentBaseLink()->index;
                for(size_t i=0; i < footLinks.size(); ++i){
                    Link* footLink = footLinks[i];
                    if(footLink->index == currentBaseLinkIndex){
                        setBaseLink(footLink, footLinkOriginHeights[i]);
                    }
                }
            } else{
                setBaseLink(footLinks[0], footLinkOriginHeights[0]);
            }
        }
    }

    if(!baseLink){
        if(bodyItem->currentBaseLink()){
            baseLink = body->link(bodyItem->currentBaseLink()->index);
        } else {
            baseLink = body->rootLink();
        }
        fkTraverse.find(baseLink, true, true);
    }

    if(rootResult){
        Link* rootLink = body->rootLink();
        Affine3& initialPos = rootResult->at(0, 0);
        initialPos.translation() = rootLink->p;
        initialPos.linear() = rootLink->R;
    }

    return (qseqRef != 0);
}
 bool isEditable() {
     return bodyItem->isEditable() &&
         (!bodyItem->body()->isStaticModel() || enableStaticModelEditCheck->isChecked());
 }