Direct kinematics computation on Poppy

To make Poppy walk, we need to compute the direct kinematics of the robot as fast as possible.

Here is a method using pure numpy (with some special things)

We also test the library pyKDL which seems powerful but very tricky to install.

Do you have others idea ? Have you ever computed the complete direct kinematics of the robot with Jacobian matrix ?

Very interested in :stuck_out_tongue:


By looking at the results from the github, it seems that it take less than 1ms… Is it right?

In this case I’m not sur it make much sense to try to go faster than that knowing that you will most likely wait 19 other ms before pypot send the value to the real robot :smile:

First of all, the results on github are a test on 2DOF robot (equivalent to Poppy head). We have to transpose on the 27DOF Poppy (with pitch and roll angles)

Then, the purpose is to use optimisation to control the robot in real time. And here, there is a great improvement since I now compute the Jacobian of the robot analytically (using cross product) instead of numerical differenciation.
I am going to measure the time gain in a next post on Poppy. But… :smiley:

I already try to do this kind of thing with OpenRave, and it’s REALLY efficient.
OpenRave use a Python script and a decriptve XML file to create a compilable C++ code.

If you are interested about OpenRave, I advise you to talk with @XdoctorwhoZ!

1 Like

Thanks for the link ! I will compare later with the numpy solution.
I did a function in which you can choose to compute Jacobian or not, and you can exclude arms, head or whatever.
Finally here are the performances :


  • without Jacobian : 976µs
  • with Jacobian : 3723µs

Intel i5:

  • without Jacobian : 234µs
  • with Jacobian : 922µs

It is completely acceptable since have 16ms remaining to do other things :smiley:

1 Like

Hi Thot,

Direct kinematics is generally best computed with algebraic or geometric methods rather than matrices since they only use effective (non-zero ) terms. But in any case, for a serial robot, direct kinematics is quite easy to compute. Inverse kinematics can be more tricky, depending of the choosen kinematics. I do not have a whole poppy assembled but if shoulder and wrist articulations (if we consider for instance, the arm) have coincident axis, resolving inverse kinematics should’nt require any sophisticated Library.
Howhever I have tested Peter CORKE Library, freely available (and partially implemented in pypot) and it works finely. But its a generic one, meaning it is intented to resolve any kind of motors combinations. So, it is not as safe and efficient than a dedicated piece of software. Another advantage is that Peter Corke Library is both implemented in Matlab and Python (with extensive documentation. A really great great work).

It is maybe the right place to discuss about the way dynamixel positions are converted in pypot.
I think there is maybe something wrong in the way dynamixel encoder_ to_ degrees and degrees_ to_ encoder values are computed in the module *’/pypot/dynamixel/’:
the functions

              def dxl_to_degrees(value)    and    def degrees_to_dxl(value):  

use 4095 encoder values rather than 4096 which (I think) introduce an 0.08° error or a 1 encoder point in the functions results. Since there are 4096 intervals in a whole rotation (12 bits), we should use 4096 rather than 4095.

1 Like