UR5: trajectory execution aborted (controller failed)
Please excuse me for asking a really newbie question.
My problem is 2 fold.
[1] My python package is a moveit generated package for the UR5. In gazebo simulation I can get the manipulator to move using the getrandomposes() function.
I am also creating a static-transform-listener between the UR5 and a turtlebot simulated in gazebo. Then I'm reading poses , and repositioning the UR5, from a lookuptransform between the *wrist1link* of the UR5 and the *basefootprint* of the turtlebot. Again, this works in gazebo but not in the real world.
When I try the real robot, I get the following log output for the transform(), random_poses() and terminal messages:
Log[transform]:
[rospy.client][INFO] 2019-03-21 08:07:16,971: init_node, name[/ur5bot], pid[29113]
[xmlrpc][INFO] 2019-03-21 08:07:16,971: XML-RPC server binding to 127.0.0.1:0
[rospy.init][INFO] 2019-03-21 08:07:16,971: ROS Slave URI: [http://localhost:43445/]
[xmlrpc][INFO] 2019-03-21 08:07:16,971: Started XML-RPC server [http://localhost:43445/]
[rospy.impl.masterslave][INFO] 2019-03-21 08:07:16,972: _ready: http://localhost:43445/
[xmlrpc][INFO] 2019-03-21 08:07:16,972: xml rpc node: starting XML-RPC server
[rospy.registration][INFO] 2019-03-21 08:07:16,972: Registering with master node http://localhost:11311
[rospy.init][INFO] 2019-03-21 08:07:17,072: registered with master
[rospy.rosout][INFO] 2019-03-21 08:07:17,072: initializing /rosout core topic
[rospy.rosout][INFO] 2019-03-21 08:07:17,074: connected to core topic /rosout
[rospy.simtime][INFO] 2019-03-21 08:07:17,076: /use_sim_time is not set, will not subscribe to simulated time [/clock] topic
[rosout][INFO] 2019-03-21 08:07:17,082: ======== init responding
[rospy.internal][INFO] 2019-03-21 08:07:18,414: topic[/rosout] adding connection to [/rosout], count 0
[rospy.internal][INFO] 2019-03-21 08:07:18,446: topic[/tf_static] adding connection to [http://localhost:45507/], count 0
[rospy.internal][INFO] 2019-03-21 08:07:18,447: topic[/tf] adding connection to [http://localhost:45507/], count 0
[rospy.internal][INFO] 2019-03-21 08:07:18,456: topic[/tf_static] adding connection to [http://localhost:40525/], count 1
[rospy.internal][INFO] 2019-03-21 08:07:18,520: topic[/collision_object] adding connection to [/move_group], count 0
[rospy.internal][INFO] 2019-03-21 08:07:18,527: topic[/attached_collision_object] adding connection to [/move_group], count 0
[rospy.core][INFO] 2019-03-21 08:07:22,333: signal_shutdown [signal-2]
[rospy.internal][INFO] 2019-03-21 08:07:22,339: topic[/collision_object] removing connection to /move_group
[rospy.internal][INFO] 2019-03-21 08:07:22,340: topic[/attached_collision_object] removing connection to /move_group
[rospy.internal][INFO] 2019-03-21 08:07:22,340: topic[/rosout] removing connection to /rosout
[rospy.internal][INFO] 2019-03-21 08:07:22,340: topic[/tf_static] removing connection to http://localhost:45507/
[rospy.internal][WARNING] 2019-03-21 08:07:22,342: Unknown error initiating TCP/IP socket to localhost:52137 (http://localhost:45507/): Traceback (most recent call last):
File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_base.py", line 558, in connect
self.local_endpoint = self.socket.getsockname()
AttributeError: 'NoneType' object has no attribute 'getsockname'
[rospy.internal][INFO] 2019-03-21 08:07:22,342: topic[/tf_static] removing connection to http://localhost:40525/
[rospy.internal][INFO] 2019-03-21 08:07:22,342: topic[/tf] removing connection to http://localhost:45507/
[rospy.impl.masterslave][INFO] 2019-03-21 08:07:22,342: signal-2
[rospy.internal][WARNING] 2019-03-21 08:07:22,478: Unknown error initiating TCP/IP socket to localhost:32775 (http://localhost:40525/): Traceback (most recent call last):
File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_base.py", line 557, in connect
self.read_header()
File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_base.py", line 650, in read_header
self._validate_header(read_ros_handshake_header(sock, self.read_buff, self.protocol.buff_size))
File "/opt/ros/kinetic/lib/python2.7/dist-packages/rosgraph/network.py", line 364, in read_ros_handshake_header
raise ROSHandshakeException("connection from sender terminated before handshake header received. %s bytes were received. Please check sender for additional details."%b.tell())
ROSHandshakeException: connection from sender terminated before handshake header received. 0 bytes were received. Please check sender for additional details.
[rospy.core][INFO] 2019-03-21 08:07:22,478: signal_shutdown [atexit]
Log[random_poses]:
[rospy.client][INFO] 2019-03-21 07:31:38,053: init_node, name[/ur5bot], pid[5118]
[xmlrpc][INFO] 2019-03-21 07:31:38,053: XML-RPC server binding to 127.0.0.1:0
[rospy.init][INFO] 2019-03-21 07:31:38,054: ROS Slave URI: [http://localhost:37135/]
[xmlrpc][INFO] 2019-03-21 07:31:38,053: Started XML-RPC server [http://localhost:37135/]
[rospy.impl.masterslave][INFO] 2019-03-21 07:31:38,057: _ready: http://localhost:37135/
[rospy.registration][INFO] 2019-03-21 07:31:38,058: Registering with master node http://localhost:11311
[xmlrpc][INFO] 2019-03-21 07:31:38,058: xml rpc node: starting XML-RPC server
[rospy.init][INFO] 2019-03-21 07:31:38,157: registered with master
[rospy.rosout][INFO] 2019-03-21 07:31:38,157: initializing /rosout core topic
[rospy.rosout][INFO] 2019-03-21 07:31:38,160: connected to core topic /rosout
[rospy.simtime][INFO] 2019-03-21 07:31:38,161: /use_sim_time is not set, will not subscribe to simulated time [/clock] topic
[rosout][INFO] 2019-03-21 07:31:38,164: ======== init responding
[rospy.internal][INFO] 2019-03-21 07:31:39,894: topic[/rosout] adding connection to [/rosout], count 0
[rosout][INFO] 2019-03-21 07:31:39,894: position:
x: 0.404118248032
y: -0.577092974727
z: -0.173208990121
orientation:
x: 0.750149564739
y: -0.392858829961
z: 0.510404409718
w: 0.149749486756
[rospy.internal][INFO] 2019-03-21 07:31:40,206: topic[/collision_object] adding connection to [/move_group], count 0
[rospy.internal][INFO] 2019-03-21 07:31:40,359: topic[/attached_collision_object] adding connection to [/move_group], count 0
[rosout][INFO] 2019-03-21 07:31:40,358: position:
x: 0.0553050320796
y: 0.107028530838
z: 0.124198459809
orientation:
x: -0.0423558721036
y: 0.89782401388
z: -0.433582725679
w: 0.0642186903607
[rosout][INFO] 2019-03-21 07:31:40,968: position:
x: 0.411880143944
y: 0.204760686467
z: -0.317456349455
orientation:
x: -0.462901300664
y: 0.745423498108
z: 0.456791241182
w: 0.146314579902
.
.
.
[rosout][INFO] 2019-03-21 07:31:46,413: ======== Random Pose:
Terminal[random_poses]:
[ INFO] [1553179198.882580585]: rviz version 1.12.16
[ INFO] [1553179198.882649251]: compiled against Qt version 5.5.1
[ INFO] [1553179198.882669095]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1553179199.654227782]: Stereo is NOT SUPPORTED
[ INFO] [1553179199.656284830]: OpenGl version: 2.1 (GLSL 1.2).
Failed to import pyassimp, see https://github.com/ros-planning/moveit/issues/86 for more info
[INFO] [1553179200.977359]: ======== init responding
[ INFO] [1553179201.019753639]: Loading robot model 'ur5'...
[ INFO] [1553179201.019829318]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1553179201.421333824]: Loading robot model 'ur5'...
[ INFO] [1553179201.421401153]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1553179202.727467922]: Ready to take commands for planning group ur5_bot.
================ Executing Random Pose-0
[INFO] [1553179202.951430]: position:
x: -0.178781282827
y: 0.118838527347
z: -0.337893284696
orientation:
x: 0.157734020892
y: -0.0597165462881
z: 0.983661256905
w: 0.0629638341933
[ INFO] [1553179203.638487304]: ABORTED: Solution found but controller failed during execution
[ INFO] [1553179204.026712272]: ABORTED: Solution found but controller failed during execution
================ Executing Random Pose-1
[INFO] [1553179204.030212]: position:
x: -0.336402631433
y: -0.214187021174
z: 0.193706288631
orientation:
x: 0.338538602729
y: -0.913979701681
z: -0.190360686409
w: 0.117454367512
[ INFO] [1553179204.274404887]: ABORTED: Solution found but controller failed during execution
[ INFO] [1553179204.707363844]: ABORTED: Solution found but controller failed during execution
================ Executing Random Pose-2
[INFO] [1553179204.711088]: position:
x: 0.105210533957
y: -0.0357802713065
z: 0.211806998719
orientation:
x: -0.51076995905
y: 0.828248339545
z: -0.0120237615239
w: 0.23016117425
[ INFO] [1553179204.933437995]: ABORTED: Solution found but controller failed during execution
[ INFO] [1553179205.101065666]: ABORTED: Solution found but controller failed during execution
================ Executing Random Pose-3
.
.
.
Code containing functions providing poses and execution of the plan:
def transform(self):
# -----------------------------------------------
tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(tf_buffer)
rospy.sleep(1) # wait for the buffer to populate otherwise the listener will error
try:
if tf_buffer.can_transform('wrist_1_link', 'base_footprint', rospy.Time()) :
transform_stamped = tf_buffer.lookup_transform('wrist_1_link', 'base_footprint', rospy.Time())
print("*" * 25 + " \nTransform Data: ")
pose = Pose()
pose.position = transform_stamped.transform.translation
pose.orientation = transform_stamped.transform.rotation
self.move_to_pose(pose)
rospy.loginfo(transform_stamped)
print("*" * 40)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
rospy.logerr(e)
rospy.logerr("*** UR5 repositioning failed ***")
# -----------------------------------------------
def move_to_pose(self, pose):
# rospy.loginfo(pose)
self.manipulator.set_goal_tolerance(0.5)
self.manipulator.set_pose_target(pose)
plan = self.manipulator.plan()
self.manipulator.execute(plan) # display the trajectory in rviz. Follow the already computed plan
self.manipulator.go(wait=True) # execute in real robot
self.manipulator.stop() # ensure there is no residual movement
self.manipulator.clear_pose_targets() # clear targets after planning with poses
def random_poses(self):
for x in range(10):
print " ================ Executing Random Pose-"+str(x)
pose = self.manipulator.get_random_pose()
rospy.loginfo(pose.pose)
self.move_to_pose(pose.pose)
rospy.loginfo("======== Random Pose: ")
How can I solve this problem and get the real UR5 responding to pose changes?
[2] If the turtlebot is not going to be used and the UR5 is simply to subscribe to a topic which will return a pose, will the UR5 "know" accurately what point in space the pose represents?
Apologies if this question is not very clear. I assumed both robots would be connected to each other using ip addresses on the same network. Then they would be "aware" of each other. But what if there is no turtlebot and instead a person giving poses as locations for the UR5 to move to?
I would appreciate you help with these questions.
Asked by sisko on 2019-03-20 19:57:01 UTC
Comments
I think you may be misunderstanding things: planning works, it's the execution of the plan that gets aborted when you use the real robot.
It would help if you could include more log lines. Right now there isn't sufficient information to help you.
In general: don't paraphrase errors and also please provide a little context (ie: lines before and after the line that you believe shows a relevant error).
Asked by gvdhoorn on 2019-03-21 01:15:01 UTC
That would be nice, but that's not how things work.
I'll ignore the "aware" part (as that implies cognisance), but by just having two robots (ie: ROS applications) on the same IP network nothing is going to happen. Unless those robots are part of the same ROS node graph (ie: use the same master) or you have explicitly set up a multi-master configuration, no data is going to be exchanged.
Furthermore: even if all of that was in place, only by writing a node (ie: program) yourself would you be able to add the behaviour you are after to the system (ie: your program "ties" everything together).
In essence: just by bringing too ROS-enabled robots together nothing will happen. It's always the business logic implemented by you (or someone else) that makes things move / do interesting things.
Asked by gvdhoorn on 2019-03-21 01:31:04 UTC
@gvdhoom: I take your point :-). Just for the record, I wasn't suggesting robotic cognisance but you are right in that they need to be connected to a common roscore. Apologies for the vagueness of my facts. I included some more log output and some code in the hope it will help. I can include more when I get access to the UR5.
Asked by sisko on 2019-03-21 06:15:03 UTC
Not an answer, but why are you planning for
wrist_1_linkand notee_linkortool0?Also: is this the output in the terminal where you run your script? Because I would expect to see much more lines from MoveIt itself, when it is planning.
Asked by gvdhoorn on 2019-03-21 07:01:58 UTC
I'm using
wrist_3_jointbecause it's physically the last link on the robot. It currently has no attached end effector.Asked by sisko on 2019-03-21 09:06:01 UTC
Your code (shown above) uses
wrist_1_link, notwrist_3_link.Also: the urdf for the UR5 (and UR3 and UR10) already contains a default
tool0frame (here). I'd use that, and not a "random" frame.Edit: and looking at your code: you're asking for the transform between
wrist_1_linkandbase_footprint.base_footprintis "on the floor", below the turtlebot. We don't know how your UR is mounted, but it's unlikely the robot can actually plan to that position: either because it's out of reach, or because the turtlebot is occupying that space.Asked by gvdhoorn on 2019-03-21 09:11:13 UTC
The ur5 is mounted on a Clearpath Husky robot but the task is not to actually reach the turtlebot especially if it can't do so. The robots should be placed close enough to make it possible for the ur5 to reach it. But in the event it can't, it is possible to achieve a pose that would get it as close as possible? I apologise for overwhelming you. I added log output for both functions.
Asked by sisko on 2019-03-21 09:59:12 UTC