Robotics StackExchange | Archived questions

MoveIt! unable to sample any valid states for goal tree

I have a robotic arm integrated with MoveIt! and Gazebo using ROS Noetic. 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

Unable to sample any valid states for goal tree

class TaskServer(object):
    result_ = ArduinobotTaskResult()
    arm_goal_ = []
    gripper_goal_ = []

    def __init__(self, name):
        # Constructor
        # function that inizialize the ArduinobotTaskAction class and creates 
        # a Simple Action Server from the library actionlib
        # Constructor that gets called when a new instance of this class is created
        # it basically inizialize the MoveIt! API that will be used throughout the script
        # initialize the ROS interface with the robot via moveit
        moveit_commander.roscpp_initialize(sys.argv)

        # create a move group commander object that will be the interface with the robot joints
        self.arm_move_group_ = moveit_commander.MoveGroupCommander('arduinobot_arm')

        # create a move group commander object for the gripper
        self.gripper_move_group_ = moveit_commander.MoveGroupCommander('arduinobot_hand')

        self.scene = moveit_commander.PlanningSceneInterface()

        self.display_trajectory_publisher = rospy.Publisher(
            "/move_group/display_planned_path",
            moveit_msgs.msg.DisplayTrajectory,
            queue_size=100,
        )

        self.action_name_ = name
        self.as_ = actionlib.SimpleActionServer(self.action_name_, ArduinobotTaskAction, execute_cb=self.execute_cb, auto_start = False)
        self.as_.start()


    def execute_cb(self, goal):

        success = True     

        # start executing the action
        # based on the goal id received, send a different goal 
        # to the robot
        if goal.task_number == 1:
            self.arm_goal_ = geometry_msgs.msg.Pose()
            self.arm_goal_.orientation.w = 1
            self.arm_goal_.orientation.x = 1e-6
            self.arm_goal_.orientation.y = 1e-6
            self.arm_goal_.orientation.z = 1e-6
            self.arm_goal_.position.x = -0.410766
            self.arm_goal_.position.y = 0.711690
            self.arm_goal_.position.z = 0.204635
            # self.arm_move_group_.set_goal_tolerance(0.5)
            self.arm_move_group_.set_pose_target(self.arm_goal_ )
            self.arm_move_group_.plan()
            self.arm_move_group_.go(wait=True)
            self.arm_move_group_.stop()
            self.arm_move_group_.clear_pose_targets()
        else:
            rospy.logerr('Invalid goal')
            return

        # check that preempt has not been requested by the client
        if self.as_.is_preempt_requested():
            rospy.loginfo('%s: Preempted' % self.action_name_)
            self.as_.set_preempted()
            success = False

        # check if the goal request has been executed correctly
        if success:
            self.result_.success = True
            rospy.loginfo('%s: Succeeded' % self.action_name_)
            self.as_.set_succeeded(self.result_)        


if __name__ == '__main__':

    # Inizialize a ROS node called task_server
    rospy.init_node('task_server')

    server = TaskServer(rospy.get_name())

    # keeps the node up and running
    rospy.spin()

I have tried changing rotations, or removing them, it hasn't helped. When I set goal tolerance to 0.5 it works but that's bad cause it isn't consistent. Setting the goal tolerance any lower doesn't help. The robot arm has 4 degrees of freedom and has a simple gripper.

Here is the terminal output:

arduinobot_arm/arduinobot_arm: Starting planning with 1 states already in datastructure
arduinobot_arm/arduinobot_arm: Unable to sample any valid states for goal tree
arduinobot_arm/arduinobot_arm: Created 1 states (1 start + 0 goal)
No solution found after 1.210381 seconds
Unable to solve the planning problem
Fail: ABORTED: No motion plan found. No execution attempted.

Asked by screenname on 2022-12-11 05:43:43 UTC

Comments

Answers