MoveIt! unable to sample any valid states for goal tree
I have a robotic arm integrated with MoveIt! and Gazebo. When I launch the Gazebo simulation and MoveIt! controller, I can use a python script which, using the moveit_commander
, can set the joint states of the robotic arm in the Gazebo (set the angles). But When I want the MoveIt! to plan the trajectory given just the pose of the end effector, I get an error
RRTConnect: Unable to sample any valid states for goal tree
The pose I am setting to as the goal pose was obtained after moving robot to some valid position and calling get_current_pose().pose
on the moveit_commander
object, therefor I believe the position is correct and achievable.
The python script is shown below.
#!/usr/bin/env python
import sys
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from moveit_commander.conversions import pose_to_list
def all_close(goal, actual, tolerance):
"""
Convenience method for testing if a list of values are within a tolerance of their counterparts in another list
@param: goal A list of floats, a Pose or a PoseStamped
@param: actual A list of floats, a Pose or a PoseStamped
@param: tolerance A float
@returns: bool
"""
all_equal = True
if type(goal) is list:
for index in range(len(goal)):
if abs(actual[index] - goal[index]) > tolerance:
return False
elif type(goal) is geometry_msgs.msg.PoseStamped:
return all_close(goal.pose, actual.pose, tolerance)
elif type(goal) is geometry_msgs.msg.Pose:
return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)
return True
class MoveItContext(object):
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('robotic_arm_moveit', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
planning_frame = move_group.get_planning_frame()
end_effector_link = move_group.get_end_effector_link()
group_names = robot.get_group_names()
self.box_name = ''
self.robot = robot
self.scene = scene
self.move_group = move_group
self.planning_frame = planning_frame
self.end_effector_link = end_effector_link
self.group_names = group_names
def go_to_pose_goal(self):
print("Starting planning to go to pose goal.\n")
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.w = 0.978
pose_goal.orientation.x = 0.155
pose_goal.orientation.y = 0.127
pose_goal.orientation.z = 0.051
pose_goal.position.x = -0.1073
pose_goal.position.y = -0.5189
pose_goal.position.z = 1.6552
self.move_group.set_pose_target(pose_goal)
plan = self.move_group.go(wait=True)
self.move_group.stop()
self.move_group.clear_pose_targets()
# For testing:
current_pose = self.move_group.get_current_pose().pose
return all_close(pose_goal, current_pose, 0.1)
def main():
moveit_context = MoveItContext()
moveit_context.go_to_pose_goal()
moveit_context.go_to_home_pose_goal()
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass
Here is the full terminal output:
[ INFO] [1556812617.272210690, 11.282000000]: Ready to take commands for planning group arm.
Starting planning to go to pose goal.
[ INFO] [1556812617.494753682, 11.504000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1556812617.495350350, 11.505000000]: Planning attempt 1 of at most 1
[ INFO] [1556812617.497207153, 11.507000000]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1556812617.498081362, 11.508000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1556812617.558521982, 11.568000000]: RRTConnect: Unable to ...
Can you just set w to 1, and remove the rest of the quaternions. (don't make it to 0, but remove all) and tell me what happens? Even I'm facing the same problem!
@pmuthu2s When setting only the w to the 1, nothing changed. The terminal output is the same.
Hmm! I think,we need to wait for some expert opinion!
Facing the same problem. Any solution found?
@macrot51 no, sorry. When I used different robotic arm, the script worked. So I assume there is something wrong with the robotic arm model or the MoveIt! setting.
I'm having the same issue right now, except that it stops after 5 seconds. Anyone found a solution yet?
Setting goal tolerance group.set_goal_tolerance(0.5) worked, but this is too high a number. Setting it lower doesn't work. RviZ planning works, so suspect this is something to do with Inverse Kinematics, as we don't need IK solver in Rviz.
Hi, I am having nearly the same problem. @sritee and @kump Did one of you solve this problem?