Ejemplo n.º 1
0
bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const
{
    // clear chain
    chain = Chain();

    // walk down from chain_root and chain_tip to the root of the tree
    vector<SegmentMap::key_type> parents_chain_root, parents_chain_tip;
    for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s=s->second.parent){
        parents_chain_root.push_back(s->first);
        if (s->first == root_name) break;
    }
    if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false;
    for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s=s->second.parent){
        parents_chain_tip.push_back(s->first);
        if (s->first == root_name) break;
    }
    if (parents_chain_tip.empty() || parents_chain_tip.back()  != root_name) return false;

    // remove common part of segment lists
    SegmentMap::key_type last_segment = root_name;
    while (!parents_chain_root.empty() && !parents_chain_tip.empty() &&
           parents_chain_root.back() == parents_chain_tip.back()){
        last_segment = parents_chain_root.back();
        parents_chain_root.pop_back();
        parents_chain_tip.pop_back();
    }
    parents_chain_root.push_back(last_segment);


    // add the segments from the root to the common frame
    for (unsigned int s=0; s<parents_chain_root.size()-1; s++){
        Segment seg = getSegment(parents_chain_root[s])->second.segment;
        Frame f_tip = seg.pose(0.0).Inverse();
        Joint jnt = seg.getJoint();
        if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis)
            jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::RotAxis);
        else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis)
            jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::TransAxis);
        chain.addSegment(Segment(getSegment(parents_chain_root[s+1])->second.segment.getName(),
                                 jnt, f_tip, getSegment(parents_chain_root[s+1])->second.segment.getInertia()));
    }

    // add the segments from the common frame to the tip frame
    for (int s=parents_chain_tip.size()-1; s>-1; s--){
        chain.addSegment(getSegment(parents_chain_tip[s])->second.segment);
    }
    return true;
}
COLLADASW::Joint KDLColladaLibraryJointsExporter::makeColladaSWJoint(COLLADASW::StreamWriter* streamWriter, Joint& kdlJoint, string uniqueId)
{


    string jointName = kdlJoint.getName();

    if (jointName == kdlDefaultJointName)
        jointName = uniqueId;
    Joint::JointType jointType = kdlJoint.getType();
    Vector jointAxis = kdlJoint.JointAxis();


    COLLADAFW::JointPrimitive::Type type;
    switch (jointType)
    {
        case Joint::RotAxis :
        {
            type = COLLADAFW::JointPrimitive::REVOLUTE;
            break;
        }
        case Joint::RotX :
        {
            type = COLLADAFW::JointPrimitive::REVOLUTE;
            break;
        }
        case Joint::RotY :
        {
            type = COLLADAFW::JointPrimitive::REVOLUTE;
            break;
        }
        case Joint::RotZ :
        {
            type = COLLADAFW::JointPrimitive::REVOLUTE;
            break;
        }

        case Joint::TransAxis :
        {
            type = COLLADAFW::JointPrimitive::PRISMATIC;
            break;
        }

        case Joint::TransX :
        {
            type = COLLADAFW::JointPrimitive::PRISMATIC;
            break;
        }

        case Joint::TransY :
        {
            type = COLLADAFW::JointPrimitive::PRISMATIC;
            break;
        }

        case Joint::TransZ :
        {
            type = COLLADAFW::JointPrimitive::PRISMATIC;
            break;
        }

        default :
            LOG(ERROR) << "Unknown joint type: " << kdlJoint.getTypeName() << ", changing to revolute type with no axis.";
            type = COLLADAFW::JointPrimitive::REVOLUTE;
    }

    COLLADABU::Math::Vector3 axis(jointAxis.x(), jointAxis.y(), jointAxis.z());

    COLLADAFW::JointPrimitive jointPrimitive(COLLADAFW::UniqueId(uniqueId), type);
    jointPrimitive.setAxis(axis);

   // jointPrimitive.setHardLimitMax(10);
   // jointPrimitive.setHardLimitMin(-10);


    COLLADASW::Joint colladaJoint(streamWriter, jointPrimitive, uniqueId, jointName); //TODO make sure that ID is unique


//    kdlJoint.name = uniqueId;

    LOG(INFO) << "Joint id: " << colladaJoint.getJointId();
    LOG(INFO) << "Joint name: " << colladaJoint.getJointName();


    return colladaJoint;
}