C++ Tree::addSegment方法代码示例

//.........这里部分代码省略.........
        segmentcontainer[i + 1].setInertia(inertiacontainer[i + 1]);

        if (i < 8)
        {
            ostringstream converter2, converter5;
            converter2 << "joint00" << i + 2;
            jointname = converter2.str();
            converter5 << "link00" << i + 2;
            linkname = converter5.str();
            linknamecontainer[i + 2] = linkname;
//            std::cout << jointname << linkname << std::endl;
        }

        else
        {
            ostringstream converter2, converter5;
            converter2 << "joint0" << i + 2;
            jointname = converter2.str();
            converter5 << "link0" << i + 2;
            linkname = converter5.str();
            linknamecontainer[i + 2] = linkname;
//            std::cout << jointname << linkname << std::endl;

        }
        jointcontainer[i + 2] = Joint(jointname, Joint::RotY, 1, 0, 0.01);
        framecontainer[i + 2] = Frame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
        segmentcontainer[i + 2] = Segment(linkname, jointcontainer[i + 2], framecontainer[i + 2]);
        inertiacontainer[i + 2] = RigidBodyInertia(pointMass, Vector(0.0, -0.4, 0.0), rotInerSeg);
        segmentcontainer[i + 2].setInertia(inertiacontainer[i + 2]);
    }

    //add created segments to the tree (1 initial base chain + 5 x branches)

    //connect initial base chain to tree root
    a_tree.addSegment(segmentcontainer[0], "L0");
    std::cout << "Initial base chain" << std::endl;
    for (unsigned int i = 0; i < numberofbranches - 1; i++) //chain including link0-link4 (5 segments)
    {
        a_tree.addSegment(segmentcontainer[i + 1], linknamecontainer[i]);
        std::cout << linknamecontainer[i] << " and " << segmentcontainer[i + 1].getName() << std::endl;
    }

    int initialChainElementNumber = a_tree.getNrOfSegments(); //number of segments in initial base chain section of the tree
    int elementsInBranch = (numberofsegments - initialChainElementNumber) / numberofbranches; //number of elements in each branch

    //connect 1st branch to the last link of the initial chain
    a_tree.addSegment(segmentcontainer[numberofbranches], linknamecontainer[numberofbranches - 1]);

    std::cout << "Branch " << numberofbranches-4 << std::endl;
    //segments of the 1st tree branch
    for (unsigned int j = numberofbranches; j < (elementsInBranch + initialChainElementNumber) - 1; j++)
    {
        a_tree.addSegment(segmentcontainer[j + 1], linknamecontainer[j]);
        std::cout << linknamecontainer[j] << " and " << segmentcontainer[j + 1].getName() << std::endl;
    }

    //connect 2nd branch to the last link of the initial chain
    a_tree.addSegment(segmentcontainer[(elementsInBranch + initialChainElementNumber)], linknamecontainer[numberofbranches - 2]);
    
    std::cout << "Branch " << numberofbranches-3 << std::endl;
    //segments of the 2nd tree branch
    for (unsigned int j = (elementsInBranch + initialChainElementNumber); j < (2 * elementsInBranch + initialChainElementNumber) - 1; j++)
    {
        a_tree.addSegment(segmentcontainer[j + 1], linknamecontainer[j]);
        std::cout << linknamecontainer[j] << " and " << segmentcontainer[j + 1].getName() << std::endl;
    }

    //connect 3rd branch to the last link of the initial chain
    a_tree.addSegment(segmentcontainer[(2 * elementsInBranch + initialChainElementNumber)], linknamecontainer[numberofbranches - 3]);

    std::cout << "Branch " << numberofbranches-2 << std::endl;
    //segments of the 3rd tree branch
    for (unsigned int j = (2 * elementsInBranch + initialChainElementNumber); j < (3 * elementsInBranch + initialChainElementNumber) - 1; j++)
    {
        a_tree.addSegment(segmentcontainer[j + 1], linknamecontainer[j]);
        std::cout << linknamecontainer[j] << " and " << segmentcontainer[j + 1].getName() << std::endl;
    }

    //connect 4th branch to the last link of the initial chain
    a_tree.addSegment(segmentcontainer[(3 * elementsInBranch + initialChainElementNumber)], linknamecontainer[numberofbranches - 4]);

    std::cout << "Branch " << numberofbranches-1 << std::endl;
    //segments of the 4ht tree branch
    for (unsigned int j = (3 * elementsInBranch + initialChainElementNumber); j < (4 * elementsInBranch + initialChainElementNumber) - 1; j++)
    {
        a_tree.addSegment(segmentcontainer[j + 1], linknamecontainer[j]);
        std::cout << linknamecontainer[j] << " and " << segmentcontainer[j + 1].getName() << std::endl;
    }

    //connect 5th branch to the last link of the initial chain
    a_tree.addSegment(segmentcontainer[(4 * elementsInBranch + initialChainElementNumber)], linknamecontainer[numberofbranches - 1]);

    //segments of the 5th tree branch
    std::cout << "Branch " << numberofbranches << std::endl;
    for (unsigned int j = (4 * elementsInBranch + initialChainElementNumber); j < (5 * elementsInBranch + initialChainElementNumber) - 1; j++)
    {
        a_tree.addSegment(segmentcontainer[j + 1], linknamecontainer[j]);
        std::cout << linknamecontainer[j] << " and " << segmentcontainer[j + 1].getName() << std::endl;
    }
}

void createMyTree(KDL::Tree& twoBranchTree)
{  
    
    KDL::Joint joint1 = KDL::Joint("j1", KDL::Joint::RotZ, 1, 0, 0.01);
    KDL::Joint joint2 = KDL::Joint("j2", KDL::Joint::RotZ, 1, 0, 0.01);
    KDL::Joint joint3 = KDL::Joint("j3", KDL::Joint::RotZ, 1, 0, 0.01);
    KDL::Joint joint4 = KDL::Joint("j4", KDL::Joint::RotZ, 1, 0, 0.01);
    KDL::Joint joint5 = KDL::Joint("j5", KDL::Joint::RotZ, 1, 0, 0.01);
    KDL::Joint joint6 = KDL::Joint("j6", KDL::Joint::RotZ, 1, 0, 0.01);
    KDL::Joint joint7 = KDL::Joint("j7", KDL::Joint::RotZ, 1, 0, 0.01);
    KDL::Joint joint8 = KDL::Joint("j8", KDL::Joint::RotZ, 1, 0, 0.01);
    KDL::Joint joint9 = KDL::Joint("j9", KDL::Joint::RotZ, 1, 0, 0.01);
    KDL::Joint joint10 = KDL::Joint("j10", KDL::Joint::RotZ, 1, 0, 0.01);

    KDL::Frame frame1(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, -0.4, 0.0));
    KDL::Frame frame2(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, -0.4, 0.0));
    KDL::Frame frame3(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, -0.4, 0.0));
    KDL::Frame frame4(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, -0.4, 0.0));
    KDL::Frame frame5(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, -0.4, 0.0));
    KDL::Frame frame6(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, -0.4, 0.0));
    KDL::Frame frame7(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, -0.4, 0.0));
    KDL::Frame frame8(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, -0.4, 0.0));
    KDL::Frame frame9(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, -0.4, 0.0));
    KDL::Frame frame10(KDL::Rotation::RPY(0.0, 0.0, 0.0), KDL::Vector(0.0, -0.4, 0.0));

    KDL::Segment segment1 = KDL::Segment("L1", joint1, frame1);
    KDL::Segment segment2 = KDL::Segment("L2", joint2, frame2);
    KDL::Segment segment3 = KDL::Segment("L3", joint3, frame3);
    KDL::Segment segment4 = KDL::Segment("L4", joint4, frame4);
    KDL::Segment segment5 = KDL::Segment("L5", joint5, frame5);
    KDL::Segment segment6 = KDL::Segment("L6", joint6, frame6);
    KDL::Segment segment7 = KDL::Segment("L7", joint7, frame7);
    KDL::Segment segment8 = KDL::Segment("L8", joint8, frame8);
    KDL::Segment segment9 = KDL::Segment("L9", joint9, frame9);
    KDL::Segment segment10 = KDL::Segment("M0", joint10, frame10);

    KDL::RotationalInertia rotInerSeg1(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); //around symmetry axis of rotation
    double pointMass = 0.25; //in kg
    KDL::RigidBodyInertia inerSegment1(pointMass, KDL::Vector(0.0, -0.4, 0.0), rotInerSeg1);
    KDL::RigidBodyInertia inerSegment2(pointMass, KDL::Vector(0.0, -0.4, 0.0), rotInerSeg1);
    KDL::RigidBodyInertia inerSegment3(pointMass, KDL::Vector(0.0, -0.4, 0.0), rotInerSeg1);
    KDL::RigidBodyInertia inerSegment4(pointMass, KDL::Vector(0.0, -0.4, 0.0), rotInerSeg1);
    KDL::RigidBodyInertia inerSegment5(pointMass, KDL::Vector(0.0, -0.4, 0.0), rotInerSeg1);
    KDL::RigidBodyInertia inerSegment6(pointMass, KDL::Vector(0.0, -0.4, 0.0), rotInerSeg1);
    KDL::RigidBodyInertia inerSegment7(pointMass, KDL::Vector(0.0, -0.4, 0.0), rotInerSeg1);
    KDL::RigidBodyInertia inerSegment8(pointMass, KDL::Vector(0.0, -0.4, 0.0), rotInerSeg1);
    KDL::RigidBodyInertia inerSegment9(pointMass, KDL::Vector(0.0, -0.4, 0.0), rotInerSeg1);
    KDL::RigidBodyInertia inerSegment10(pointMass, KDL::Vector(0.0, -0.4, 0.0), rotInerSeg1);

    segment1.setInertia(inerSegment1);
    segment2.setInertia(inerSegment2);
    segment3.setInertia(inerSegment3);
    segment4.setInertia(inerSegment4);
    segment5.setInertia(inerSegment5);
    segment6.setInertia(inerSegment6);
    segment7.setInertia(inerSegment7);
    segment8.setInertia(inerSegment8);
    segment9.setInertia(inerSegment9);
    segment10.setInertia(inerSegment10);

    //Tree twoBranchTree("L0");
    twoBranchTree.addSegment(segment1, "L0");
    twoBranchTree.addSegment(segment2, "L1");
    twoBranchTree.addSegment(segment3, "L2");
    twoBranchTree.addSegment(segment4, "L3");
    twoBranchTree.addSegment(segment10, "L4");
    // twoBranchTree.addSegment(segment5, "L2"); //branches connect at joint 3 and j5 is co-located with j3
    // twoBranchTree.addSegment(segment6, "L5");
    // twoBranchTree.addSegment(segment7, "L6");
    // twoBranchTree.addSegment(segment8, "L7");
    // twoBranchTree.addSegment(segment9, "L8");

}