MoveIt! unable to sample any valid states for goal tree

asked 2019-05-02 11:11:19 -0500

kump gravatar image

updated 2019-05-03 02:21:52 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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 gravatar imagepmuthu2s ( 2019-05-03 08:49:43 -0500 )edit

@pmuthu2s When setting only the w to the 1, nothing changed. The terminal output is the same.

kump gravatar imagekump ( 2019-05-03 09:04:24 -0500 )edit

Hmm! I think,we need to wait for some expert opinion!

pmuthu2s gravatar imagepmuthu2s ( 2019-05-03 09:31:08 -0500 )edit

Facing the same problem. Any solution found?

macrot51 gravatar imagemacrot51 ( 2019-07-09 03:53:33 -0500 )edit

@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.

kump gravatar imagekump ( 2019-07-09 06:38:37 -0500 )edit