MoveIt! pick and place error: Manipulation plan failed at stage 'reachable & valid pose filter'

[niga...@gmail.com]

Hello all,

I'm new to ROS so forgive me if this is a newbie question.  I've been attempting to get pick and place working on our robot and I've had no luck.  Currently, I'm adapting the following example from Patrick Goebel for our robot (code attached):

https://github.com/pirobot/rbx2/blob/hydro-devel/rbx2_arm_nav/scripts/moveit_pick_and_place_demo.py

I've changed all joint and pose names, and adapted the environment to fit our robot.  I've tried using different reference frames for the gripper, as well as changing the granularity of the poses generated.  The image attached shows the arm set to the grasp pose, and the red arrow is the gripper pose being published.  Every time I get the same error:

IK failed

Sampler failed to produce a state

Manipulation plan failed at stage 'reachable & valid pose filter'

I've been iterating through different solutions for a couple days now.  Anyone have any suggestions?

Thanks in advance,

Nate

updroid.com

[niga...@gmail.com]

Update:

I determined that the poses being generated where wildly out of range of the target object by commanding the arm to them explicitly.  Working backwards, I read the position of the arm when in the predefined "grasp" pose I created (shown in screenshot below), and then passed this position in as the target pose.  I then took the orientation of the grasp pose, converted it to Euler angles (roll, pitch, yaw), and iterated closely around those particular angles when generating grasp pose orientations.  Still no luck, though I think I'm getting closer.  Updated code attached.

[niga...@gmail.com]

I was informed that this problem may be a limitation of our 5DOF arm.  I generated the IKFast plugin (with help) for both arms to improve planning.  Also displayed the published grasps as axes and verified that they line up with the ideal gripping pose and that the gripper can be directly commanded to the location (see screenshots).  For the pre grasp approach I set the vector to be -z and for the post grasp retreat I set the vector to +z.  Still no luck.  Updated code attached.