search

pantor / frankx

High-Level Motion Library for Collaborative Robots
https://pantor.github.io/frankx/
GNU Lesser General Public License v3.0
270 stars 65 forks source link

forwardKinematics is not working consistent? #27

Open peetCreative opened 3 years ago

peetCreative commented 3 years ago

While adding some tests to my library I came across strange behaviors of the function Robot::forwardKinematics. So I added tests #26 to frankx. The first test simply checks if Robot::forwardKinematics with Robot::currentJointPositions results in the same Affine as Robot::currentPose. As I see in the implementation these functions read (readOnce) from libfranka. So I ask myself, if the implementation of forwardKinematics is incorrect. However I suspect my self to have a misconception as this function is one of the central functions.

Maybe you want to add the tests anyway to the library. Thanks

peetCreative commented 3 years ago

After a bit debugging I added some tests which showed me, that the Transformation calculated by Kinematics::forward is exactly is just 180° (opposit direction) to the expected. The pairs of corresponding pose and joint positions the test expected I read by small a home example application. I started having a look at your forward implementation but was not sure.. You might integrate the test if you want.. Thanks

pantor commented 2 years ago

So the forward and inverse kinematics implementation are really just a minor spin-off of this library. They are (and should not be) used for central functions, and do much worse in terms of speed, accuracy and robustness than KDL or other solvers. I just use them to calculate an elbow angle that matches my requirements.

As these methods are quite tailored to my needs, I've probably added a fixed offset to the kinematics calculation, leading to this 180° difference between the official and my end-effector position. Please also note that some things changed with libfranka version v0.8.0 in this regard. I'll have a look at your tests soon.

firstPrinciple1 commented 2 years ago

@pantor @peetCreative Did you manage to resolve these issues in the meantime?