void createMyTree(KDL::Tree& a_tree)
{
unsigned int numberofsegments = 66;
unsigned int numberofbranches = 5;
std::string linknamecontainer[numberofsegments];
Joint jointcontainer[numberofsegments];
Frame framecontainer[numberofsegments];
Segment segmentcontainer[numberofsegments];
RigidBodyInertia inertiacontainer[numberofsegments];
RotationalInertia rotInerSeg(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); //around symmetry axis of rotation
double pointMass = 0.25; //in kg
std::string jointname, linkname;
//create names and segments of the tree
for (unsigned int i = 0; i < numberofsegments - 2; i = i + 3)
{
//this name manipulation is required to ensure that
//topological order of segments in the tree and the order std::map data structure are equivalent
if (i < 10)
{
ostringstream converter, converter3;
converter << "joint00" << i;
jointname = converter.str();
converter3 << "link00" << i;
linkname = converter3.str();
linknamecontainer[i] = linkname;
// std::cout << jointname << linkname << std::endl;
}
else
{
ostringstream converter, converter3;
converter << "joint0" << i;
jointname = converter.str();
converter3 << "link0" << i;
linkname = converter3.str();
linknamecontainer[i] = linkname;
// std::cout << jointname << linkname << std::endl;
}
jointcontainer[i] = Joint(jointname, Joint::RotZ, 1, 0, 0.01);
framecontainer[i] = Frame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
segmentcontainer[i] = Segment(linkname, jointcontainer[i], framecontainer[i]);
inertiacontainer[i] = RigidBodyInertia(pointMass, Vector(0.0, -0.4, 0.0), rotInerSeg);
segmentcontainer[i].setInertia(inertiacontainer[i]);
if (i < 9)
{
ostringstream converter1, converter4;
converter1 << "joint00" << i + 1;
jointname = converter1.str();
converter4 << "link00" << i + 1;
linkname = converter4.str();
linknamecontainer[i + 1] = linkname;
// std::cout << jointname << linkname << std::endl;
}
else
{
ostringstream converter1, converter4;
converter1 << "joint0" << i + 1;
jointname = converter1.str();
converter4 << "link0" << i + 1;
linkname = converter4.str();
linknamecontainer[i + 1] = linkname;
// std::cout << jointname << linkname << std::endl;
}
jointcontainer[i + 1] = Joint(jointname, Joint::RotX, 1, 0, 0.01);
framecontainer[i + 1] = Frame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 0.0));
segmentcontainer[i + 1] = Segment(linkname, jointcontainer[i + 1], framecontainer[i + 1]);
inertiacontainer[i + 1] = RigidBodyInertia(pointMass, Vector(0.0, -0.4, 0.0), rotInerSeg);
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");
}
