Building a KDL Tree

Hi!

I'm tring to model a robot by constructing a tree made of chains and segments. Unfortunately I don't manage to construct it.

I don't know if the error comes from the name of my chains or segments (does the name correspond to the name of my variable?)

Does anybody have an idea?

Thanks!

Julia

 
 
  KDL::Tree tree = Tree();
 
  //reference segment : middle of the torso
  KDL::Segment base_segment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, 0.0, 0.0, 0.0));
 
  tree.addChain(right_arm.get_chain(), "right_arm", "base_segment");
  tree.addChain(left_arm.get_chain(), "left_arm", "base_segment");
  tree.addChain(right_leg.get_chain(), "right_leg", "base_segment");
  tree.addChain(left_leg.get_chain(), "left_leg", "base_segment");
 
  KDL::Segment seg1 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, M_PI/2 ,0,position.theta ));  
  KDL::Segment seg2 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, 0.0            ,0.0        , position.phi   ));  
  KDL::Segment seg3 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, -M_PI/2        , position.r, position.phi   ));  
  KDL::Segment seg4 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, 0.0            , 0.0       , position.thetaz));  
  KDL::Segment seg5 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0,  M_PI/2        , 0.0       , position.thetax));  
  KDL::Segment seg6 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, -M_PI/2        , 0.0       , position.thetay));  
 
  tree.addSegment(seg1, "seg1", "base_segment");
  tree.addSegment(seg2, "seg2", "base_segment");
  tree.addSegment(seg3, "seg3", "base_segment");
  tree.addSegment(seg4, "seg4", "base_segment");
  tree.addSegment(seg5, "seg5", "base_segment");
  tree.addSegment(seg6, "seg6", "base_segment");

Ruben Smits's picture

Building a KDL Tree

On Thursday October 23 2008 10:26:42 julia [dot] jesse [..] ... wrote:
> Hi!
>
> I'm tring to model a robot by constructing a tree made of chains and
> segments. Unfortunately I don't manage to construct it.
>
> I don't know if the error comes from the name of my chains or segments
> (does the name correspond to the name of my variable?)
>
> Does anybody have an idea?

The tree code is still very experimental, it has been a while since i looked
at the code. Can you give some more information about the error you got?

Ruben

>
>
> KDL::Tree tree = Tree();
>
> //reference segment : middle of the torso
> KDL::Segment base_segment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
> KDL::Frame().DH(0.0, 0.0, 0.0, 0.0));
>
> tree.addChain(right_arm.get_chain(), "right_arm", "base_segment");
> tree.addChain(left_arm.get_chain(), "left_arm", "base_segment");
> tree.addChain(right_leg.get_chain(), "right_leg", "base_segment");
> tree.addChain(left_leg.get_chain(), "left_leg", "base_segment");
>
> KDL::Segment seg1 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
> KDL::Frame().DH(0.0, M_PI/2 ,0,position.theta )); KDL::Segment seg2 =
> KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, 0.0
> ,0.0 , position.phi )); KDL::Segment seg3 =
> KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, -M_PI/2
> , position.r, position.phi )); KDL::Segment seg4 =
> KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, 0.0
> , 0.0 , position.thetaz)); KDL::Segment seg5 =
> KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, M_PI/2
> , 0.0 , position.thetax)); KDL::Segment seg6 =
> KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, -M_PI/2
> , 0.0 , position.thetay));
>
> tree.addSegment(seg1, "seg1", "base_segment");
> tree.addSegment(seg2, "seg2", "base_segment");
> tree.addSegment(seg3, "seg3", "base_segment");
> tree.addSegment(seg4, "seg4", "base_segment");
> tree.addSegment(seg5, "seg5", "base_segment");
> tree.addSegment(seg6, "seg6", "base_segment");
>
> --
> Orocos-Users mailing list
> Orocos-Users [..] ...
> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>
> Disclaimer: http://www.kuleuven.be/cwis/email_disclaimer.htm

Building a KDL Tree

Hi!

I tried to add first one chain to my base segment:
and print the result.
but success == 0, so i think something went wrong, isn't it?

bool success = false;

success = tree.addChain(right_arm.get_chain(), "right_arm", "base_segmentsegment1");
cout << success << endl;

(I have to model a biped walking robot in order to study later its movements, it is why i wanted to use a tree.)

Ruben Smits's picture

Building a KDL Tree

On Tuesday October 28 2008 21:59:49 julia [dot] jesse [..] ... wrote:
> Hi!
>
> I tried to add first one chain to my base segment:
> and print the result.
> but success == 0, so i think something went wrong, isn't it?
>
>
> bool success = false;
>
> success = tree.addChain(right_arm.get_chain(), "right_arm",
> "base_segmentsegment1"); cout

I checked the code, and the api of addChain is as follows:

bool Tree::addChain(const Chain& chain, const std::string& chain_name, const
std::string& hook_name)

When the tree is created using the default constructor there is only one
"hook" available. It has the name "root", so the very first segment/chain you
add should get connected to "root": if you change your code just a little bit:

----------------------

KDL::Tree tree = Tree();

//reference segment : middle of the torso
KDL::Segment base_segment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame().DH(0.0, 0.0, 0.0, 0.0));

//add base_segment to the root of the tree

+ tree.addSegment(base_segment,"base_segment","root")

tree.addChain(right_arm.get_chain(), "right_arm", "base_segment");
tree.addChain(left_arm.get_chain(), "left_arm", "base_segment");
tree.addChain(right_leg.get_chain(), "right_leg", "base_segment");
tree.addChain(left_leg.get_chain(), "left_leg", "base_segment");

KDL::Segment seg1 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame().DH(0.0, M_PI/2 ,0,position.theta ));
KDL::Segment seg2 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame().DH(0.0, 0.0 ,0.0 , position.phi ));
KDL::Segment seg3 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame().DH(0.0, -M_PI/2 , position.r, position.phi ));
KDL::Segment seg4 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame().DH(0.0, 0.0 , 0.0 , position.thetaz));
KDL::Segment seg5 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame().DH(0.0, M_PI/2 , 0.0 , position.thetax));
KDL::Segment seg6 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame().DH(0.0, -M_PI/2 , 0.0 , position.thetay));

tree.addSegment(seg1, "seg1", "base_segment");
tree.addSegment(seg2, "seg2", "base_segment");
tree.addSegment(seg3, "seg3", "base_segment");
tree.addSegment(seg4, "seg4", "base_segment");
tree.addSegment(seg5, "seg5", "base_segment");
tree.addSegment(seg6, "seg6", "base_segment"
------------------------------

Let me know if this works, also when you want to develop forward and inverse
kinematics for the tree, i would like to help you out (i already have some
experimental implementations and ideas for the forward position and velocity
kinematics and for the creation of jacobians between different hooks in the
tree).

Ruben
> --
> Orocos-Users mailing list
> Orocos-Users [..] ...
> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>
> Disclaimer: http://www.kuleuven.be/cwis/email_disclaimer.htm

Building a KDL Tree

Hi,

thank you! the construction of the tree works now :)

i still got a strange thing:
i tried to get the number of segments when i'm building my tree

KDL::Tree tree = Tree();
cout

Building a KDL Tree

Hi,

thank you! the construction of the tree works now :)

i still got a strange thing:
i tried to get the number of segments when i'm building my tree


KDL::Tree tree = Tree();
cout << "nb of tree segments : " << tree.getNrOfSegments() << endl;

//reference segment : middle of the torso
KDL::Segment base_segment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, 0.0, 0.0, 0.0));

tree.addSegment(base_segment, "base_segment", "root");
cout << "nb of tree segments : " << tree.getNrOfSegments() << endl;

tree.addChain(right_arm.get_chain(), "right_arm", "base_segment");
cout << "nb of tree segments : " << tree.getNrOfSegments() << endl;

i get as an answer :

nb of tree segments : 134520522
nb of tree segments : 134520523
nb of tree segments : 134520533

it is correct in the sens that i first add 1 segment and then 10 segments. but why doesn't it begin with 0 instead of 134520522?

I let you know when I will develop the inverse kinematic for my tree

Julia

Ruben Smits's picture

Building a KDL Tree

It is a bug, the variable nrOfSegments in the tree class wasn't initialised to zero resulting in some random initial value.

This is fixed in trunk/kdl

Ruben

[

weena wrote:

Hi,

thank you! the construction of the tree works now :)

i still got a strange thing:
i tried to get the number of segments when i'm building my tree


KDL::Tree tree = Tree();
cout << "nb of tree segments : " << tree.getNrOfSegments() << endl;

//reference segment : middle of the torso
KDL::Segment base_segment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, 0.0, 0.0, 0.0));

tree.addSegment(base_segment, "base_segment", "root");
cout << "nb of tree segments : " << tree.getNrOfSegments() << endl;

tree.addChain(right_arm.get_chain(), "right_arm", "base_segment");
cout << "nb of tree segments : " << tree.getNrOfSegments() << endl;

i get as an answer :

nb of tree segments : 134520522
nb of tree segments : 134520523
nb of tree segments : 134520533

it is correct in the sens that i first add 1 segment and then 10 segments. but why doesn't it begin with 0 instead of 134520522?

I let you know when I will develop the inverse kinematic for my tree

Julia

Building a KDL Tree

Hi!

I'm tring to model a robot by constructing a tree made of chains and segments.
Unfortunately I don't manage to construct it.

I don't know if the error comes from the name of my chains or segments (does the name correspond to the name of my variable?)

Does anybody have an idea?

Thanks!

Julia

KDL::Tree tree = Tree();

//reference segment : middle of the torso
KDL::Segment base_segment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, 0.0, 0.0, 0.0));

tree.addChain(right_arm.get_chain(), "right_arm", "base_segment");
tree.addChain(left_arm.get_chain(), "left_arm", "base_segment");
tree.addChain(right_leg.get_chain(), "right_leg", "base_segment");
tree.addChain(left_leg.get_chain(), "left_leg", "base_segment");

KDL::Segment seg1 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, M_PI/2 ,0,position.theta ));
KDL::Segment seg2 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, 0.0 ,0.0 , position.phi ));
KDL::Segment seg3 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, -M_PI/2 , position.r, position.phi ));
KDL::Segment seg4 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, 0.0 , 0.0 , position.thetaz));
KDL::Segment seg5 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, M_PI/2 , 0.0 , position.thetax));
KDL::Segment seg6 = KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(0.0, -M_PI/2 , 0.0 , position.thetay));

tree.addSegment(seg1, "seg1", "base_segment");
tree.addSegment(seg2, "seg2", "base_segment");
tree.addSegment(seg3, "seg3", "base_segment");
tree.addSegment(seg4, "seg4", "base_segment");
tree.addSegment(seg5, "seg5", "base_segment");
tree.addSegment(seg6, "seg6", "base_segment");