diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index 48de91f596aae94f3d7766accd76e1149cb2cc81..3d5ee73a22d5f8d05df6f7c153a47b3c1f219b87 100755
--- a/ur5_driver/driver.py
+++ b/ur5_driver/driver.py
@@ -10,6 +10,7 @@ import rospy
import actionlib
from sensor_msgs.msg import JointState
from control_msgs.msg import FollowJointTrajectoryAction
+from trajectory_msgs.msg import JointTrajectoryPoint
HOSTNAME='ur-xx'
HOSTNAME="10.0.1.20"
@@ -159,6 +160,21 @@ def get_segment_duration(traj, index):
return traj.points[0].time_from_start.to_sec()
return (traj.points[index].time_from_start - traj.points[index-1].time_from_start).to_sec()
+# Reorders the JointTrajectory traj according to the order in
+# joint_names. Destructive.
+def reorder_traj_joints(traj, joint_names):
+ order = [traj.joint_names.index(j) for j in joint_names]
+
+ new_points = []
+ for p in traj.points:
+ new_points.append(JointTrajectoryPoint(
+ positions = [p.positions[i] for i in order],
+ velocities = [p.velocities[i] for i in order] if p.velocities else [],
+ accelerations = [p.accelerations[i] for i in order] if p.accelerations else [],
+ time_from_start = p.time_from_start))
+ traj.joint_names = joint_names
+ traj.points = new_points
+
class UR5TrajectoryFollower(object):
def __init__(self, robot):
self.robot = robot
@@ -180,14 +196,23 @@ class UR5TrajectoryFollower(object):
def on_goal(self, goal_handle):
log("on_goal")
- if self.goal_handle:
- rospy.logerr("Already have a goal in progress! Rejecting. (TODO)")
+
+ # Checks if the joints are just incorrect
+ if set(goal_handle.get_goal().trajectory.joint_names) != set(JOINT_NAMES):
+ rospy.logerr("Received a goal with incorrect joint names: (%s)" % \
+ ', '.join(goal_handle.get_goal().trajectory.joint_names))
goal_handle.set_rejected()
return
- # TODO: Verify that the goal has the correct joints. Reorder if necessary
-
+ # Orders the joints of the trajectory according to JOINT_NAMES
+ reorder_traj_joints(goal_handle.get_goal().trajectory, JOINT_NAMES)
+
with self.following_lock:
+ if self.goal_handle:
+ rospy.logerr("Already have a goal in progress! Rejecting. (TODO)")
+ goal_handle.set_rejected()
+ return
+
self.goal_handle = goal_handle
self.tracking_i = 0
self.pending_i = 0
@@ -196,7 +221,6 @@ class UR5TrajectoryFollower(object):
# Sends a tracking point and a pending point to the robot
self.goal_handle.set_accepted()
traj = self.goal_handle.get_goal().trajectory
- # TODO: joints may have a different order
self.robot.send_movej(self.first_waypoint_id, traj.points[0].positions,
t=get_segment_duration(traj, 0))
self.robot.send_movej(self.first_waypoint_id + 1, traj.points[1].positions,
@@ -211,32 +235,32 @@ class UR5TrajectoryFollower(object):
# The URScript program sends back waypoint_finished messages,
# which trigger this callback.
def on_waypoint_finished(self, waypoint_id):
- log("Waypoint finished: %i" % waypoint_id)
- index = waypoint_id - self.first_waypoint_id
- if index != self.tracking_i:
- rospy.logerr("Completed waypoint %i (id=%i), but tracking %i (id=%i)" % \
- (index, waypoint_id, self.tracking_i,
- self.first_waypoint_id + self.tracking_i))
- # TODO: Probably need to fail here
-
- traj = self.goal_handle.get_goal().trajectory
- traj_len = len(traj.points)
-
- # Checks if we've completed the trajectory
- if index == traj_len - 1:
- self.goal_handle.set_succeeded()
- self.first_waypoint_id += traj_len
- self.goal_handle = None
-
- # Moves onto the next segment
- self.tracking_i += 1
- if self.pending_i + 1 < traj_len:
- self.pending_i += 1
- # TODO: reorder joint positions
- self.robot.send_movej(self.first_waypoint_id + self.pending_i,
- traj.points[self.pending_i],
- t=get_segment_duration(traj, self.pending_i))
-
+ with self.following_lock:
+ log("Waypoint finished: %i" % waypoint_id)
+ index = waypoint_id - self.first_waypoint_id
+ if index != self.tracking_i:
+ rospy.logerr("Completed waypoint %i (id=%i), but tracking %i (id=%i)" % \
+ (index, waypoint_id, self.tracking_i,
+ self.first_waypoint_id + self.tracking_i))
+ # TODO: Probably need to fail here
+
+ traj = self.goal_handle.get_goal().trajectory
+ traj_len = len(traj.points)
+
+ # Checks if we've completed the trajectory
+ if index == traj_len - 1:
+ self.goal_handle.set_succeeded()
+ self.first_waypoint_id += traj_len
+ self.goal_handle = None
+
+ # Moves onto the next segment
+ self.tracking_i += 1
+ if self.pending_i + 1 < traj_len:
+ self.pending_i += 1
+ self.robot.send_movej(self.first_waypoint_id + self.pending_i,
+ traj.points[self.pending_i].positions,
+ t=get_segment_duration(traj, self.pending_i))
+
sock = None
def main():
diff --git a/ur5_driver/test_move.py b/ur5_driver/test_move.py
index 4812d7a26b8a26fbbd44e244db7354598f62fab1..a3d6ec088f16a1915b81ae077cd8feee99fddf08 100755
--- a/ur5_driver/test_move.py
+++ b/ur5_driver/test_move.py
@@ -9,6 +9,7 @@ JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
Q1 = [2.2,0,-1.57,0,0,0]
Q2 = [1.5,0,-1.57,0,0,0]
+Q3 = [1.5,-0.2,-1.57,0,0,0]
client = None
@@ -18,8 +19,26 @@ def move1():
g.trajectory.joint_names = JOINT_NAMES
g.trajectory.points = [
JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
- JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0))]
+ JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
+ JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
client.send_goal(g)
+ client.wait_for_result()
+
+def move_disordered():
+ order = [4, 2, 3, 1, 5, 0]
+ g = FollowJointTrajectoryGoal()
+ g.trajectory = JointTrajectory()
+ g.trajectory.joint_names = [JOINT_NAMES[i] for i in order]
+ q1 = [Q1[i] for i in order]
+ q2 = [Q2[i] for i in order]
+ q3 = [Q3[i] for i in order]
+ g.trajectory.points = [
+ JointTrajectoryPoint(positions=q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
+ JointTrajectoryPoint(positions=q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
+ JointTrajectoryPoint(positions=q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
+ client.send_goal(g)
+ client.wait_for_result()
+
def main():
global client
@@ -30,6 +49,7 @@ def main():
client.wait_for_server()
print "Connected to server"
move1()
+ #move_disordered()
except KeyboardInterrupt:
rospy.signal_shutdown("KeyboardInterrupt")
raise