MoveIt Motion Planning Failed: Unable to Sample Any Valid States for Goal Tree (Python Interface)
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 jointgoal section of this code and commenting the posegoal 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 setpositiontarget and setposetarget, 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. Thank you in advance!
EDIT 1: I tried using movegroup.setjointvaluetarget(posegoal, "linkwrist", False) and got the error:
moveit_commander.exception.MoveItCommanderException: Error setting joint target. Is IK running?
I also tried using movegroup.setjointvaluetarget(posegoal, "linkwrist", True) and got the error:
moveit_commander.exception.MoveItCommanderException: Error setting joint target. Does your IK solver support approximate IK?
EDIT 2: I see now that the issue is most likely the fact that I am trying to specify a 6DOF pose but have 4DOF robot arm, which will throw an error (duh, I guess that does make sense). Trying to find a workaround online, any help is appreciated. Thanks! In the meantime, I might try adding two fake joints that are constrained to not move at all and see if that works.
Asked by Ryan P on 2020-05-14 21:34:47 UTC
Answers
I had a similar problem with a 4DOF arm. I was able to solve it using IKFast as sugested in one comment above.
I have followed this tutorial using the TranslationZAxisAngleYNorm4D IK type: https://ros-planning.github.io/moveit_tutorials/doc/ikfast/ikfast_tutorial.html
Asked by mth_sousa on 2022-09-20 07:44:36 UTC
Comments
Would be great if you could answer this when you find a solution.
Asked by fvd on 2020-05-18 06:07:34 UTC
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.
Asked by Ryan P on 2020-05-18 12:15:53 UTC
Hello, have you found a solution for this? I'm facing the exactly same issue. I can move my 4DOF arm Rviz, but when it comes to python interface, it gives exactly same error.
Asked by Altai on 2022-04-10 06:54:34 UTC