bool MultiAffine3Seq::doWriteSeq(YAMLWriter& writer)
{
    if(BaseSeqType::doWriteSeq(writer)){

        writer.putKeyValue("format", "XYZRPY");
    
        writer.putKey("frames");
        writer.startListing();
        const int m = numParts();
        const int n = numFrames();
        for(int i=0; i < n; ++i){
            Frame f = frame(i);
            writer.startFlowStyleListing();
            for(int j=0; j < m; ++j){
                writeAffine3(writer, f[j]);
            }
            writer.endListing();
        }
        writer.endListing();

        return true;
    }
    
    return false;
}
Beispiel #2
0
bool BodyMotion::write(YAMLWriter& writer)
{
    writer.startListing();

    if(jointPosSeq_->numFrames() > 0){
        if(!jointPosSeq_->writeSeq(writer)){
            addSeqMessage(jointPosSeq_->seqMessage());
            return false;
        }
    }
    if(linkPosSeq_->numFrames() > 0){
        if(!linkPosSeq_->writeSeq(writer)){
            addSeqMessage(linkPosSeq_->seqMessage());
            return false;
        }
    }

    for(ExtraSeqMap::iterator p = extraSeqs.begin(); p != extraSeqs.end(); ++p){
        AbstractSeqPtr& seq = p->second;
        if(!seq->writeSeq(writer)){
            addSeqMessage(seq->seqMessage());
            return false;
        }
    }

    writer.endListing();

    return true;
}
Beispiel #3
0
bool Vector3Seq::doWriteSeq(YAMLWriter& writer)
{
    if(BaseSeqType::doWriteSeq(writer)){
        writer.putKey("frames");
        writer.startListing();
        const int n = numFrames();
        for(int i=0; i < n; ++i){
            writer.startFlowStyleListing();
            const Vector3& v = (*this)[i];
            for(int j=0; j < 3; ++j){
                writer.putScalar(v[j]);
            }
            writer.endListing();
        }
        writer.endListing();
        return true;
    }
    return false;
}
Beispiel #4
0
bool AbstractMultiSeq::writeSeqPartLabels(YAMLWriter& writer)
{
    writer.putKey("partLabels");
    writer.startFlowStyleListing();
    int n = getNumParts();
    for(int i=0; i < n; ++i){
        writer.putDoubleQuotedString(partLabel(i));
    }
    writer.endListing();
    return true;
}
bool MultiValueSeq::doWriteSeq(YAMLWriter& writer, std::function<void()> additionalPartCallback)
{
    return BaseSeqType::doWriteSeq(
        writer,
        [&](){
            if(additionalPartCallback) additionalPartCallback();
            
            writer.putKey("frames");
            writer.startListing();
            const int n = numFrames();
            const int m = numParts();
            for(int i=0; i < n; ++i){
                writer.startFlowStyleListing();
                Frame v = frame(i);
                for(int j=0; j < m; ++j){
                    writer.putScalar(v[j]);
                }
                writer.endListing();
            }
            writer.endListing();
        });
}
Beispiel #6
0
void CollisionSeq::writeCollsionData(YAMLWriter& writer, std::shared_ptr<const CollisionLinkPairList> ptr)
{
    writer.startMapping();
    writer.putKey("LinkPairs");

    writer.startListing();
    for(auto it = ptr->begin(); it != ptr->end(); ++it){
        CollisionLinkPairPtr linkPair = *it;
        writer.startMapping();
        writer.putKeyValue("body0",linkPair->body[0]->name());
        writer.putKeyValue("link0",linkPair->link[0]->name());
        writer.putKeyValue("body1",linkPair->body[1]->name());
        writer.putKeyValue("link1",linkPair->link[1]->name());
        int numCollisions = linkPair->collisions.size();
        writer.putKey("Collisions");
        writer.startListing();
        for(int j=0; j<numCollisions; j++){
            Collision& collision = linkPair->collisions[j];
            writer.startFlowStyleListing();
            const Vector3& point = collision.point;
            writer.putScalar(point.x());
            writer.putScalar(point.y());
            writer.putScalar(point.z());
            const Vector3& normal = collision.normal;
            writer.putScalar(normal.x());
            writer.putScalar(normal.y());
            writer.putScalar(normal.z());
            writer.putScalar(collision.depth);
            writer.endListing();
        }
        writer.endListing();
        writer.endMapping();
        }
    writer.endListing();

    writer.endMapping();
}
static inline void writeAffine3(YAMLWriter& writer, const Affine3& value)
{
    writer.startFlowStyleListing();

    writer.putScalar(value.translation().x());
    writer.putScalar(value.translation().y());
    writer.putScalar(value.translation().z());

    Vector3 rpy(rpyFromRot(value.linear()));
    writer.putScalar(rpy[0]);
    writer.putScalar(rpy[1]);
    writer.putScalar(rpy[2]);

    writer.endListing();
}
Beispiel #8
0
static inline void writeSE3(YAMLWriter& writer, const SE3& value)
{
    writer.startFlowStyleListing();

    const Vector3& p = value.translation();
    writer.putScalar(p.x());
    writer.putScalar(p.y());
    writer.putScalar(p.z());

    const Quat& q = value.rotation();
    writer.putScalar(q.w());
    writer.putScalar(q.x());
    writer.putScalar(q.y());
    writer.putScalar(q.z());

    writer.endListing();
}
Beispiel #9
0
bool CollisionSeq::doWriteSeq(YAMLWriter& writer, std::function<void()> additionalPartCallback)
{
    return BaseSeqType::doWriteSeq(
        writer,
        [&](){
            writer.putKeyValue("format", "PxPyPzNxNyNzD");

            if(additionalPartCallback) additionalPartCallback();
            
            writer.putKey("frames");
            writer.startListing();
            const int n = numFrames();
            for(int i=0; i < n; ++i){
                Frame f = frame(i);
                writeCollsionData(writer, f[0]);
            }
            writer.endListing();
        });
}