- Extensive support for :
- Geometric primitives: point, frame, twist ...
- Kinematic Trees: chain and tree structures. In literature, multiple definitions exist for a kinematic structure: a chain as the equivalent for all types of kinematic structures (chain, tree, graph) or chain as the serial version of a kinematic structure. KDL uses the last, or using graph-theory terminology:
- A closed-loop mechanism is a graph,
- an open-loop mechanism is a tree, and
- an unbranched tree is a chain.

- Next to kinematics, also parameters for dynamics are included (inertia...)

- Kinematic and Dynamic Solvers: various generic forward and inverse kinematic algorithms, redundancy resolution, ...
- Instantaneous Motions: under construction (Expected release, Spring 2011)
- Motion Trajectories: Cartesian paths, velocity profiles, Cartesian trajectories

- Realtime-safe operations/functions whenever relevant: they do not lead to dynamic memory allocations and all of them are deterministic in time.

- Python bindings

- Typekits and transport-kits for Orocos/RTT

- Integrated in ROS

Available Primitives

A Vector is a 3x1 matrix containing X-Y-Z coordinate values. It is used for representing: 3D position of a point wrt a reference frame, rotational and translational part of a 6D motion or force entity : <equation id="vector"><equation>

Vector v1; //The default constructor, X-Y-Z are initialized to zero Vector v2(x,y,z); //X-Y-Z are initialized with the given values Vector v3(v2); //The copy constructor Vector v4 = Vector::Zero(); //All values are set to zero

The operators [ ] and ( ) use indices from 0..2, index checking is enabled/disabled by the DEBUG/NDEBUG definitions:

v1[0]=v2[1];//copy y value of v2 to x value of v1 v2(1)=v3(3);//copy z value of v3 to y value of v2 v3.x( v4.y() );//copy y value of v4 to x value of v3

You can multiply or divide a Vector with a double using the operator * and /:

v2=2*v1; v3=v1/2;

v2+=v1; v3-=v1; v4=v1+v2; v5=v2-v3;

v3=v1*v2; //Cross product double a=dot(v1,v2)//Scalar product

You can reset the values of a vector to zero:

SetToZero(v1);

Element by element comparison with or without user-defined accuracy:

v1==v2; v2!=v3; Equal(v3,v4,eps);//with accuracy eps

A Rotation is the 3x3 matrix that represents the 3D rotation of an object wrt the reference frame.

<equation id="rotation"><equation>

The following result always in consistent Rotations. This means the rows/columns are always normalized and orthogonal:

Rotation r1; //The default constructor, initializes to an 3x3 identity matrix Rotation r1 = Rotation::Identity();//Identity Rotation = zero rotation Rotation r2 = Rotation::RPY(roll,pitch,yaw); //Rotation built from Roll-Pitch-Yaw angles Rotation r3 = Rotation::EulerZYZ(alpha,beta,gamma); //Rotation built from Euler Z-Y-Z angles Rotation r4 = Rotation::EulerZYX(alpha,beta,gamma); //Rotation built from Euler Z-Y-X angles Rotation r5 = Rotation::Rot(vector,angle); //Rotation built from an equivalent axis(vector) and an angle.

The following should be used with care, they can result in inconsistent rotation matrices, since there is no checking if columns/rows are normalized or orthogonal

Rotation r6( Xx,Yx,Zx,Xy,Yy,Zy,Xz,Yz,Zz);//Give each individual element (Column-Major) Rotation r7(vectorX,vectorY,vectorZ);//Give each individual column

Individual values, the indices go from 0..2:

double Zx = r1(0,2);

Getting EulerZYZ, Euler ZYX, Roll-Pitch-Yaw angles , equivalent rotation axis with angle:

r1.GetEulerZYZ(alpha,beta,gamma); r1.GetEulerZYX(alpha,beta,gamma); r1.GetRPY(roll,pitch,yaw); axis = r1.GetRot();//gives only rotation axis angle = r1.GetRotAngle(axis);//gives both angle and rotation axis

Getting the Unit vectors:

vecX=r1.UnitX();//or r1.UnitX(vecX); vecY=r1.UnitY();//or r1.UnitY(vecY); vecZ=r1.UnitZ();//or r1.UnitZ(vecZ);

Replacing a rotation by its inverse:

r1.SetInverse();//r1 is inverted and overwritten

Getting the inverse rotation without overwriting the original:

r2=r1.Inverse();//r2 is the inverse rotation of r1

Compose two rotations to a new rotation, the order of the rotations is important:

r3=r1*r2;

Compose a rotation with elementary rotations around X-Y-Z:

r1.DoRotX(angle); r2.DoRotY(angle); r3.DoRotZ(angle);

this is the shorthand version of:

r1 = r1*Rotation::RotX(angle)

Rotating a Vector using a Rotation and the operator *:

v2=r1*v1;

Element by element comparison with or without user-defined accuracy:

r1==r2; r1!=r2; Equal(r1,r2,eps);

A Frame is the 4x4 matrix that represents the pose of an object/frame wrt a reference frame. It contains:

- a Rotation M for the rotation of the object/frame wrt the reference frame.
- a Vector p for the position of the origin of the object/frame in the reference frame

<equation id="frame"><equation>

Frame f1;//Creates Identity frame Frame f1=Frame::Identity();//Creates an identity frame: Rotation::Identity() and Vector::Zero() Frame f2(your_rotation);//Create a frame with your_rotation and a zero vector Frame f3(your_vector);//Create a frame with your_vector and a identity rotation Frame f4(your_rotation,your_vector);//Create a frame with your_rotation Frame f5(your_vector,your_rotation);//and your_vector Frame f5(f6);//the copy constructor

Individual values from the 4x4 matrix, the indices go from 0..3:

double x = f1(0,3); double Yy = f1(1,1);

Another way is to go through the underlying Rotation and Vector:

Vector p = f1.p; Rotation M = f1.M;

You can use the operator * to compose frames. If you have a Frame F_A_B that expresses the pose of frame B wrt frame A, and a Frame F_B_C that expresses the pose of frame C wrt to frame B, the calculation of Frame F_A_C that expresses the pose of frame C wrt to frame A is as follows:

Frame F_A_C = F_A_B * F_B_C;

F_A_C.p is the location of the origin of frame C expressed in frame A, and F_A_C.M is the rotation of frame C expressed in frame A.

Replacing a frame by its inverse:

` //not yet implemented`

Getting the inverse:

f2=f1.Inverse();//f2 is the inverse of f1

Element by element comparison with or without user-defined accuracy:

f1==f2; f1!=f2; Equal(f1,f2,eps);

A Twist is the 6x1 matrix that represents the velocity of a Frame using a 3D translational velocity Vector *vel* and a 3D angular velocity Vector *rot*:

<equation id="twist"><equation>

Twist t1; //Default constructor, initializes both vel and rot to Zero Twist t2(vel,rot);//Vector vel, and Vector rot Twist t3 = Twist::Zero();//Zero twist

Note: in contrast to the creation of Frames, the order in which vel and rot Vectors are supplied to the constructor is important.

Using the operators [ ] and ( ), the indices from 0..2 return the elements of vel, the indices from 3..5 return the elements of rot:

double vx = t1(0); double omega_y = t1[4]; t1(1) = vy; t1[5] = omega_z;

Because some robotics literature put the rotation part on top it is safer to use the vel, rot members to access the individual elements:

double vx = t1.vel.x();//or vx = t1.vel(0); double omega_y = t1.rot.y();//or omega_y = t1.rot(1); t1.vel.y(v_y);//or t1.vel(1)=v_y; //etc

The same operators as for Vector are available:

t2=2*t1; t2=t1*2; t2=t1/2;

The same operators as for Vector are available:

t1+=t2; t1-=t2; t3=t1+t2; t3=t1-t2;

Element by element comparison with or without user-defined accuracy:

t1==t2; t1!=t2; Equal(t1,t2,eps);

A Wrench is the 6x1 matrix that represents a force on a Frame using a 3D translational force Vector *force* and a 3D moment Vector *torque*:

<equation id="wrench"><equation>

Wrench w1; //Default constructor, initializes force and torque to Zero Wrench w2(force,torque);//Vector force, and Vector torque Wrench w3 = Wrench::Zero();//Zero wrench

Using the operators [ ] and ( ), the indices from 0..2 return the elements of force, the indices from 3..5 return the elements of torque:

double fx = w1(0); double ty = w1[4]; w1(1) = fy; w1[5] = tz;

Because some robotics literature put the torque part on top it is safer to use the torque, force members to access the individual elements:

double fx = w1.force.x();//or fx = w1.force(0); double ty = w1.torque.y();//or ty = w1.torque(1); w1.force.y(fy);//or w1.force(1)=fy;//etc

The same operators as for Vector are available:

w2=2*w1; w2=w1*2; w2=w1/2;

The same operators as for Twist are available:

w1+=w2; w1-=w2; w3=w1+w2; w3=w1-w2;

Element by element comparison with or without user-defined accuracy:

w1==w2; w1!=w2; Equal(w1,w2,eps);

Wrenches and Twists are expressed in a certain **reference frame**; the translational Vector *vel* of the Twists and the moment Vector *torque* of the Wrenches represent the velocity of, resp. the moment on, a certain **reference point** in that frame. Common choices for the reference point are the origin of the reference frame or a task specific point.

The values of a Wrench or Twist change if the reference frame or reference point is changed.

If you want to change the reference point you need the Vector v_old_new from the old reference point to the new reference point expressed in the reference frame of the Wrench or Twist:

t2 = t1.RefPoint(v_old_new); w2 = w1.RefPoint(v_old_new);

If you want to change the reference frame but want to keep the reference point intact you can use a Rotation matrix R_AB which expresses the rotation of the current reference frame B wrt to the new reference frame A:

ta = R_AB*tb; wa = R_AB*wb;

Note: This operation seems to *multiply* a 3x3 matrix R_AB with 6x1 matrices tb or wb, while in reality it uses the 6x6 *Screw transformation matrix* derived from R_AB.

If you want to change both the reference frame and the reference point you can use a Frame F_AB which contains (i) Rotation matrix R_AB which expresses the rotation of the current reference frame B wrt to the new reference frame A and (ii) the Vector v_old_new from the old reference point to the new reference point expressed in A:

ta = F_AB*tb; wa = F_AB*wb;

Note: This operation seems to *multiply* a 4x4 matrix F_AB with 6x1 matrices tb or wb, while in reality it uses the 6x6 *Screw transformation matrix* derived from F_AB.

t = diff(F_w_A,F_w_B,timestep)//differentiation F_w_B = F_w_A.addDelta(t,timestep)//integration

t is the twist that moves frame A to frame B in *timestep* seconds. t is expressed in reference frame w using the origin of A as velocity reference point.

Select your revision:

Table of Contents

A Joint allows translation or rotation in one degree of freedom between two Segments

Joint rx = Joint(Joint::RotX);//Rotational Joint about X Joint ry = Joint(Joint::RotY);//Rotational Joint about Y Joint rz = Joint(Joint::RotZ);//Rotational Joint about Z Joint tx = Joint(Joint::TransX);//Translational Joint along X Joint ty = Joint(Joint::TransY);//Translational Joint along Y Joint tz = Joint(Joint::TransZ);//Translational Joint along Z Joint fixed = Joint(Joint::None);//Rigid Connection

Joint rx = Joint(Joint::RotX); double q = M_PI/4;//Joint position Frame f = rx.pose(q); double qdot = 0.1;//Joint velocity Twist t = rx.twist(qdot);

A Segment is an ideal rigid body to which one single Joint is connected and one single tip frame. It contains:

- a Joint located at the root frame of the Segment.
- a Frame describing the pose between the end of the Joint and the tip frame of the Segment.

Segment s = Segment(Joint(Joint::RotX), Frame(Rotation::RPY(0.0,M_PI/4,0.0), Vector(0.1,0.2,0.3) ) );

double q=M_PI/2;//joint position Frame f = s.pose(q);//s constructed as in previous example double qdot=0.1;//joint velocity Twist t = s.twist(q,qdot);

A KDL::Chain is

- a kinematic description of a serial chain of bodies connected by joints.
- built out of KDL::Segments.

A Chain has

- a default constructor, creating an empty chain without any segments.
- a copy-constructor, creating a copy of an existing chain.
- a =-operator.

Chain chain1; Chain chain2(chain3); Chain chain4 = chain5;

Chains are constructed by adding segments or existing chains to the end of the chain. These functions add copies of the arguments, not the arguments themselves!

chain1.addSegment(segment1); chain1.addChain(chain2);

You can get the number of joints and number of segments (this is not always the same since a segment can have a Joint::None, which is not included in the number of joints):

unsigned int nj = chain1.getNrOfJoints(); unsigned int js = chain1.getNrOfSegments();

You can iterate over the segments of a chain by getting a reference to each successive segment:

Segment& segment3 = chain1.getSegment(3);

A KDL::Tree is

- a kinematic description of a tree of bodies connected by joints.
- built out of KDL::Segments.

A Tree has

- a constructor that creates an empty tree without any segments and with the given name as its root name. The root name will be "root" if no name is given.
- a copy-constructor, creating a copy of an existing tree.
- a =-operator

Tree tree1; Tree tree2("RootName"); Tree tree3(tree4); Tree tree5 = tree6;

Trees are constructed by adding segments, existing chains or existing trees to a given hook name. The methods will return false if the given hook name is not in the tree. These functions add copies of the arguments, not the arguments themselves!

bool exit_value; exit_value = tree1.addSegment(segment1,"root"); exit_value = tree1.addChain(chain1,"Segment 1"); exit_value = tree1.addTree(tree2,"root");

You can get the number of joints and number of segments (this is not always the same since a segment can have a fixed joint (Joint::None), which is not included in the number of joints):

unsigned int nj = tree1.getNrOfJoints(); unsigned int js = tree1.getNrOfSegments();

You can retrieve the root segment:

std::map<std::string,TreeElement>::const_iterator root = tree1.getRootSegment();

You can also retrieve a specific segment in a tree by its name:

std::map<std::string,TreeElement>::const_iterator segment3 = tree1.getSegment("Segment 3");

You can retrieve the segments in the tree:

std::map<std::string,TreeElement>& segments = tree1.getSegments();

It is possible to request the chain in a tree between a certain root and a tip:

bool exit_value; Chain chain; exit_value = tree1.getChain("Segment 1","Segment 3",chain); //Segment 1 and segment 3 are included but segment 1 is renamed. Chain chain2; exit_value = tree1.getChain("Segment 3","Segment 1",chain2); //Segment 1 and segment 3 are included but segment 3 is renamed.

Table of Contents

A Joint allows translation or rotation in one degree of freedom between two Segments

Joint rx = Joint(Joint::RotX); double q = M_PI/4;//Joint position Frame f = rx.pose(q); double qdot = 0.1;//Joint velocity Twist t = rx.twist(qdot);

A Segment is an ideal rigid body to which one single Joint is connected and one single tip frame. It contains:

- a Joint located at the root frame of the Segment.
- a Frame describing the pose between the end of the Joint and the tip frame of the Segment.

double q=M_PI/2;//joint position Frame f = s.pose(q);//s constructed as in previous example double qdot=0.1;//joint velocity Twist t = s.twist(q,qdot);

A KDL::Chain is

- a kinematic description of a serial chain of bodies connected by joints.
- built out of KDL::Segments.

A Chain has

- a default constructor, creating an empty chain without any segments.
- a copy-constructor, creating a copy of an existing chain.
- a =-operator is supported too.

Chain chain1; Chain chain2(chain3); Chain chain4 = chain5;

Chains are constructed by adding segments or existing chains to the end of the chain. All segments must have a different name (or "NoName"), otherwise the methods will return false and the segments will not be added. The functions add copies of the arguments, not the arguments themselves!

bool exit_value; bool exit_value = chain1.addSegment(segment1); exit_value = chain1.addChain(chain2);

You can get the number of joints and number of segments (this is not always the same since a segment can have a Joint::None, which is not included in the number of joints):

unsigned int nj = chain1.getNrOfJoints(); unsigned int js = chain1.getNrOfSegments();

You can iterate over the segments of a chain by getting a reference to each successive segment. The method will return false if the index is out of bounds.

Segment segment3; bool exit_value = chain1.getSegment(3, segment3);

You can also request a segment by name:

Segment segment3; bool exit_value = chain1.getSegment("Segment 3", segment3);

The root and leaf segment can be requested, as well as all segments in the chain.

bool exit_value; Segment root_segment; Segment leaf_segment; std::vector<Segment> segments; exit_value = chain1.getRootSegment(root_segment); exit_value = chain1.getLeafSegment(leaf_segment); exit_value = chain1.getSegments(segments);

You can request a part of the chain between a certain root and a tip:

bool exit_value; Chain part_chain; exit_value = chain1.getChain_Including(1,3, part_chain); exit_value = chain1.getChain_Including("Segment 1","Segment 3", part_chain); //Segment 1 and Segment 3 are included in the new chain! exit_value = chain1.getChain_Excluding(1,3, part_chain); exit_value = chain1.getChain_Excluding("Segment 1","Segment 3", part_chain); //Segment 1 is not included in the chain. Segment 3 is included in the chain.

There is a function to copy the chain up to a given segment number or segment name:

bool exit_value; Chain chain_copy; exit_value = chain1.copy(3, chain_copy); exit_value = chain1.copy("Segment 3", chain_copy); //Segment 3, 4,... are not included in the copy!

A KDL::Tree is

- a kinematic description of a tree of bodies connected by joints.
- built out of KDL::Segments.

A Tree has

- a constructor that creates an empty tree without any segments and with the given name as its root name. The root name will be "root" if no name is given.
- a copy-constructor, creating a copy of an existing tree.
- a =-operator.

Tree tree1; Tree tree2("RootName"); Tree tree3(tree4); Tree tree5 = tree6;

Trees are constructed by adding segments, existing chains or existing trees to a given hook name. The methods will return false if the given hook name is not in the tree. These functions add copies of the arguments, not the arguments themselves!

bool exit_value; exit_value = tree1.addSegment(segment1,"root"); exit_value = tree1.addChain(chain1,"Segment 1"); exit_value = tree1.addTree(tree2,"root");

You can get the number of joints and number of segments (this is not always the same since a segment can have a Joint::None, which is not included in the number of joints):

unsigned int nj = tree1.getNrOfJoints(); unsigned int js = tree1.getNrOfSegments();

You can retrieve the root segment and the leaf segments:

bool exit_value; std::map<std::string,TreeElement>::const_iterator root; std::map<std::string,TreeElement> leafs; exit_value = tree1.getRootSegment(root); exit_value = tree1.getLeafSegments(leafs);

You can also retrieve a specific segment in a tree by its name:

std::map<std::string,TreeElement>::const_iterator segment3; bool exit_value = tree1.getSegment("Segment 3",segment3);

You can retrieve the segments in the tree:

std::map<std::string,TreeElement> segments; bool exit_value = tree1.getSegments(segments);

It is possible to request the chain in a tree between a certain root and a tip:

bool exit_value; Chain chain; exit_value = tree1.getChain("Segment 1","Segment 3",chain); //Segment 1 and segment 3 are included but segment 1 is renamed. Chain chain2; exit_value = tree1.getChain("Segment 3","Segment 1",chain2); //Segment 1 and segment 3 are included but segment 3 is renamed.

This chain can also be requested in a tree structure with the given root name ("root" if no name is given).

bool exit_value; Tree tree; exit_value = tree1.getChain("Segment 1","Segment 3",tree,"RootName"); Tree tree2; exit_value = tree1.getChain("Segment 3","Segment 1",tree2,"RootName");

There is a function to copy a tree excluding some segments and all their decendants.

bool exit_value; Tree tree_copy; exit_value = tree1.copy("Segment 3", tree_copy); //tree1 is copied up to segment 3 (excluding segment 3). std::vector<std::string> vect; vect.push_back("Segment 1"); vect.push_back("Segment 7"); exit_value = tree1.copy(vect,tree_copy);

KDL contains for the moment only generic solvers for kinematic chains. They can be used (with care) for every KDL::Chain.

The idea behind the generic solvers is to have a uniform API. We do this by inheriting from the abstract classes for each type of solver:

- ChainFkSolverPos
- ChainFkSolverVel
- ChainIkSolverVel
- ChainIkSolverPos

A seperate solver has to be created for each chain. At construction time, it will allocate all necessary resources.

A specific type of solver can add some solver-specific functions/parameters to the interface, but still has to use the generic interface for it's main solving purpose.

The forward kinematics use the function **JntToCart(...)** to calculate the Cartesian space values from the Joint space values. The inverse kinematics use the function **CartToJnt(...)** to calculate the Joint space values from the Cartesian space values.

It recursively adds the poses/velocity of the successive segments, going from the first to the last segment, you can also get intermediate results by giving a Segment number:

ChainFkSolverPos_recursive fksolver(chain1); JntArray q(chain1.getNrOfJoints); q=... Frame F_result; fksolver.JntToCart(q,F_result,segment_nr);