Error when using Moveit Pick and Place with Jaco 2

[Five F]

When I create a plan with RViz the Trac IK works fine. But as soon as I try to do a complete Pick and Place I get the error below. With KDL it works in both cases.

[ERROR] [1448979787.390686362]: NaNs from NLOpt!! [ INFO] [1448979787.415879782]: Manipulation plan 25 failed at stage 'reachable & valid pose filter' on thread 2 move_group: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:394: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::DenseCoeffsBase<Derived, 1>::Index) [with Derived = Eigen::Matrix<double, -1, 1>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double; Eigen::DenseCoeffsBase<Derived, 1>::Index = long int]: Assertion `index >= 0 && index < size()' failed.

[Patrick Beeson]

Ok. From the limited messages you sent, it looks like NLOpt is getting a divide by zero error (which is it prone to do if the library user doesn't set up the NL problem properly), but the Eigen error looks to be from MoveIt! itself not properly initializing an Eigen data type with a size.

1) Can you please attach the full output, so that I can ensure the URDF was properly read in?

2) In any case, TRAC-IK should not be experiencing those NaN errors any more -- TRAC-IK should be enforcing that seed values are within limits. Can you please pull the git branch debug/Issue5, re-compile, and run with that? That branch currently only includes extra output statements, but hopefully, it will provide enough info for me to run TRAC-IK on the particular input that causes the NaN issues, and track down the problem.

[Five F]

Hey,

here the full log generated with the Issue5 branch: move_group.log

There are two chains loaded, one is the jaco only and the second is jaco + a linear actuator. But the request is sent only for the jaco.

[Patrick Beeson]

Short answer: I just pushed a bigfix to Issue5. Please try again.

Long answer: So before you pulled the Issue5 branch, I did add one extra bugfix in rare cases where a seed was outside the limit values. That would have been the obvious culprit for the NLOpt NaN's, as the starting point for the NL optimization would have violated the NL problem constraints. This seems to have fixed any NaN issues, as they are no longer in the log.

It looks to me, like your real issue is that something is not properly initializing an Eigen structure properly. This is probably a bug somewhere in MoveIt!, as I cannot see where in my code I would have an unsized Eigen structure.

However, that is probably the symptom of my plugin behaving incorrectly. The following error lead me to find the potential culrit. [ INFO] [1449057612.315181894]: Manipulation plan 18 failed at stage 'reachable & valid pose filter' on thread 1

This may be potential reason for failure during different MoveIt use cases (if the passed in error_code data structure is not initialized to SUCCESS).

[Patrick Beeson]

Short answer: I just pushed a bigfix to Issue5. Please try again.

[Patrick Beeson]

Testing shows that planning for a single grasp works. Planning for many grasps at once seems to fail. So, short term fix is to plan for a single grasp at a time.

[Patrick Beeson]

Fixed with commit 28460f7

MoveIt! can have multiple threads with the same copy of the IK plugin, which means that is the plugin needed to be thread safe.

[Patrick Beeson]

Fixed with commit 28460f7

[Edi Moreira M. de araujo]

which pick and place code that you guys are using?, c++ or python?