bool MultiSE3Seq::doReadSeq(const Mapping& archive) { if(BaseSeqType::doReadSeq(archive)){ const string& type = archive["type"].toString(); if(type == seqType() || type == "MultiSe3Seq" || type == "MultiAffine3Seq"){ string formatString = archive["format"].toString(); const Listing& values = *archive.findListing("frames"); if(values.isValid()){ const int nParts = archive["numParts"].toInt(); const int nFrames = values.size(); setDimension(nFrames, nParts); if(formatString == "XYZQWQXQYQZ"){ readPosQuatSeq(nParts, nFrames, values, true); } else if(formatString == "XYZQXQYQZQW"){ readPosQuatSeq(nParts, nFrames, values, false); } else if(formatString == "XYZRPY"){ readPosRpySeq(nParts, nFrames, values); } else { addSeqMessage( str(format(("Unknown format \"%1%\" cannot be loaded into MultiSE3Seq.")) % formatString)); return false; } } } return true; } return false; }
bool MultiAffine3Seq::doReadSeq(const Mapping& archive) { if(BaseSeqType::doReadSeq(archive)){ const string& type = archive["type"].toString(); if((type == seqType() || type == "MultiSe3Seq") && (archive["format"].toString() == "XYZRPY")){ const Listing& values = *archive.findListing("frames"); if(values.isValid()){ const int nParts = archive["numParts"].toInt(); const int nFrames = values.size(); setDimension(nFrames, nParts); for(int i=0; i < nFrames; ++i){ const Listing& frameNode = *values[i].toListing(); Frame f = frame(i); const int n = std::min(frameNode.size(), nParts); for(int j=0; j < n; ++j){ readAffine3(*frameNode[j].toListing(), f[j]); } } return true; } } } return false; }
bool PoseSeq::restore(const Mapping& archive, const BodyPtr body) { setTargetBodyName(archive.get("targetBody", body->name())); const Listing& refs = *archive.findListing("refs"); if(!refs.isValid()){ return false; } PoseSeq::iterator current = begin(); for(int i=0; i < refs.size(); ++i){ const Mapping& ref = *refs[i].toMapping(); bool isInserted = false; double time = ref["time"].toDouble(); const ValueNode& referred = ref["refer"]; if(referred.isScalar()){ const string& name = referred; if(!name.empty()){ current = insert(current, time, name); isInserted = true; } } else if(referred.isMapping()){ const Mapping& mReferred = *referred.toMapping(); const string& type = mReferred["type"]; PoseUnitPtr poseUnit; if(type == "Pose"){ poseUnit = new Pose(); } else if(type == "PronunSymbol"){ poseUnit = new PronunSymbol(); } /* else if(type == "PoseSeq"){ poseUnit = createLocalPoseSeq(); } */ if(poseUnit && poseUnit->restore(mReferred, body)){ poseUnit->name_ = mReferred["name"]; current = insert(current, time, poseUnit); isInserted = true; } } if(isInserted){ current->setMaxTransitionTime(ref.get("maxTransitionTime", 0.0)); } } return true; }
std::vector<std::string> AbstractMultiSeq::readSeqPartLabels(const Mapping& archive) { vector<string> labelStrings; const Listing& labels = *archive.findListing("partLabels"); if(labels.isValid()){ for(int i=0; i < labels.size(); ++i){ labelStrings.push_back(labels[i].toString()); } } return labelStrings; }
bool AbstractMultiSeq::readSeqPartLabels(const Mapping& archive, SetPartLabelFunction setPartLabel) { const Listing& labels = *archive.findListing("partLabels"); if(labels.isValid()){ for(int i=0; i < labels.size(); ++i){ setPartLabel(labels[i].toString(), i); } return true; } else { addSeqMessage("\"partLabels\" is not valid."); } return false; }
bool Vector3Seq::doReadSeq(const Mapping& archive) { if(BaseSeqType::doReadSeq(archive)){ if(archive["type"].toString() == seqType()){ const Listing& frames = *archive.findListing("frames"); if(!frames.isValid()){ addSeqMessage(_("Valid \"frames\" field of Vector3Seq is not found.")); } else { const int n = frames.size(); setNumFrames(n); for(int i=0; i < n; ++i){ const Listing& frame = *frames[i].toListing(); Vector3& v = (*this)[i]; for(int j=0; j < 3; ++j){ v[j] = frame[j].toDouble(); } } return true; } } } return false; }
bool PoseSeqItem::convertSub(BodyPtr orgBody, const Mapping& convInfo) { bool converted = false; const Listing& jointMap = *convInfo.findListing("jointMap"); const Mapping& linkMap = *convInfo.findMapping("linkMap"); BodyPtr body = ownerBodyItem->body(); for(PoseSeq::iterator p = seq->begin(); p != seq->end(); ++p){ PosePtr pose = p->get<Pose>(); if(pose){ bool modified = false; seq->beginPoseModification(p); PosePtr orgPose = dynamic_cast<Pose*>(pose->duplicate()); if(jointMap.isValid()){ modified = true; pose->setNumJoints(0); int n = orgPose->numJoints(); for(int i=0; i < n; ++i){ if(orgPose->isJointValid(i)){ if(i < jointMap.size()){ int newJointId = jointMap[i].toInt(); if(newJointId >= 0){ pose->setJointPosition(newJointId, orgPose->jointPosition(i)); pose->setJointStationaryPoint(newJointId, orgPose->isJointStationaryPoint(i)); } } } } } if(linkMap.isValid()){ modified = true; pose->clearIkLinks(); int baseLinkIndex = -1; for(Pose::LinkInfoMap::const_iterator q = orgPose->ikLinkBegin(); q != orgPose->ikLinkEnd(); ++q){ Link* orgLink = orgBody->link(q->first); string linkName; ValueNode* linkNameNode = linkMap.find(orgLink->name()); if(linkNameNode->isValid()){ linkName = linkNameNode->toString(); } else { linkName = orgLink->name(); } Link* link = body->link(linkName); if(link){ const Pose::LinkInfo& orgLinkInfo = q->second; Pose::LinkInfo* linkInfo = pose->addIkLink(link->index()); linkInfo->p = orgLinkInfo.p; linkInfo->R = orgLinkInfo.R; linkInfo->setStationaryPoint(orgLinkInfo.isStationaryPoint()); if(orgLinkInfo.isTouching()){ linkInfo->setTouching(orgLinkInfo.partingDirection()); } linkInfo->setSlave(orgLinkInfo.isSlave()); if(orgLinkInfo.isBaseLink()){ baseLinkIndex = link->index(); } } } if(baseLinkIndex >= 0){ pose->setBaseLink(baseLinkIndex); } } if(modified){ seq->endPoseModification(p); converted = true; } } } return converted; }