Using KDL Inverse Kinematics for Producibility Check?
| Submitted by benjaminbihler on Tue, 2012-08-14 07:44 |
Hello,
I have been using KDL::ChainIkSolverPos_NR_JL as inverse kinematics solver
for my robot chain. As described here
http://www.orocos.org/forum/orocos/orocos-users/kdl-ik-position-solver
there can be the problem, that an iterative solver does not find a solution
if the starting point is somehow unfriendly. By choosing different starting
points (perhaps randomly) the probability to find a solution can be raised.
Now my question: if I do not know for sure whether the robot can reach a
certain position and if I want to use the inverse kinematics to answer this
question, is KDL a good tool for this at all? How can I distinguish whether
there is no solution for the inverse kinematics problem or whether the
solver just has not found it yet?
Did anyone already use KDL for producibility checks? Which solver did you
use?
Thank you very much,
Benjamin
Using KDL Inverse Kinematics for Producibility Check?
Just for other users having the same problems as me: I have again made a
mistake. When I create the LMA solver like this
KDL::ChainIkSolverPos_LMA lmaSolver(chain, 1e-5 * 1e3, 500, 1e-15 * 1e3 );
the solver accepts iterates as solution even though they are no real
solutions to the problem (return value >=0, but the result is wrong).
Therefore I have adjusted my kinematic chain and enter all distances as
meter values now. Then I use the default values of the LMA constructor and
the solver can compute the correct solutions.
Using KDL Inverse Kinematics for Producibility Check?
Hello Benjamin,
On Tue, Aug 14, 2012 at 9:44 AM, Benjamin Bihler
<benjamin [dot] bihler [..] ...> wrote:
> Hello,
>
>
> I have been using KDL::ChainIkSolverPos_NR_JL as inverse kinematics solver
> for my robot chain. As described here
> http://www.orocos.org/forum/orocos/orocos-users/kdl-ik-position-solver
> there can be the problem, that an iterative solver does not find a solution
> if the starting point is somehow unfriendly. By choosing different starting
> points (perhaps randomly) the probability to find a solution can be raised.
>
> Now my question: if I do not know for sure whether the robot can reach a
> certain position and if I want to use the inverse kinematics to answer this
> question, is KDL a good tool for this at all? How can I distinguish whether
> there is no solution for the inverse kinematics problem or whether the
> solver just has not found it yet?
Currently it is not possible to distinguish between these two. Did you
already tried the new Levenberg-Marquardt based ik solver?
> Did anyone already use KDL for producibility checks? Which solver did you
> use?
>
Not that I know of. Typically generic numeric recursive solvers are
really bad at this. Closed-form solutions are in this case a much
better option.
>
> Thank you very much,
> Benjamin
>
Ruben
> --
> Orocos-Users mailing list
> Orocos-Users [..] ...
> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
Using KDL Inverse Kinematics for Producibility Check?
Please keep the list in CC!
On Tue, Aug 14, 2012 at 1:54 PM, Benjamin Bihler
<benjamin [dot] bihler [..] ...> wrote:
> On Tue, Aug 14, 2012 at 11:53 AM, Ruben Smits <ruben [dot] smits [..] ...>
> wrote:
>
> Hi Ruben,
>
>> Currently it is not possible to distinguish between these two. Did you
>> already tried the new Levenberg-Marquardt based ik solver?
>
> yes, I have. I have used the constructor providing the default weight
> matrix. But this solver cannot solve even one of the inverse kinematic
> problems.
Are you sure the destination is reachable. Do not forget that these
are 6D solvers which means that the rotations are also accounted for.
ruben
> I have used it like this:
>
> KDL::ChainIkSolverPos_LMA lmaSolver(chain);
> int ret = lmaSolver.CartToJnt(q_init, F_dest, q);
>
> The return value is always -3 (number of iterations is exceeded). Is it
> wrong to use the default weight matrix?
>
> Thank you and Ciao,
> Benjamin
>
Using KDL Inverse Kinematics for Producibility Check?
Hi,
the LMA solver works now when I adjust eps and eps_joints as written before.
Still there is a problem when trying to use it for a producibility check:
the LMA solver now always finds a solution, even in the case of positions
the robot cannot reach, since the LMA solver does not care for upper and
lower limits for joint angles like ChainIkSolverPos_NR_JL does.
Are there any assumptions I can make, like "If LMA finds a solution outside
the legal joint angles than there is no solution inside."? Probably not!?
Is there anyone else trying to use Orocos KDL for a producibility check? Or
are there any documents describing how to deal with several solutions of the
inverse kinematic problem and how to decide which is the one that the robot
will choose?
Thanks and Bye,
Benjamin
Using KDL Inverse Kinematics for Producibility Check?
Hi Ruben,
On Tue, Aug 14, 2012 at 2:00 PM, Ruben Smits <ruben.smits at
intermodalics.eu>
wrote:
> Are you sure the destination is reachable. Do not forget that these
> are 6D solvers which means that the rotations are also accounted for.
The behavior of the LMA solver is strongly depending on the numbers eps and
eps_joints. If I call the constructor like this:
KDL::ChainIkSolverPos_LMA lmaSolver(chain, 1e-5 * 1e3, 500, 1e-15 * 1e3 );
the LMA solver also finds a solution in the millimeter case of the example I
have posted before. I will keep on playing with these numbers...
Bye,
Benjamin
Using KDL Inverse Kinematics for Producibility Check?
To answer my own question...
We use the LMA solver now for producibility checks. If it finds a solution
inside the joint limits, the point is obviously reachable. And if the found
solution lies outside the joint limits we count it as unreachable. This
seems to be quite realistic. At least some points we have checked with the
robot were really not reachable if the solution found by the LMA solver was
outside the joint limits.
Using KDL Inverse Kinematics for Producibility Check?
Hi Ruben,
On Tue, Aug 14, 2012 at 2:00 PM, Ruben Smits <ruben [dot] smits [..] ...>
wrote:
> Are you sure the destination is reachable. Do not forget that these
> are 6D solvers which means that the rotations are also accounted for.
I have more hints now. The solvers seem to depend on the order of magnitude
of the numbers. Please check the source code below. If I enter the chain
dimensions in meter, both solvers find solutions, but if I enter the
dimensions in millimeter (i.e. multiply everything by 1000), only the
iterative solver is able to find at least some solution.
Is this a general rule for building up kinematic chains? It does not make a
very good impression, but of course I could divide everything by 1000 as a
quick workaround.
Thank you again for your help! Any answer is greatly appreciated.
Benjamin
Sourcecode:
#include <chain.hp
#include <chainfksolver.hp
#include <chainfksolverpos_recursive.hp
#include <chainiksolverpos_lma.hp
#include <chainiksolvervel_pinv.hp
#include <chainiksolverpos_nr.hp
#include <chainiksolverpos_nr_jl.hp
#include <frames_io.hp
#include <stdio.h>
#include <iostream>
using namespace KDL;
void solve(KDL::Chain& chain)
{
// 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++)
{
jointpositions(i) = (double) i;
}
// 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 :(");
}
KDL::JntArray q_init = JntArray(nj);
for (unsigned int index = 0; index < nj; ++index)
{
q_init(index) = 0.0;
}
KDL::JntArray q = JntArray(nj);
//Creation of the solvers:
KDL::ChainIkSolverVel_pinv iksolver1v(chain); //Inverse velocity
solver
KDL::ChainIkSolverPos_NR iksolver1(chain, fksolver, iksolver1v);
KDL::ChainIkSolverPos_LMA lmaSolver(chain);
int returnIterativeSolver = iksolver1.CartToJnt(q_init, cartpos, q);
std::cout << "Iterative Solver: " << returnIterativeSolver <<
std::endl;
std::cout << "q:" << std::endl;
for (int index = 0; index < 6; ++index)
{
std::cout << q(index) << std::endl;
}
int returnLmaSolver = lmaSolver.CartToJnt(q_init, cartpos, q);
std::cout << "LMASolver: " << returnLmaSolver << std::endl;
std::cout << "q:" << std::endl;
for (int index = 0; index < 6; ++index)
{
std::cout << q(index) << std::endl;
}
}
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, -1.0),
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, -1.0),
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),
KDL::Frame(KDL::Vector(0.010, 0.020, 0.030))));
std::cout << std::endl << "Lengths in m:" << std::endl;
solve(chain);
}
{
//Definition of a kinematic chain & add segments to the
chain
KDL::Chain chain2;
chain2.addSegment(
Segment(Joint(Joint::RotZ, -1.0),
Frame(Vector(0.0, 0.0,
1020.0))));
chain2.addSegment(
Segment(Joint(Joint::RotX),
Frame(Vector(0.0, 0.0, 480.0))));
chain2.addSegment(
Segment(Joint(Joint::RotX, -1.0),
Frame(Vector(0.0, 0.0,
645.0))));
chain2.addSegment(Segment(Joint(Joint::RotZ)));
chain2.addSegment(
Segment(Joint(Joint::RotX),
Frame(Vector(0.0, 0.0, 120.0))));
chain2.addSegment(
Segment(Joint(Joint::RotZ),
KDL::Frame(KDL::Vector(10.0,
20.0, 30.0))));
std::cout << std::endl << "Lengths in mm:" << std::endl;
solve(chain2);
}
}
Using KDL Inverse Kinematics for Producibility Check?
On Tue, Aug 14, 2012 at 2:00 PM, Ruben Smits <ruben [dot] smits [..] ...>
wrote:
> Are you sure the destination is reachable. Do not forget that these are 6D
solvers which means that the rotations are also accounted for.
Yes, I am. The iterative solver finds solutions. With 6D solver you mean,
that (in KUKA-speak) not only X, Y and Z, but also A, B, C are part of the
solution, right? Then: yes, the solutions found by the iterative solver are
real solutions, but the LMA solver does not find them.
Benjamin