It is a bit hard to know the exact cause of the error without having the (RobWork), I tried to install it following the link your provided, but I ran into some problems and I don't have the time to fix them at the moment. But I can point you to some issues that might be the source of your problem:
- The
transform
method you are given in this code doesn't have a return statement, though you have declared it to return arw::math::Transform3D<>
. - If you are near a singularity the inverse kinematics will normally return high values. The singularities depend on the robot structure you are using. You can detect this by seeing the determinant of your jacobian matrix, the closer it is to zero the closer you are to a singularity.
- Also I don't understand why you use the line
rw::math::Q dq(q)
, because normally the inverse kinematic should returndq
if your desired displacement is small enough (i.e.du
) so normally you should be able to add it directly to addq
directly to theq_current
, so may be it could be of interest to check the raw values for these variables.
Also may be it could be more accurate to use a polynomial interpolater to produce the du
and then run the whole inverse kinematic in a while loop with a boos::timer
for instance. I have actually developed and inverse kinematic for Crustcrawler arm that does that, but I use joint velocity control directly since the output of the inverse kinematic is actually joints velocity, also I am not binding myself to move in sphere I just give a goal position and duration and the model drive the end effecter nicely to it via joints velocities.
If you have more details please share them, it could help in identifying the issue more effectively. Like how invKin
is called? and what exactly are those parameters passed to it (i.e. x, y, z
)?