orocos / orocos_kinematics_dynamics

Orocos Kinematics and Dynamics C++ library

Kinematics and Dynamics Library

Orocos project to supply RealTime usable kinematics and dynamics code, it contains code for rigid body kinematics calculations and representations for kinematic structures and their inverse and forward kinematic solvers.

The C++ library is located in the orocos_kdl folder. The installation instructions can be found in INSTALL.md.

The python bindings, are located in the python_orocos_kdl folder. The installation instructions can be found in INSTALL.md.

Always use the same version of the C++ library and the python bindings. As a mismatch between these two can cause many issues.

Also when using ROS/catkin, it is preferred to use the catkin installation method over the cmake/make method.

There will be no ROS Noetic release.

orocos_kinematics_dynamics's Issues

KDL::ChainIkSolverPos_LMA is length unit sensitive

It makes a difference, whether a kinematic chain is defined with millimeter length units or meter length units. The forward kinematics solver KDL::ChainFkSolverPos_recursive gives the same solution in my example, but the inverse kinematics solver KDL::ChainIkSolverPos_LMA gives different results.

I have introduced a length scaling parameter to my code and the results are as expected only if I divide all my lengths (given in millimeter) by 1000. Otherwise the solver won't find a solution.

Excerpts from my code:

double lengthFactor = 1e-3;
...
// Add A1.
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ, -1.0, 0.0),
KDL::Frame(
KDL::Vector(350.0 * lengthFactor, 0.0 * lengthFactor,
750.0 * lengthFactor))));
...
KDL::ChainIkSolverPos_LMA lmaSolver(chain);

for (int index = 0; index < 6; ++index)
{
q_init(index) = 0.0;
}

ret = lmaSolver.CartToJnt(q_init, F_dest, q);

Here the solver will give the expected results (all joint angle values correct and "close to zero"). But when I change lengthFactor to 1.0, the solver won't find a solution and the iterate will contain angle values of several thousand degrees. I have played with the epsilon values passed to the solver constructor, but they don't make difference.

My workaround with the length factor works, therefore I have no problem with the solver right now. Still this seems to be a bug to me.

chainjnttojacdotsolver.hpp not available in Indigo, Kinetic and Lunar versions

Hi everyone.

This file is mentioned in the docs, but isn't present in the Indigo and Lunar installations of orocos_kdl.

Here suggested that I added an issue here.

Thanks in advance.

inertiatest fails on 32bit architectures

I'm currently trying to put KDL into a Fedora package, but the inertiatest consistently fails on 32bit architectures. As an example, see this build log.

How to reproduce:

1. Build on a 32bit architecture
2. Run the tests

Expected behavior
All tests should terminate successfully.

Actual behavior
The inertia test fails, all other tests complete successfully.

The following output is produced on a 64bit machine compiling for 32bit:

$ make CFLAGS='-m32 -march=i686' CXXFLAGS='-m32 -march=i686' VERBOSE=1
[...]
$ make CFLAGS='-m32 -march=i686' CXXFLAGS='-m32 -march=i686' VERBOSE=1 check
/usr/bin/cmake -H/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl -B/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl --check-build-system CMakeFiles/Makefile.cmake 0
make -f CMakeFiles/Makefile2 check
make[1]: Entering directory `/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl'
/usr/bin/cmake -H/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl -B/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl --check-build-system CMakeFiles/Makefile.cmake 0
/usr/bin/cmake -E cmake_progress_start /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/CMakeFiles 0
make -f CMakeFiles/Makefile2 tests/CMakeFiles/check.dir/all
make[2]: Entering directory `/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl'
make -f tests/CMakeFiles/check.dir/build.make tests/CMakeFiles/check.dir/depend
make[3]: Entering directory `/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl'
cd /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl && /usr/bin/cmake -E cmake_depends "Unix Makefiles" /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests/CMakeFiles/check.dir/DependInfo.cmake --color=
make[3]: Leaving directory `/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl'
make -f tests/CMakeFiles/check.dir/build.make tests/CMakeFiles/check.dir/build
make[3]: Entering directory `/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl'
cd /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests && ctest -V
UpdateCTestConfiguration  from :/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests/DartConfiguration.tcl
UpdateCTestConfiguration  from :/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests/DartConfiguration.tcl
Test project /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests
Constructing a list of tests
Done constructing a list of tests
Checking test dependency graph...
Checking test dependency graph end
test 1
    Start 1: framestest

1: Test command: /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests/framestest
1: Test timeout computed to be: 9.99988e+06
1: .........
1: 
1/6 Test #1: framestest .......................   Passed    0.01 sec
test 2
    Start 2: kinfamtest

2: Test command: /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests/kinfamtest
2: Test timeout computed to be: 9.99988e+06
2: ....root(q_nr: 0)
2:      
2: 
2: root(q_nr: 0)
2:      Segment 1(q_nr: 0)
2:      Segment 3(q_nr: 1)
2:      Segment 4(q_nr: 2)
2:      
2:  
2:  
2:  Segment 2(q_nr: 0)
2:      
2:  
2: 
2: root(q_nr: 0)
2:      Segment 5(q_nr: 0)
2:      
2:  Segment 6(q_nr: 1)
2:      Segment 7(q_nr: 2)
2:      
2:  
2:  
2: 
2: root(q_nr: 0)
2:      Segment 5(q_nr: 0)
2:      
2:  Segment 6(q_nr: 1)
2:      Segment 7(q_nr: 2)
2:      
2:  Segment 8(q_nr: 3)
2:      Segment 9(q_nr: 4)
2:      Segment 10(q_nr: 5)
2:      Segment 11(q_nr: 6)
2:      Segment 12(q_nr: 7)
2:      Segment 13(q_nr: 8)
2:      
2:  
2:  
2:  
2:  
2:  
2:  
2:  
2: 
2: root(q_nr: 0)
2:      Segment 1(q_nr: 0)
2:      Segment 3(q_nr: 1)
2:      Segment 4(q_nr: 2)
2:      
2:  
2:  
2:  Segment 2(q_nr: 0)
2:      Segment 5(q_nr: 3)
2:      
2:  Segment 6(q_nr: 4)
2:      Segment 7(q_nr: 5)
2:      
2:  Segment 8(q_nr: 6)
2:      Segment 9(q_nr: 7)
2:      Segment 10(q_nr: 8)
2:      Segment 11(q_nr: 9)
2:      Segment 12(q_nr: 10)
2:      Segment 13(q_nr: 11)
2:      
2:  
2:  
2:  
2:  
2:  
2:  
2:  
2:  
2: 
2: 
2: 
2/6 Test #2: kinfamtest .......................   Passed    0.01 sec
test 3
    Start 3: solvertest

3: Test command: /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests/solvertest
3: Test timeout computed to be: 9.99988e+06
3: ...square problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: underdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: overdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: overdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: .square problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: underdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: overdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: underdetermined problem with WGs segment constructor
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: .0.000000          0.000228      0.523108       0.045580     -0.098212       4.558028      -9.821186     0.410223      0.568421
3: 0.010000          0.000777      0.521924       0.064329     -0.138607       1.874883      -4.039482     0.267959      0.371448
3: 0.020000          0.001560      0.520237       0.092266     -0.198784       2.793640      -6.017780     0.317335      0.440332
3: 0.030000          0.002606      0.517985       0.116808     -0.251636       2.454215      -5.285122     0.300258      0.417227
3: 0.040000          0.003901      0.515194       0.142247     -0.306397       2.543890      -5.476142     0.306241      0.426347
3: 0.050000          0.005447      0.511864       0.167010     -0.359679       2.476367      -5.328224     0.304240      0.424554
3: 0.060000          0.007240      0.508003       0.191565     -0.412482       2.455505      -5.280259     0.305018      0.426829
3: 0.070000          0.009276      0.503619       0.215668     -0.464277       2.410306      -5.179493     0.304841      0.427962
3: 0.080000          0.011551      0.498723       0.239323     -0.515067       2.365461      -5.079057     0.305001      0.429758
3: 0.090000          0.014060      0.493324       0.262446     -0.564671       2.312321      -4.960401     0.305049      0.431584
3: 0.100000          0.016797      0.487436       0.284985     -0.612971       2.253869      -4.829973     0.305136      0.433650
3: .KDL-IK Solver Tests for Near Zero SVs
3: norminal case:  convergence
3: nonconvergence:  pseudoinverse singular
3: nonconvergence:  large displacement, low iterations
3: nonconvergence:  fully singular
3: .KDL-IK WDLS Vel Solver Tests for Near Zero SVs
3: smallest singular value is above threshold (no WDLS)
3: smallest singular value is below threshold (lambda is scaled)
3: smallest singular value is zero (lambda_scaled=lambda)
3: 
3: 
3/6 Test #3: solvertest .......................   Passed    0.01 sec
test 4
    Start 4: inertiatest

4: Test command: /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests/inertiatest
4: Test timeout computed to be: 9.99988e+06
4: ...F
4: 
4/6 Test #4: inertiatest ......................***Failed    0.00 sec
test 5
    Start 5: jacobiantest

5: Test command: /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests/jacobiantest
5: Test timeout computed to be: 9.99988e+06
5: .....
5: 
5/6 Test #5: jacobiantest .....................   Passed    0.00 sec
test 6
    Start 6: velocityprofiletest

6: Test command: /home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl/tests/velocityprofiletest
6: Test timeout computed to be: 9.99988e+06
6: ........
6: 
6/6 Test #6: velocityprofiletest ..............   Passed    0.00 sec

83% tests passed, 1 tests failed out of 6

Total Test time (real) =   0.04 sec

The following tests FAILED:
      4 - inertiatest (Failed)
Errors while running CTest
make[3]: *** [tests/CMakeFiles/check] Error 8
make[3]: Leaving directory `/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl'
make[2]: *** [tests/CMakeFiles/check.dir/all] Error 2
make[2]: Leaving directory `/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl'
make[1]: *** [tests/CMakeFiles/check.dir/rule] Error 2
make[1]: Leaving directory `/home/robocup/rpmbuild/SOURCES/orocos_kinematics_dynamics-2aa76640f0a1c5ac57946c20e844372578b55743/orocos_kdl'
make: *** [check] Error 2

Error in documentation: Rotation::RPY [frames.hpp: 426]

While the documentation says the Rotation object is changed, a new Rotation object is instead returned and no modification is done to the caller.

Properly update version number during release

It looks like the minor version of the orocos_kdl library was updated in the most recent release, and it's breaking builds for users (myself included).

More details here: http://answers.ros.org/question/116401/cant-link-to-liborocos-kdlso11/

Please don't do this in the future, or at least make some effort to make the upgrade path cleaner. Spending 20 minutes rebuilding my entire workspace is unacceptable.

catkin dependency

I was wondering why this package is depending on catkin.

JointType typedef broken by None define in X11 library

We use the Stage Simulator and KDL in combination for a robotics course and stumbled upon a problem that arose in KDL 1.3.

In joint.hpp line 47:
typedef enum { RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,None} JointType;

The None type was already defined in X11/X.h and hat to be undef, as we otherwise would get the following error when compiling our code.

enum expected identifier before numeric constant

Fix: add #undef None before typedef.

JntArray::Equal does not always work

I noticed that doing a KDL::Equal(q_out,KDL:JntArray(q_out.rows()),1000) always returns false. That's a HUGE epsilon, and it's false. I tracked the down to a change in KDL, where the hand-coded equal that looped through the array was replaced by the Eigen isApprox() function. Apparently, is Approx does not work when either of the vectors is very close to zero. This seems bad/wrong.

http://eigen.tuxfamily.org/dox/classEigen_1_1DenseBase.html#a158c2184951e6e415c2e9b98db8e8966

What you wanted for arrays a & b was probably something like:

isZero(a.data-b.data,eps)

pkg-config wrong when path contains spaces

When building orocos_kdl in a workspace with spaces in the path it breaks the build of downstream packages since its pkg-config implementation does not handle this case well.

When e.g. eigen_conversion find_package(orocos_kdl) the result is wrong due to the spaces in the paths:

Looking for KDL in: /tmp/ws/desk top/devel isolated/orocos_kdl
  Includes in: /tmp/ws/desk;/usr/include/eigen3
Compiler flags: top/devel;isolated/orocos_kdl/include;-I/tmp/ws/desk;-I/usr/include/eigen3
  Libraries: orocos-kdl
  Libraries in: /tmp/ws/desk
  Linker flags :
  Defines:

Issue in velocityprofile_spline.cpp

I think one line is missing in quintic spline interpolation method(velocityprofile_spline.cpp).

I think this line should be added at line 111.

duration_ = duration;

Root link can not have mass

Hi,
KDL does not support, that the Root TreeElement contains a mass.
Is this intentional, or can I add a patch to support this use case ?
Greetings
Janosch

Vector::Normalize bug/inconsistency?

@smits
There is a bug in KDL or at least an inconsistency between the documentation and the code.

/** Normalizes this vector and returns it norm

• makes v a unitvector and returns the norm of v.
• if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
• if this is not good, check the return value of this method.
  */

Today, Normalize(eps) does not return 0 when norm of v < eps.

When will there be a new release?

I'm working to integrate this package in the meta-ros repository - see KristofRobot/meta-ros@d146b86#commitcomment-4470564

As I noticed that the latest release (https://github.com/orocos/orocos_kinematics_dynamics/releases/tag/1.1.102) is more than half a year old, and several bug fixes have been applied since then, I decided to reference the git repo directly.

However, it would be much "cleaner" if I could reference a tagged release.

Are there any plans to create a release in the near future?

Thanks!

Kristof

upcoming new release ?

hey, is it possible to have a new release ? 33400d4 is crucial to get ROS working with Python3 (which is default on Arch Linux). Thx !

PyKDL keyword arguments don't work on 14.04/indigo

It appears that the defaults for SIP have changed, so now if you try the following, it will cause a segmentation fault:

import PyKDL
i = PyKDL.RigidBodyInertia(m=1)

chainiksolvervel_pinv_nso math implementation incorrect

I am working on a 7 DOF arm. With testing, chainiksolvervel_pinv_nso gives the exact same answer of chainiksolvervel_pinv every time.

Looking at the code, the nullspace math is not implemented properly on lines 106 - 123. There is no documentation for the attempted approach, and the reference paper from 1977 uses a different optimization without SVD decomposition.

As far as I can guess, the attempted technique is

t = weights * (q_opt - q_in)

q'_out += alpha * V_c * V_n^T * t

where V_c is the first 6 columns of V, and V_n are the remaining columns. Is this correct, and is there a paper or other references for this method?

If I know the desired method, I should be able to submit a patch quickly.

Other issues:
Line 110: The index i should start at jac.rows() and not jac.rows()+1. For a 7 dof arm, jac is 6x7, and this loop will never be run as written. Same error is repeated in line 119.

Line 120: attempts to read tmp2 at index jac.rows()+1, but tmp2 only has jac.columns()-jac.rows() elements.

PyKDL: specifying a root name when intializing a Tree causes segfault

from PyKDL import Tree
ohno = Tree(root_name="carrot")
# kaboom

Segmentation fault: 11

Make4x4 divide by 1000

I've never understood the purpose of the divide by 1000 found here: https://github.com/orocos/orocos_kinematics_dynamics/blob/master/orocos_kdl/src/frames.cpp#L46

I'm not sure I want to see it patched any time soon, because I feel like people have already worked around this problem, but could someone explain why it's there?

Improvement of chainiksolverpos_nr_jl.hpp

Hi,

unfortunately the implementation of chainiksolverpos_nr_jl.hpp is not very robust.

Someone made some extensive benchmark here https://bitbucket.org/traclabs/trac_ik

An alternative that seems to work pretty well is to fix the joint limit moving in the null space of the solution.

In terms of robustness (ability to find a valid solution) ist smashes the previous implementation.

To do that I need the pseudoinverse of the Jacobian.
It is a pity that most of the implementations of ChainIkSolverVel::CartToJnt calculates it, but I don't have any method that allows me to access it from ChainIkSolverPos::CartToJnt :(

This oblige me to calculate jacobian and pseudo inverse twice in a row.

Another solution would be to add also an implementation of ChainIkSolverVel_pinv_JL and require ChainIkSolverPos_NR_JL to use that.

So the questions are:

1. are you interested to consider (and review) a pull request?
2. How terrible would it be to add to ChainIkSolverVel a method such as getLastJacobian and/or getLasJacobianPinv? I know you are very reticent at the idea of changing the API.

PyKDL: __version__ method missing

currently, there's no way of knowing which version of the PyKDL module is loaded.
a PyKDL.version method would be a welcome idea

PyKDL bindings on Precise are flaky

We've just witnessed the following behavior with the python bindings generated with sip 4.13.2 on Ubuntu Precise:

Digging deeper revealed that the underlying C++ object is correct but that the generation of the Python KDL.Vector object is wrong:

A: [[           1,           0,           0;
            0,           1,           0;
            0,           0,           1]
[           1,           2,           3]]
B: [[           1,           0,           0;
            0,           1,           0;
            0,           0,           1]
[           0,           0,           0]]
C: [[           1,           0,           0;
            0,           1,           0;
            0,           0,           1]
[           1,           2,           3]]
(B*A): [[           1,           0,           0;
            0,           1,           0;
            0,           0,           1]
[           1,           2,           3]]
C.p: [           1,           2,           3]
(B * A).p: [           0,           2,           3]
(B*A): [[           1,           0,           0;
            0,           1,           0;
            0,           0,           1]
[           1,           2,           3]]
(B*A)[0][3]: 1.0
(B*A)[1][3]: 2.0
(B*A)[2][3]: 3.0
(B*A).p[0]: 0.0

This does not happen with bindings generated with sip 4.15.x on Ubuntu Precise or Trusty:

python test.py 
A: [[           1,           0,           0;
            0,           1,           0;
            0,           0,           1]
[           1,           2,           3]]
B: [[           1,           0,           0;
            0,           1,           0;
            0,           0,           1]
[           0,           0,           0]]
C: [[           1,           0,           0;
            0,           1,           0;
            0,           0,           1]
[           1,           2,           3]]
(B*A): [[           1,           0,           0;
            0,           1,           0;
            0,           0,           1]
[           1,           2,           3]]
C.p: [           1,           2,           3]
(B * A).p: [           1,           2,           3]
(B*A): [[           1,           0,           0;
            0,           1,           0;
            0,           0,           1]
[           1,           2,           3]]
(B*A)[0][3]: 1.0
(B*A)[1][3]: 2.0
(B*A)[2][3]: 3.0
(B*A).p[0]: 1.0

I currently see no easy way to resolve this on Ubuntu Precise except for updating your sip to 4.15. I personally used the following ppa for that: ppa:pali/kubuntu-backports

[BUG?] Path_Circle::Clone() has a deg2rad conversion

I found this when passing to a constructor of a KDL::Trajectory_Segment a clone of a Path_Circle.
Here it follows the code where the extra deg2rad is called.

Path* Path_Circle::Clone() {
    return new Path_Circle(
        Pos(0),
        F_base_center.p,
        F_base_center.M.UnitY(),
        orient->Pos(pathlength*scalerot),
        pathlength*scalelin/radius/deg2rad,
        orient->Clone(),
        eqradius,
        aggregate
    );
}

PyKDL Tree - AttributeError

Hi,

I have a problem with the PyKDL module more specifically with the Tree/TreeElement class.

> import PyKDL as kdl
> kdl.Tree.addSegment
<built-in function addSegment>

> kdl.Tree.getRootSegment
AttributeError: type object 'Tree' has no attribute 'getRootSegment'
> kdl.TreeElement
AttributeError: 'module' object has no attribute 'TreeElement'

I can only access to the addSegment, getChain, getNrOfJoints, and getNrOfSegments methods of the Tree class. I can NOT access to the other methods (getSegments, getRootSegment, getSegment, addTree, addChain, etc), and also the TreeElement class.

I tested it on my MacOSX 10.9.4 and on Ubuntu 14.04, and I have the same problem.

Do you know how to fix it?

Thanks

Incorrect version number

You have released a 1.3.1 version of kdl. However, in CMakeLists and package.xml still remains 1.3.0 version.

python-orocos-kdl: FindSIP.cmake - crosscompiling problems and proposed fixes

Hi,

While crosscompiling python-orocos-kdl, I encountered several problems with FindSIP.cmake [1]:

(1) Unable to find FindSIP.py
Following line does not find the right file:

FIND_FILE(_find_sip_py FindSIP.py PATHS ${CMAKE_MODULE_PATH})

This can be fixed by adding NO_CMAKE_FIND_ROOT_PATH:

FIND_FILE(_find_sip_py FindSIP.py PATHS ${CMAKE_MODULE_PATH} NO_CMAKE_FIND_ROOT_PATH)`

(2) Unable to find sip
Following line does not find the right file:

 STRING(REGEX REPLACE ".*\nsip_bin:([^\n]+).*$" "\\1" SIP_EXECUTABLE ${sip_config})

This can be fixed by replacing above line by:

FIND_PROGRAM(SIP_EXECUTABLE sip)

(3) Unable to resolve correct SIP_INCLUDE_DIR
Following line does not resolve to the right path:

STRING(REGEX REPLACE ".*\nsip_inc_dir:([^\n]+).*$" "\\1" SIP_INCLUDE_DIR ${sip_config})

Specifically, it is not prepending the correct sysroot path when crosscompiling.
The only place where SIP_INCLUDE_DIR seems to be used is in the following CMakeLists.txt [2] line:

include_directories(${SIP_INCLUDE_DIR} ${PYTHON_INCLUDE_DIRS})

Moreover, it seems that both SIP_INCLUDE_DIR and PYTHON_INCLUDE_DIRS resolve to the same path, therefore SIP_INCLUDE_DIR seems redundant.
To fix the above issue, I simply removed the assignment to SIP_INCLUDE_DIR:

#    STRING(REGEX REPLACE ".*\nsip_inc_dir:([^\n]+).*$" "\\1" SIP_INCLUDE_DIR ${sip_config})

A better permanent fix is probably to change the line in CMakeLists.txt, omitting ${SIP_INCLUDE_DIR}:

include_directories(${PYTHON_INCLUDE_DIRS})

Applying above changes allow me to crosscompile, and the resulting code seems to work correctly. See https://github.com/KristofRobot/meta-ros/blob/kdl/recipes-extended/kdl/python-orocos-kdl/0001-findSIP-fix.patch for the patch with above change.
For additional discussion, see ros/meta-ros#203.

Are these fixes generally applicable?
Are there perhaps better solutions?

I'm willing to launch a pull request with these fixes if there is interest.

Thanks,

Kristof

[1] https://github.com/orocos/orocos_kinematics_dynamics/blob/master/python_orocos_kdl/cmake/FindSIP.cmake
[2] https://github.com/orocos/orocos_kinematics_dynamics/blob/master/python_orocos_kdl/CMakeLists.txt

PyKDL.so is installed to lib64

ROS only uses the "lib" directory, but PyKDL.so is getting installed to lib64.

The offending code is at https://github.com/orocos/orocos_kinematics_dynamics/blob/master/python_orocos_kdl/CMakeLists.txt#L12

Right now, PyKDL.so is the ONLY thing that ends up in lib64 when building "desktop-full" for Jade on Fedora.

Thanks,

--scott

Rotation::Quaternion may return invalid rotation matrix

Constructing a rotation matrix from a non unit quaternion results in an invalid rotation matrix. It should either fail or return a valid rotation matrix as e.g. explained here.

Can't make orocos-kdl-models with Qt

Trying to build kdl with Qt, and got error on orocos-kdl-models building,

Qt work with Visual C++ compiler, main application is builded, but orocos-kdl-models raise error

NMAKE:-1: error: U1073: don't know how to make 'models\orocos-kdl-models.lib'

Can you help?

liborocos-kdl.so.1.4: cannot open shared object file: No such file or directory

Hi,

I'm Florian i work with KDL to solve ik problems.

But when i run my program i've this error : liborocos-kdl.so.1.4: cannot open shared object file: No such file or directory

Nevertheless i've this file in usr/local/lib and LD_LIBRARY = usr/local/lib.
Moreover in my program i've LIBS += "usr/local/lib/*.so".

So i don't understand, if you have any idea ..... ?

Thanks

version tags incompatible with rosinstall_generator --upstream

Hey everyone,

I'd like to use a rather unknown but useful feature of rosinstall_generator to construct my ros-workspace:
rosinstall_generator --upstream

This ignores the *-release repository overhead and directly fetches the upstream source repository
of a ros-module into the workspace.
Now this works great with all of ROS, but it fails to work with this project and that's the reason for this issue:
rosinstall_generator expects the versions released into ROS to exist as tags in the upstream
repository. However, your current version tag is called "v1.3.0" instead of "1.3.0" (the version).

The most easy way to fix this would be to also add a tag without the leading "v" in your repository.
It would be great if you could do that.

Error in CMakeLists.txt of orocos/orocos_kinematics_dynamics

Hi,

I have been hunting a compile bug for this package and finally found it.
See entire search here: https://aur.archlinux.org/packages/ros-indigo-orocos-kdl/#news

The problem is that the Eigen_INCLUDE_DIR(S) is different on different systems, and you are using Eigen_INCLUDE_DIR to find the include.
However, the standard from the FindEigen.cmake files is Eigen_INCLUDE_DIRS <- Note the S on the end.

To fix this, check is Eigen_INCLUDE_DIR is empty and then use Eigen_INCLUDE_DIRS instead if it is.

I am referring to this file: orocos_kinematics_dynamics/orocos_kdl/CMakeLists.txt
Lines: 50-51

If you do not have time I can fix a patch for it.

Thanks!

ImportError: dynamic module does not define module export function (PyInit_PyKDL)

Using python 3.5
When I try to import PyKDL python3 -c "import PyKDL", I got that error

ImportError: dynamic module does not define module export function (PyInit_PyKDL)

PyKDL does not provide wrappers to ChainIkSolverVel_pinv_nso, ChainIkSolverVel_pinv_givens and most important ChainIkSolverPos_LMA

( I'd rather create a pull request, but have no write access, sorry for the inconvenience )

I've uncommented the repr method of Joint, since this fails to compile on OSX, please be aware...

diff --git a/python_orocos_kdl/PyKDL/kinfam.sip b/python_orocos_kdl/PyKDL/kinfam.sip
index 6bc9586..b919cb6 100644
--- a/python_orocos_kdl/PyKDL/kinfam.sip
+++ b/python_orocos_kdl/PyKDL/kinfam.sip
@@ -49,13 +49,13 @@ public:
     JointType getType() const;
     std::string getTypeName() const;
     const char* __repr__();
-%MethodCode
-    std::ostringstream oss;
-    oss<<(*sipCpp);
-    std::string s(oss.str());
-    sipRes=s.c_str();
-%End
-};
+// %MethodCode
+//     std::ostringstream oss;
+//     oss<<(*sipCpp);
+//     std::string s(oss.str());
+//     sipRes=s.c_str();
+// %End
+// };

 class RotationalInertia
 {
@@ -488,6 +488,47 @@ public:

 };

+
+class ChainIkSolverPos_LMA : ChainIkSolverPos
+{
+%TypeHeaderCode
+#include <kdl/chainiksolverpos_lma.hpp>
+using namespace KDL;
+%End
+public:
+    ChainIkSolverPos_LMA(const Chain& chain, double eps=1E-5, int _maxiter=500, double _eps_joints=1E-15);
+
+    virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out);
+};
+
+
+class ChainIkSolverVel_pinv_nso : ChainIkSolverVel
+{
+%TypeHeaderCode
+#include <kdl/chainiksolvervel_pinv_nso.hpp>
+using namespace KDL;
+%End
+public:
+    ChainIkSolverVel_pinv_nso(const Chain& chain,double eps=0.00001,int maxiter=150, double alpha=0.25);
+        
+    virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
+};
+
+class ChainIkSolverVel_pinv_givens : ChainIkSolverVel
+{
+%TypeHeaderCode
+#include <kdl/chainiksolvervel_pinv_givens.hpp>
+using namespace KDL;
+%End
+public:
+    ChainIkSolverVel_pinv_givens(const Chain& chain);
+        
+    virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
+
+};
+
+
+
 class ChainJntToJacSolver
 {
 %TypeHeaderCode

Python_orocos_kdl: python API docs do not seem to be building

Not sure if these Python API docs were ever building but it seemed like a good idea to mention it here.

Additional suggestions for the fix >> orocos/orocos_kinematics_dynamics: 1cceebe

1. The condition of trace being larger than epsilon (1e-12) made sense before the fix, but now it is not necessary. Before the fix this was checking the condition of the (trace + 1 > epsilon) which is relevant for matrices with trace being near -1. (e.q. R = [-1,0,0; 0,-1,0;0,0,1])
   The new condition should be such that trace needs to be larger or equal to zero. Epsilon is not required. ( you can check this by looking into a matrix (eg. R = [0,-1,0; 0,0,1,-1,0,0]) which has a trace = 0).
2. The quaternion can be still normalized at the end of all the transformations to ensure the unity condition.

Unable to build python_orocos_kdl on OS X with brewed Python

Appears to be due to an upstream bug in the CMake PythonLibs module:

Homebrew/legacy-homebrew#25118

Would you consider a PR with a temporary workaround for OS X?

Dynamic reconfiguring of a Chain object inside a solver e.g., ChainDynParam

I need to dynamically change the dynamic parameters i.e. mass of a chain segment, which is used with a solver, such as e.g., ChainDynParam. Since I cannot allocate memory on the heap, it is not feasible for me to always create a new ChainDynParam object with the reconfigured chain as parameter.

What would be the right way to do this?

chainidsolver_recursive_newton_euler.cpp lines 36-38

1. The chainidsolver_recursive_newton_euler function makes sure that q.rows() is equal to nj; but AFAIK, if there are Joint::None, then q.rows() is actually less than nj but the algorithm should still be able to run. If this is the case, I suggest removing lines

   orocos_kinematics_dynamics/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp

   Lines 36 to 38 in 707ce2c

   	//Check sizes when in debug mode
   	if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
   	return (error = -1);

   .
2. In case we do agree on some kind of size check, the returned error code -1 should be modified as it currently collides with E_NO_CONVERGE=-1 and should be something else, e.g. E_BAD_SIZES=-2.

RigidBodyInertia addition

The addition operator for RigidBodyIneratia looks like it does the wrong thing [1]. Currently, the addition of two inertias is defined as the following:

    RigidBodyInertia operator+(const RigidBodyInertia& Ia, const RigidBodyInertia& Ib){
        return RigidBodyInertia(Ia.m+Ib.m,Ia.h+Ib.h,Ia.I+Ib.I,mhi);
    }

The key error being that it simply adds the positions of the centers of mass, h, instead of performing a weighted sum:

    RigidBodyInertia operator+(const RigidBodyInertia& Ia, const RigidBodyInertia& Ib){
        static const double eps = 1E-6;
        const double m = Ia.m+Ib.m;
        const Vector h = (m>eps) ? ( (Ia.m*Ia.h + Ib.m*Ib.h) / m ) : Vector::Zero() ;
        return RigidBodyInertia(m,h,Ia.I+Ib.I,mhi);
    }

[1]

orocos_kinematics_dynamics/orocos_kdl/src/rigidbodyinertia.cpp

Line 49 in de247c6

return RigidBodyInertia(Ia.m+Ib.m,Ia.h+Ib.h,Ia.I+Ib.I,mhi);

ChainIkSolverVel_wdls Fails on Assert

I recently received the same assert as this post: http://www.orocos.org/forum/orocos/orocos-users/position-only-ik:

python: /usr/include/eigen3/Eigen/src/Core/Block.h:141: Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>::Block(XprType&, Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>::Index, Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>::Index, Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>::Index, Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>::Index) [with XprType = Eigen::Matrix<double, -1, -1>; int BlockRows = -1; int BlockCols = -1; bool InnerPanel = false; Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>::Index = long int]: Assertion `a_startRow >= 0 && blockRows >= 0 && a_startRow <= xpr.rows() - blockRows && a_startCol >= 0 && blockCols >= 0 && a_startCol <= xpr.cols() - blockCols' failed.
Aborted (core dumped)

I signed up for the forum but still have not been granted login rights to post there. Thus I figured I'd email you before trying to hack around on the solution myself. I also couldn't find any commits on github relating to this issue, so I imagine it is still active.

I'm solving ChainIkSolverVel_wdls through ChainIkSolverPos_NR_JL() via Python in ros-indigo installed on Ubuntu 14.04.

Eigen version 3.2.0-8
ros-orocos-kdl Version: 1.3.0-0trusty-20151111-0032-+0000

For context, I'm trying to run position only IK on a 4 link finger. I have succesfully run FK as well as a full IK on the chains I create for the fingers.

I'm setting the task space weights as:

task_space_ik_weights = np.diag([1.0, 1.0, 1.0, 0.0, 0.0, 0.0]).tolist()

But I got the same error when giving an identity matrix.

I'm happy to report any more info that you need to help solve this. Also, happy to post this to the forum if my account gets activated.

Landing ABI changes in ROS Lunar

Hi @smits and orocos maintainers!

We will enter the beta testing phase of ROS Lunar at the end of the week. We try not to have any API/ABI breakage after that point. It was mentionned here that the current version was a placeholder before an ABI-incompatible version was released. Do you have any update on that ?

Cheers

Python bindings fail to build with python 3

[ 11%] Building CXX object CMakeFiles/python_module_PyKDL.dir/PyKDL/sipPyKDLpart7.o
/home/allenh1/catkin_ws/src/orocos_kinematics_dynamics/python_orocos_kdl/PyKDL/std_string.sip: In function ‘int convertTo_std_string(PyObject*, void**, int*, PyObject*)’:
/home/allenh1/catkin_ws/src/orocos_kinematics_dynamics/python_orocos_kdl/PyKDL/std_string.sip:41:37: error: ‘PyString_Check’ was not declared in this scope
         return (PyString_Check(sipPy) || PyUnicode_Check(sipPy));
                                     ^
/home/allenh1/catkin_ws/src/orocos_kinematics_dynamics/python_orocos_kdl/PyKDL/std_string.sip:48:58: error: ‘PyString_AS_STRING’ was not declared in this scope
         *sipCppPtr = new std::string(PyString_AS_STRING(s));
                                                          ^
/home/allenh1/catkin_ws/src/orocos_kinematics_dynamics/python_orocos_kdl/PyKDL/std_string.sip:52:30: error: ‘PyString_Check’ was not declared in this scope
      if (PyString_Check(sipPy)) {
                              ^
/home/allenh1/catkin_ws/src/orocos_kinematics_dynamics/python_orocos_kdl/PyKDL/std_string.sip:53:62: error: ‘PyString_AS_STRING’ was not declared in this scope
         *sipCppPtr = new std::string(PyString_AS_STRING(sipPy));
                                                              ^
/home/allenh1/catkin_ws/src/orocos_kinematics_dynamics/python_orocos_kdl/PyKDL/std_string.sip: In function ‘PyObject* convertFrom_std_string(void*, PyObject*)’:
/home/allenh1/catkin_ws/src/orocos_kinematics_dynamics/python_orocos_kdl/PyKDL/std_string.sip:30:56: error: ‘PyString_FromString’ was not declared in this scope
         newstring = PyString_FromString(sipCpp->c_str());
                                                        ^
CMakeFiles/python_module_PyKDL.dir/build.make:243: recipe for target 'CMakeFiles/python_module_PyKDL.dir/PyKDL/sipPyKDLpart7.o' failed

PyKDL and Accelerations

Dear,
I apologize, it is not an issue but just a question:
I was wondering if it is planned to make the python/cpp binding of the classes/methods for the joint accelerations management (base classes and ik/fk solvers).

Thanks
Nicola

Install cmake configs to share/orocos_kdl/cmake for catkin convenience

orocos_kdl installs CMake configuration files to share/orocos_kdl. Calling find_package(catkin REQUIRED COMPONENTS orocos_kdl) fails because it expects the config files to be in a subdirectory called cmake. The fix is to install the config files to share/orocos_kdl/cmake instead.

A workaround is to use find_package(orocos_kdl REQUIRED) and then use ${orocos_kdl_INCLUDE_DIRS} and ${orocos_kdl_LIBRARIES} directly. It would just be more convenient to find the package as a catkin component.

(thanks @dirk-thomas for the explanation)

merge rosdistro blocks for src/doc and release

As discussed at ros-infrastructure/rosinstall_generator#21 (comment), at least for hydro there are entries orocos_kdl and orocos_kinematics_dynamics, which should maybe be merged in to one entry orocos_kinematics_dynamics?

See https://github.com/ros/rosdistro/blob/master/hydro/distribution.yaml#L3053-L3070

Using Eigen for KDL::Vector, KDL::Rotation, KDL::RotationInertia, etc.

Hi,

I wonder why KDL::Vector and KDL::Rotation are not just Eigen Vector3d and Matrix3d.

I guess there is a problem of API/ABI compatibility (where the former is easier to solve).
I have the gut feeling that we might get some performance improvement if we drop our own Vector/Rotation/Frame implementation, but I guess that maybe I am missing something...

Regards

Davide

Release 1.3.1 for ROS kinetic?

ROS kinetic currently has version 1.3.0 only, while indigo and jade have 1.3.1:
http://repositories.ros.org/status_page/compare_indigo_jade_kinetic.html?q=orocos_kinematics_dynamics

Diff from 1.3.0 to 1.3.1:
v1.3.0...v1.3.1

PyKDL __repr__ method for Joint type generates error

/Users/jelleferinga/GIT/orocos_kinematics_dynamics/python_orocos_kdl/PyKDL/kinfam.sip:54:8: error: invalid
operands to binary expression ('std::ostringstream' (aka 'basic_ostringstream') and 'KDL::Joint')
oss<<(*sipCpp);
~~~^ ~~~~~~~~~

alpha default value ChainIkSolverVel_pinv_nso is 0.25, while that type is an int

the default constructor alpha value for ChainIkSolverVel_pinv_nso is 0.25, while that type is an int. not 100% sure, but assuming that type should be a double and that the default value is correct

diff --git a/orocos_kdl/src/chainiksolvervel_pinv_nso.cpp b/orocos_kdl/src/chainiksolvervel_pinv_nso.cpp
index eeffdd7..f5eefab 100644
--- a/orocos_kdl/src/chainiksolvervel_pinv_nso.cpp
+++ b/orocos_kdl/src/chainiksolvervel_pinv_nso.cpp
@@ -23,7 +23,7 @@

 namespace KDL
 {
-    ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, JntArray _opt_pos, JntArray _weights, double _eps, int _maxiter, double _alpha):
+    ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, JntArray _opt_pos, JntArray _weights, double _eps, int _maxiter, int _alpha):
         chain(_chain),
         jnt2jac(chain),
         jac(chain.getNrOfJoints()),
@@ -41,7 +41,7 @@ namespace KDL
     {
     }

-        ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, double _eps, int _maxiter, double _alpha):
+        ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, double _eps, int _maxiter, int _alpha):
         chain(_chain),
         jnt2jac(chain),
         jac(chain.getNrOfJoints()),
@@ -136,7 +136,7 @@ namespace KDL
        opt_pos = _opt_pos;
        return 0;
    }
-   int ChainIkSolverVel_pinv_nso::setAlpha(const double _alpha)
+   int ChainIkSolverVel_pinv_nso::setAlpha(const int _alpha)
    {
        alpha = _alpha;
        return 0;
diff --git a/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp b/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp
index 0b1df66..8577024 100644
--- a/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp
+++ b/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp
@@ -60,8 +60,8 @@ namespace KDL
          * @param alpha the null-space velocity gain
          *
          */
-        ChainIkSolverVel_pinv_nso(const Chain& chain, JntArray opt_pos, JntArray weights, double eps=0.00001,int maxiter=150, double alpha = 0.25);
-        explicit ChainIkSolverVel_pinv_nso(const Chain& chain, double eps=0.00001,int maxiter=150, double alpha = 0.25);
+        ChainIkSolverVel_pinv_nso(const Chain& chain, JntArray opt_pos, JntArray weights, double eps=0.00001,int maxiter=150, int alpha = 0.25);
+        explicit ChainIkSolverVel_pinv_nso(const Chain& chain, double eps=0.00001,int maxiter=150, int alpha = 0.25);
         ~ChainIkSolverVel_pinv_nso();

         virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
@@ -94,7 +94,7 @@ namespace KDL
     *@param alpha NUllspace velocity cgain
     *
     */
-   virtual int setAlpha(const double alpha);
+   virtual int setAlpha(const int alpha);

     private:
         const Chain chain;
@@ -109,7 +109,7 @@ namespace KDL
         double eps;
         int maxiter;

-   double alpha;
+   int alpha;
    JntArray weights;
    JntArray opt_pos;

orocos_kdl breaks catkin make under ROS Indigo

I am migrating to ROS Indigo. A package which uses the orocos-kdl ROS package breaks with the following error message:

CMake Error at /opt/ros/indigo/share/catkin/cmake/catkinConfig.cmake:75 (find_package):
Could not find a package configuration file provided by "orocos_kdl" with
any of the following names:
orocos_kdlConfig.cmake
orocos_kdl-config.cmake

The system is Ubuntu 14.04 freshly installed with ROS Indigo (full desktop). I have the following orocos packages installed:

dpkg -list | grep orocos
ros-indigo-orocos-kdl
ros-indigo-python-orocos-kdl

The package.xml listens the orocos_kdl package as build and as run dependency. CMakeLists include the package as

find_package(catkin REQUIRED COMPONENTS [snip] orocos_kdl [snip])

Looking at the installed package under /opt/ros/indigo/share shows:

beh1si@SI-Z0IF6:/opt/ros/indigo/share/orocos_kdl$ ls -1
orocos_kdl-config.cmake
orocos_kdl-config-version.cmake
OrocosKDLTargets.cmake
OrocosKDLTargets-none.cmake
package.xml

What bugs me is that all other packages have the -config.cmake files in a subfolder named *cmake.