Pick server works when I send it 1 grasp at a time, but if I send multiple I get an error related to Orocos IK / Eigen / IKConstraintSampler
ERROR ros.moveit_ros_manipulation: Manipulation plan failed at stage 'reachable & valid pose filter' on thread 1
DEBUG ros.moveit_ros_planning: searchPositionIK2: Position request pose is 0.250475 0.000301798 0.169484 -0.000166572 0.70527 0.002466 0.708934
move_group_action_server: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:407: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::DenseCoeffsBase<Derived, 1>::Index) [with Derived = Eigen::Matrix<double, -0x00000000000000001, 1>, Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double, Eigen::DenseCoeffsBase<Derived, 1>::Index = long int]: Assertion `index >= 0 && index < size()' failed.
move_group_action_server: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:407: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::DenseCoeffsBase<Derived, 1>::Index) [with Derived = Eigen::Matrix<double, -0x00000000000000001, 1>, Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double, Eigen::DenseCoeffsBase<Derived, 1>::Index = long int]: Assertion `index >= 0 && index < size()' failed.
Program received signal SIGABRT, Aborted.
[Switching to Thread 0x7fffb57fa700 (LWP 29286)]
0x00007ffff4024425 in __GI_raise (sig=<optimized out>) at ../nptl/sysdeps/unix/sysv/linux/raise.c:64
64 ../nptl/sysdeps/unix/sysv/linux/raise.c: No such file or directory.
(gdb) bt
#0 0x00007ffff4024425 in __GI_raise (sig=<optimized out>) at ../nptl/sysdeps/unix/sysv/linux/raise.c:64
#1 0x00007ffff4027b8b in __GI_abort () at abort.c:91
#2 0x00007ffff401d0ee in __assert_fail_base (fmt=<optimized out>,
assertion=0x7ffff2c89ff1 "index >= 0 && index < size()",
file=0x7ffff2c89650 "/usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h", line=<optimized out>,
function=<optimized out>) at assert.c:94
#3 0x00007ffff401d192 in __GI___assert_fail (assertion=0x7ffff2c89ff1 "index >= 0 && index < size()",
file=0x7ffff2c89650 "/usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h", line=407,
function=0x7ffff2c98840 "Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::DenseCoeffsBase<Derived, 1>::Index) [with Derived = Eigen::Matrix<double, -0x", '0' <repeats 16 times>, "1, 1>, Ei"...) at assert.c:103
#4 0x00007ffff2c5fcfe in KDL::JntArray::operator()(unsigned int, unsigned int) ()
from /opt/ros/groovy/lib/liborocos-kdl.so.1.1
#5 0x00007ffff2c83d28 in KDL::SVD_HH::calculate(KDL::Jacobian const&, std::vector<KDL::JntArray, std::allocator<KDL::JntArray> >&, KDL::JntArray&, std::vector<KDL::JntArray, std::allocator<KDL::JntArray> >&, int) ()
from /opt/ros/groovy/lib/liborocos-kdl.so.1.1
#6 0x00007ffff2c50c0d in KDL::ChainIkSolverVel_pinv::CartToJnt(KDL::JntArray const&, KDL::Twist const&, KDL::JntArray&) () from /opt/ros/groovy/lib/liborocos-kdl.so.1.1
#7 0x00007ffff2c7df59 in KDL::ChainIkSolverPos_NR_JL::CartToJnt(KDL::JntArray const&, KDL::Frame const&, KDL::JntArray&) () from /opt/ros/groovy/lib/liborocos-kdl.so.1.1
#8 0x00007fffcbdf51de in kdl_kinematics_plugin::KDLKinematicsPlugin::searchPositionIK(geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, double, std::vector<double, std::allocator<double> >&, boost::function<void (geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&)> const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&, std::vector<double, std::allocator<double> > const&) const ()
from /home/dave/ros/moveit/devel/lib//libmoveit_kdl_kinematics_plugin.so
#9 0x00007fffcbdf5cae in kdl_kinematics_plugin::KDLKinematicsPlugin::searchPositionIK(geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, double, std::vector<double, std::allocator<double> >&, boost::function<void (geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&)> const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&) const () from /home/dave/ros/moveit/devel/lib//libmoveit_kdl_kinematics_plugin.so
#10 0x00007ffff09cb7d7 in constraint_samplers::IKConstraintSampler::callIK(geometry_msgs::Pose_<std::allocator<void> > const&, boost::function<void (geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&)> const&, double, robot_state::JointStateGroup*, bool) () from /home/dave/ros/moveit/devel/lib/libmoveit_constraint_samplers.so
#11 0x00007ffff09cf12e in constraint_samplers::IKConstraintSampler::sampleHelper(robot_state::JointStateGroup*, robot_---Type <return> to continue, or q <return> to quit---
state::RobotState const&, unsigned int, bool) ()
from /home/dave/ros/moveit/devel/lib/libmoveit_constraint_samplers.so
#12 0x00007ffff507a6e9 in pick_place::ReachableAndValidPoseFilter::evaluate(boost::shared_ptr<pick_place::ManipulationPlan> const&) const () from /home/dave/ros/moveit/devel/lib/libmoveit_pick_place_planner.so
#13 0x00007ffff50729be in pick_place::ManipulationPipeline::processingThread(unsigned int) ()
from /home/dave/ros/moveit/devel/lib/libmoveit_pick_place_planner.so
#14 0x00007ffff6a72ce9 in thread_proxy () from /usr/lib/libboost_thread.so.1.46.1
#15 0x00007ffff6850e9a in start_thread (arg=0x7fffb57fa700) at pthread_create.c:308
#16 0x00007ffff40e1cbd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:112
#17 0x0000000000000000 in ?? ()