MoveIt Motion Planning Failed: Unable to Sample Any Valid States for Goal Tree (Python Interface)

edit

Hello ROS community!

So I've been banging my head against a wall with this issue for many hours now and thought I would post a question here. I have a 4DOF arm that I've built, and I've uploaded the URDF properly such that I can move the arm manually on RViz user interface (via the demo.launch file) to random valid positions without an issue.

However, when I go to use the Python interface for Moveit, I get the error:

[ERROR] [1589507918.238474601]: agrobot_arm/agrobot_arm: Unable to sample any valid states for goal tree [INFO] [1589507918.238523629]: agrobot_arm/agrobot_arm: Created 1 states (1 start + 0 goal) [INFO] [1589507918.238580646]: No solution found after 0.112091 seconds [ INFO] [1589507918.238608377]: Unable to solve the planning problem

My script that I am running in Python is as follows:

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('motion_plan_test', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "agrobot_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                               moveit_msgs.msg.DisplayTrajectory,
                                                   queue_size=20)

# PLANNING EXAMPLE POSE MOVEMENT
# joint_goal = move_group.get_current_joint_values()
# joint_goal[0] = pi/6
# joint_goal[1] = -pi/4
# joint_goal[2] = -pi/4
# joint_goal[3] = pi/4
#
# move_group.go(joint_goal, wait=True)
# move_group.stop()
#rospy.sleep(2)

print(move_group.get_current_pose())
move_group.set_goal_tolerance(0.01)
move_group.set_planning_time(5)
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.w = 0.56100798
pose_goal.orientation.x = 0.7010547
pose_goal.orientation.y = -0.09231608
pose_goal.orientation.z = 0.43043006
pose_goal.position.x = 0.09746
pose_goal.position.y = -0.02658
pose_goal.position.z = 0.32816

move_group.set_pose_target(pose_goal, "link_wrist")

move_group.go(pose_goal)

For some reason, when I send joint commands (by uncommenting the joint_goal section of this code and commenting the pose_goal section), everything works fine and I can see the robot arm move in RViz. However, when I try to move the arm via a pose goal, it throws this error. I know the pose is reachable by the arm because it is the exact same pose that is reached when I give the joint commands that are in the joint_goal section. I've scraped around online and tried some things. I'm fairly new at Moveit and ROS, so I was trying whatever suggestions I could find:

1) Running sudo apt-get update and sudo apt-get dist-upgrade

2) Changed my IK solver to IKFast from KDL (because I heard the default IK solver doesn't work well for <6DOF?) with the TranslationZAxisAngle4D IK type.

3) Messed around with the planning times and goal tolerances

4) Trying both set_position_target and set_pose_target, with and without "link_wrist" as one of the arguments

5) Tried moving the arm to some random valid position first (in case the starting position was at a singularity or something) and then sending a pose_goal command

I'm very stumped and hoping it is a careless error due to my lack of coding experience. Does anyone have a suggestion? Please let me know if you need any more info, and I'll try to respond to the best of my knowledge ... (more)