Building a KDL Tree

Submitted by weena on Thu, 2008-10-23 08:26

Orocos-users

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");

Building a KDL Tree

Submitted by Ruben Smits on Mon, 2008-10-27 12:20.

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

Submitted by weena on Tue, 2008-10-28 20:59.

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.)

Building a KDL Tree

Submitted by Ruben Smits on Wed, 2008-10-29 07:55.

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

Submitted by weena on Thu, 2008-10-30 07:50.

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

Submitted by weena on Thu, 2008-10-30 07:49.

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


    » Login or register to post comments



  
    
  
Building a KDL Tree
    Submitted by Ruben Smits on Thu, 2008-10-30 08:30.
    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



    » Login or register to post comments


  
    
  
Building a KDL Tree
    Submitted by weena on Thu, 2008-10-23 08:50.
    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");

    » Login or register to post comments