KDL-Examples

Some small examples for usage.

Do not hesitate to add your own small examples.

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

Forward kinematic chain example

// Copyright  (C)  2007  Francois Cauwe <francois at cauwe dot org>
 
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
 
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <stdio.h>
#include <iostream>
 
using namespace KDL;
 
 
int main( int argc, char** argv )
{
    //Definition of a kinematic chain & add segments to the chain
    KDL::Chain chain;
    chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
    chain.addSegment(Segment(Joint(Joint::RotZ)));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
    chain.addSegment(Segment(Joint(Joint::RotZ)));
 
    // Create solver based on kinematic chain
    ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
 
    // Create joint array
    unsigned int nj = chain.getNrOfJoints();
    KDL::JntArray jointpositions = JntArray(nj);
 
    // Assign some values to the joint positions
    for(unsigned int i=0;i<nj;i++){
        float myinput;
        printf ("Enter the position of joint %i: ",i);
        scanf ("%e",&myinput);
        jointpositions(i)=(double)myinput;
    }
 
    // Create the frame that will contain the results
    KDL::Frame cartpos;    
 
    // Calculate forward position kinematics
    bool kinematics_status;
    kinematics_status = fksolver.JntToCart(jointpositions,cartpos);
    if(kinematics_status>=0){
        std::cout << cartpos <<std::endl;
        printf("%s \n","Succes, thanks KDL!");
    }else{
        printf("%s \n","Error: could not calculate forward kinematics :(");
    }
}

Using KDL

Hi, I have a basic question...how should look like my makefile in order to compile the example posted above???

Thanks,

Breno Carneiro Pinheiro Electrical Engineering

I got a error when I tried to run this FK example

I got the following error when main() returns.

 "Run-Time Check Failure #2 - Stack around the variable 'fksolver' was corrupted."
Windows 7 + visual studio 2008.

I tried to put FK solver into a function. When that function returns, same error will always happen.

Ruben Smits's picture

Inverse position example

//Creation of the chain:
KDL::Chain chain;
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
chain.addSegment(Segment(Joint(Joint::RotZ)));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
chain.addSegment(Segment(Joint(Joint::RotZ)));
 
//Creation of the solvers:
ChainFkSolverPos_recursive fksolver1(chain1);//Forward position solver
ChainIkSolverVel_pinv iksolver1v(chain1);//Inverse velocity solver
ChainIkSolverPos_NR iksolver1(chain1,fksolver1,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6
 
//Creation of jntarrays:
JntArray q(chain.getNrOfJoints());
JntArray q_init(chain.getNrOfJoints());
 
//Set destination frame
Frame F_dest=...;
 
int ret = iksolverpos.CartToJnt(q_init,F_dest,q);

Inverse position example

Excuse me, a question is, how can we implement the Inverse Kinematics with some joint parameters, such as angle limits?

Ruben Smits's picture

You will have to create

You will have to create your own solver for this. But you can start from the existing solver ;). If you need any help, don't hesitate to contact me.

Ruben

Ruben Smits's picture

FindKDL.cmake

Since people want to use KDL in their own project i'm posting a FindKDL.cmake file that can find KDL and the RTT-bindings if you want to:

your CMakeLists.txt should be something like this:
{{{
#------
PROJECT(Dummy)

INCLUDE (${PROJECT_SOURCE_DIR}/FindPkgConfig.cmake)

## Find KDL
SET( KDL_INSTALL ${CMAKE_INSTALL_PREFIX} CACHE PATH "The KDL installation directory.")
INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
#--------
}}}

A link to the files:

http://people.mech.kuleuven.be/~rsmits/FindKDL/

Inverse Kinematic

Does anyone know where I can find a working example of Inverse Kinematic? I can't compile the example above! thank you!!

Apply KDL to Delta Robot (parallel robot)

Hello,

We are investigating on how to control our Delta Robot http://en.wikipedia.org/wiki/Delta_robot. We found KDL, but we are not yet sure if we can apply it to our Delta Robot to do kinematic calculations (inverse & forward). It seems to support parallel robots with kinematic chains, but perhaps someone could clarify this? Thanks.

getting the name of the last segment of a tree vs. a chain

There is an API difference for a chain and a tree:

  • for a chain:

std::string end_segment_name = chain_base.getSegment(chain_base.getNrOfSegments()-1).getName();

  • for a tree:

std::string end_segment_name = your_tree.getSegments().end()->second.segment.getName();

The end segment of a chain is the last segment while the end segment you get from the tree like this, has no physical meaning related with 'end' (it's just the last element in the map).