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

asked 2020-05-14 21:49:22 -0500

Ryan P gravatar image

updated 2020-05-15 20:40:45 -0500

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)

edit retag flag offensive close merge delete

Comments

Would be great if you could answer this when you find a solution.

fvd gravatar image fvd  ( 2020-05-18 06:07:34 -0500 )edit

Unfortunately, I haven't gotten a solution yet. Still trying different things, but I ultimately haven't gotten IKFast to generate a working solution with IKFast's Translation3D and Translation4D IK types. Haven't tried adding two fake links/joints to bump the # of DOFs to 6 yet, though, so I'll give that a shot later.

Ryan P gravatar image Ryan P  ( 2020-05-18 12:15:53 -0500 )edit