Inverse kinematics. Currrent state of the arm not taken into account to compute the solution?

[ernestspaceapps]

I'm using this library for a 7 DOF arm. When I compute the inverse kinematics the solution obtained is correct. The problem occurs when I compute ik for a pose near the current one, the solution is normally very far at joint space, i.e. the joint angles are very different from the current ones. This happens always and it's not related to a singular position. Am I right and the algorithm is not taking into account the current state of the arm? If it is implemented, why am I getting these results? this happens with JacobianInverseKinematics and NloptInverseKinematics methods.

[rickertm]

The iterative IK algorithms start with the current configuration specified via setPosition() in the given rl::mdl::Kinematic model. They will retry from a randomly generated configuration in case they get stuck until the duration or iteration limit is reached. You can influence this by setting duration and iterations to zero. NloptInverseKinematics also allows to set upper/lower bounds to limit the search to a given section of the configuration space, e.g., the joint ranges suitable for your task similar to "left/right", "elbow up/down" in analytical IKs.

[ernestspaceapps]

Still the same results. This is my code, am I doing something wrong?
joint_pos_now = current joint state of the robot
q_2 = cartesian position goal

    rl::mdl::UrdfFactory urdf_factory;
    std::shared_ptr<rl::mdl::Model> model(urdf_factory.create("/dir/robot.urdf"));
    rl::mdl::Kinematic *kinematics = dynamic_cast<rl::mdl::Kinematic *>(model.get());
    kinematics->setPosition(joint_pos_now);
    
    rl::mdl::NloptInverseKinematics ik(kinematics);
    
    rl::math::Transform goal;
    goal.linear() = rl::math::AngleAxis(q_2(5) * rl::math::DEG2RAD, rl::math::Vector3::UnitZ()) *
                    rl::math::AngleAxis(q_2(4) * rl::math::DEG2RAD, rl::math::Vector3::UnitY()) *
                    rl::math::AngleAxis(q_2(3) * rl::math::DEG2RAD, rl::math::Vector3::UnitX()).toRotationMatrix();
    goal.translation() = rl::math::Vector3(q_2(0), q_2(1), q_2(2));
    ik.goals.push_back(::std::make_pair(goal, 0));

    bool result = ik.solve();
    rl::math::Vector solution = kinematics->getPosition() * rl::math::RAD2DEG;

[rickertm]

Even with an initial guess provided via setPosition(), both iterative solvers will keep retrying with random configurations as starting point until the given duration or iteration limits have been reached.

For NloptInverseKinematics you can specify lower/upper bounds via setLowerBounds() and setUpperBounds() to limit the search to your desired part of the configuration space, e.g. +45° to -45° instead of the max/min values for a joint.

If you only want to find solutions that can be reached via a direct inverse/transpose Jacobian iteration from your initial guess, you can use JacobianInverseKinematics and setDuration(std::chrono::seconds(0)) and setIterations(0) to skip random retries after the first failure.

Please make sure to check the value of result to see if the solver was able to find a valid solution, otherwise solution will be set to the value of the (failed) last iteration attempt.

[ernestspaceapps]

Then, what is the point of having an initial guess? If there is an option to give an initial guess it's because you want to have the closest solution to this initial guess, if you randomly look for a solution giving an initial guess is pointless.

I would suggest you work a little bit on this inverse kinematics function. It would be very useful to have a solution near the initial guess to use it for trajectory interpolations.

By the way, these setDuration and setIterations functions don't exist... You have to use the object attributes duration and iterations.

Going back to my problem, moving the end-effector only 2mm in the x-direction I get these two joint configurations:
-94.1912 54.4802 166.503 129.564 -11.2846 103.89 75.1267
90.1076 87.1461 -166.87 130.232 18.8056 135.269 -75.6278

Is there a way to have closer results or should I start looking for another library?

[rickertm]

The initial guess is the starting value for the iterative solvers and the solutions adhere to the specified boundary conditions. In particular for NLopt's SLSQP, you can only influence the mentioned parameters and tolerances. The random restarts are only performed in case the solvers fail and the alternative would be to return no solution. If the Jacobian solver cannot find a solution from the initial guess without restart, the target pose likely requires a different joint configuration and a straight-line interpolation is not possible due to singularities or joint limits.

Adding a parameter to the iterative solvers that defines the number of random restarts is certainly possible. The setDuration and setIteration functions exist on the current master branch, as do the setLowerBounds and setUpperBounds functions of NloptInverseKinematics. Version 0.7 still uses public members variables.

For trajectory interpolation I would recommend to directly use the Jacobian matrix inverse similar to how JacobianInverseKinematics works so you can decide how to handle moving through singularities and control velocity and acceleration values.

[xianjimli]

For trajectory interpolation I would recommend to directly use the Jacobian matrix inverse similar to how JacobianInverseKinematics works so you can decide how to handle moving through singularities and control velocity and acceleration values.

Trajectory interpolation is a commonly used function that many people may need. Could you provide an example?