Quantcast
Channel: ROS Answers: Open Source Q&A Forum - RSS feed
Viewing all articles
Browse latest Browse all 1516

Problem with moveit_commander interface; trying to plan trajectories for ur5 simulation with Gazebo

$
0
0
Hello, I apologise in advance, for I am very new to programming with ROS. I have been trying to connect to a simulated ur5 through gazebo and control it in python using the moveit_commander interface. After attempting to write a simple script for controlling the ur5 gazebo simulation using moveit_commander interface I'm receiving the following errors, firstly in the script terminal window and secondly in the terminal running the moveit_planning _execution node: [ WARN] [1456503108.574669066, 548.823000000]: Joint values for monitored state are requested but the full state is not known joint_state: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: /world name: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] velocity: [] effort: [] multi_dof_joint_state: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: /world joint_names: [] transforms: [] twist: [] wrench: [] attached_collision_objects: [] is_diff: False ============ ============ Generating plan 1 [ WARN] [1456503113.598260823, 551.508000000]: Fail: ABORTED: No motion plan found. No execution attempted. ============ Waiting while RVIZ displays plan1... ============ Visualizing plan1 ============ Waiting while plan1 is visualized (again)... terminate called after throwing an instance of 'class_loader::LibraryUnloadException' what(): Attempt to unload library that class_loader is unaware of. Aborted (core dumped) * All is well! Everyone is happy! You can start planning now! [ INFO] [1456502653.160737839, 126.273000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline. [ INFO] [1456502653.162058984, 126.273000000]: No planner specified. Using default. [ INFO] [1456502653.162381392, 126.273000000]: RRTConnect: Starting planning with 1 states already in datastructure [ERROR] [1456502658.169349483, 130.953000000]: RRTConnect: Unable to sample any valid states for goal tree [ INFO] [1456502658.170807752, 130.957000000]: RRTConnect: Created 1 states (1 start + 0 goal) [ INFO] [1456502658.171010925, 130.957000000]: No solution found after 5.008731 seconds [ INFO] [1456502658.209078107, 130.983000000]: Unable to solve the planning problem [ INFO] [1456502658.216058459, 130.990000000]: Received new trajectory execution service request... [ WARN] [1456502658.216106023, 130.991000000]: The trajectory to execute is empty [ INFO] [1456503008.235511782, 473.390000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline. [ INFO] [1456503008.235972587, 473.390000000]: No planner specified. Using default. [ INFO] [1456503008.236136625, 473.390000000]: RRTConnect: Starting planning with 1 states already in datastructure [ERROR] [1456503013.247658046, 477.166000000]: RRTConnect: Unable to sample any valid states for goal tree [ INFO] [1456503013.249219738, 477.172000000]: RRTConnect: Created 1 states (1 start + 0 goal) [ INFO] [1456503013.249677542, 477.172000000]: No solution found after 5.013654 seconds [ INFO] [1456503013.253513681, 477.174000000]: Unable to solve the planning problem [ INFO] [1456503013.260155299, 477.178000000]: Received new trajectory execution service request... [ WARN] [1456503013.260188972, 477.179000000]: The trajectory to execute is empty [ INFO] [1456503108.575331207, 548.823000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline. [ INFO] [1456503108.575958594, 548.823000000]: RRTConnect: Starting planning with 1 states already in datastructure [ERROR] [1456503113.584912230, 551.502000000]: RRTConnect: Unable to sample any valid states for goal tree [ INFO] [1456503113.587574652, 551.504000000]: RRTConnect: Created 1 states (1 start + 0 goal) [ INFO] [1456503113.591495319, 551.505000000]: No solution found after 5.015622 seconds [ INFO] [1456503113.596049476, 551.507000000]: Unable to solve the planning problem [ INFO] [1456503113.599612790, 551.508000000]: Received new trajectory execution service request... [ WARN] [1456503113.599910654, 551.508000000]: The trajectory to execute is empty ^C[move_group-1] killing on exit shutting down processing monitor... ... shutting down processing monitor complete done The important parts of both of these consoles, I believe are ============ Generating plan 1 [ WARN] [1456503113.598260823, 551.508000000]: Fail: ABORTED: No motion plan found. No execution attempted. and [ERROR] [1456503113.584912230, 551.502000000]: RRTConnect: Unable to sample any valid states for goal tree which are suggesting that something's not quite working with either setting the target trajectory/pose or generating the plan in my code?? Here is my code, which has basically been adjusted (perhaps not suitably) from the [moveit_commander tutorial with the pr2](https://github.com/ros-planning/moveit_pr2/blob/hydro-devel/pr2_moveit_tutorials/planning/scripts/move_group_python_interface_tutorial.py) provided by moveit. #!/usr/bin/env python import sys import rospy import moveit_commander import moveit_msgs.msg import geometry_msgs.msg from std_msgs.msg import String def test(): ##initialize moveit_commander and rospy print "============ Starting test setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('test2', anonymous=True) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. scene = moveit_commander.PlanningSceneInterface() ## Instantiate a MoveGroupCommander object. This object is an interface ## to one group of joints. In this case the group is the joints in the left ## arm. This interface can be used to plan and execute motions on the left ## manipulator. group = moveit_commander.MoveGroupCommander("manipulator") ## We create this DisplayTrajectory publisher which is used below to publish ## trajectories for RVIZ to visualize. display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10) ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." rospy.sleep(10) print "============ Starting test " ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ ## ## We can get the name of the reference frame for this robot print "============ Reference frame: %s" % group.get_planning_frame() ## We can also print the name of the end-effector link for this group print "============ Reference frame: %s" % group.get_end_effector_link() ## We can get a list of all the groups in the robot print "============ Robot Groups:" print robot.get_group_names() ## Sometimes for debugging it is useful to print the entire state of the ## robot. print "============ Printing robot state" print robot.get_current_state() print "============" ## Planning to a Pose goal ## ^^^^^^^^^^^^^^^^^^^^^^^ ## We can plan a motion for this group to a desired pose for the ## end-effector print "============ Generating plan 1" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.7 pose_target.position.y = -0.05 pose_target.position.z = 1.1 group.set_pose_target(pose_target) ## Now, we call the planner to compute the plan ## and visualize it if successful ## Note that we are just planning, not asking move_group ## to actually move the robot plan1 = group.plan() ##execute the plan for gazebo the recieve group.execute(plan1) print "============ Waiting while RVIZ displays plan1..." rospy.sleep(5) ## You can ask RVIZ to visualize a plan (aka trajectory) for you. But the ## group.plan() method does this automatically so this is not that useful ## here (it just displays the same trajectory again). print "============ Visualizing plan1" display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan1) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while plan1 is visualized (again)..." rospy.sleep(5) if __name__=='__main__': try: test() except rospy.ROSInterruptException: pass So, I think the problem is coming at either the set_pose_target(), or plan(), functions? Do I need to execute the generated plan using execute() for gazebo to receive the target pose? What do you guys think I might be doing wrong? I could have missed loads of stuff, in which case sorry! Just a little more background: Initially, I'm setting up the ur5 gazebo, move_group, and rviz nodes using the following commands: roslaunch ur_gazebo ur5.launch roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true roslaunch ur5_moveit_config moveit_rviz.launch config:=true as described on the ur gitub readme doc. using Ros indigo desktop full, on ubuntu 14.04 vm, with ur5 packages downloaded from [here](https://github.com/ros-industrial/universal_robot). Thanks so much in advance!

Viewing all articles
Browse latest Browse all 1516

Trending Articles