I have a [robotic arm](https://github.com/kkumpa/ros-robotic-arm) integrated with MoveIt! and Gazebo. When I launch the [Gazebo simulation](https://github.com/kkumpa/ros-robotic-arm/blob/master/robotic_arm_gazebo/launch/empty_world.launch) and [MoveIt! controller](https://github.com/kkumpa/ros-robotic-arm/blob/master/robotic_arm_moveit_config/launch/moveit_planning_execution.launch), 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 giver just the pose of the enf 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 sample any valid states for goal tree
[ INFO] [1556812617.558575471, 11.568000000]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1556812617.558631894, 11.568000000]: No solution found after 0.060714 seconds
[ INFO] [1556812617.558746589, 11.568000000]: Unable to solve the planning problem
[ INFO] [1556812617.559072767, 11.569000000]: ABORTED: No motion plan found. No execution attempted.
[ INFO] [1556812617.559270945, 11.569000000]: Received event 'stop'
How do I make the `set_pose_target()` work for my robotic arm?
↧