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