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!
↧
Problem with moveit_commander interface; trying to plan trajectories for ur5 simulation with Gazebo
↧