00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 from __future__ import division
00037
00038
00039 __author__ = 'Antons Rebguns'
00040 __copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
00041
00042 __license__ = 'BSD'
00043 __maintainer__ = 'Antons Rebguns'
00044 __email__ = 'anton@email.arizona.edu'
00045
00046
00047 import roslib
00048 roslib.load_manifest('dynamixel_controllers')
00049
00050 import rospy
00051 import actionlib
00052
00053 from std_msgs.msg import Float64
00054 from trajectory_msgs.msg import JointTrajectory
00055 from pr2_controllers_msgs.msg import JointTrajectoryControllerState
00056 from pr2_controllers_msgs.msg import JointTrajectoryAction
00057 from dynamixel_msgs.msg import JointState
00058
00059 from pr2_controllers_msgs.srv import QueryTrajectoryState
00060 from dynamixel_controllers.srv import SetSpeed
00061
00062 class Segment():
00063 def __init__(self, num_joints):
00064 self.start_time = 0.0
00065 self.duration = 0.0
00066 self.positions = [0.0] * num_joints
00067 self.velocities = [0.0] * num_joints
00068
00069 class JointTrajectoryActionController():
00070 def __init__(self):
00071 self.update_rate = 1000
00072 self.trajectory = []
00073
00074 self.controller_namespace = rospy.get_param('~controller_namespace', 'l_arm_controller')
00075 self.joint_controllers = rospy.get_param(self.controller_namespace + '/joint_controllers', [])
00076 self.joint_names = []
00077
00078 for controller in self.joint_controllers:
00079 self.joint_names.append(rospy.get_param(controller + '/joint_name'))
00080
00081 self.joint_states = dict(zip(self.joint_names, [JointState(name=jn) for jn in self.joint_names]))
00082 self.num_joints = len(self.joint_names)
00083 self.joint_to_idx = dict(zip(self.joint_names, range(self.num_joints)))
00084
00085 ns = self.controller_namespace + '/joint_trajectory_action_node/constraints'
00086 self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0)
00087 self.stopped_velocity_tolerance = rospy.get_param(ns + '/stopped_velocity_tolerance', 0.01)
00088 self.goal_constraints = []
00089 self.trajectory_constraints = []
00090 self.min_velocity = rospy.get_param(self.controller_namespace + '/joint_trajectory_action_node/min_velocity', 0.1)
00091
00092 for joint in self.joint_names:
00093 self.goal_constraints.append(rospy.get_param(ns + '/' + joint + '/goal', -1.0))
00094 self.trajectory_constraints.append(rospy.get_param(ns + '/' + joint + '/trajectory', -1.0))
00095
00096
00097 self.msg = JointTrajectoryControllerState()
00098 self.msg.joint_names = self.joint_names
00099 self.msg.desired.positions = [0.0] * self.num_joints
00100 self.msg.desired.velocities = [0.0] * self.num_joints
00101 self.msg.desired.accelerations = [0.0] * self.num_joints
00102 self.msg.actual.positions = [0.0] * self.num_joints
00103 self.msg.actual.velocities = [0.0] * self.num_joints
00104 self.msg.error.positions = [0.0] * self.num_joints
00105 self.msg.error.velocities = [0.0] * self.num_joints
00106
00107
00108 self.last_commanded = {}
00109 for joint in self.joint_names:
00110 self.last_commanded[joint] = { 'position': None, 'velocity': None }
00111
00112
00113 self.state_pub = rospy.Publisher(self.controller_namespace + '/state', JointTrajectoryControllerState)
00114 self.joint_position_pubs = [rospy.Publisher(controller + '/command', Float64) for controller in self.joint_controllers]
00115
00116
00117 self.command_sub = rospy.Subscriber(self.controller_namespace + '/command', JointTrajectory, self.process_command)
00118 self.joint_state_subs = [rospy.Subscriber(controller + '/state', JointState, self.process_joint_states) for controller in self.joint_controllers]
00119
00120
00121 self.joint_velocity_srvs = [rospy.ServiceProxy(controller + '/set_speed', SetSpeed, persistent=True) for controller in self.joint_controllers]
00122 self.query_state_service = rospy.Service(self.controller_namespace + '/query_state', QueryTrajectoryState, self.process_query_state)
00123 self.action_server = actionlib.SimpleActionServer(self.controller_namespace + '/joint_trajectory_action',
00124 JointTrajectoryAction,
00125 execute_cb=self.process_trajectory_action,
00126 auto_start=False)
00127 self.action_server.start()
00128
00129 def process_command(self, msg):
00130 if self.action_server.is_active(): self.action_server.set_preempted()
00131
00132 while self.action_server.is_active():
00133 sleep(0.01)
00134
00135 process_trajectory_action(msg)
00136
00137 def process_query_state(self, req):
00138 if not self.trajectory: return False
00139
00140
00141 seg = -1
00142 while seg + 1 < len(self.trajectory) and self.trajectory[seg+1].start_time < req.time.to_sec():
00143 seg += 1
00144
00145 if seg == -1: return False
00146
00147 resp = QueryTrajectoryState()
00148 resp.name = self.joint_names
00149 resp.position = [0.0] * self.num_joints
00150 resp.velocity = [0.0] * self.num_joints
00151 resp.acceleration = [0.0] * self.num_joints
00152
00153 seg_end_time = self.trajectory.start_time + self.trajectory.duration
00154 t = self.trajectory.duration - (seg_end_time - req.time)
00155
00156 for j, joint in enumerate(self.joint_names):
00157 resp.position[j] = self.joint_states[joint].current_pos
00158 resp.velocity[j] = self.joint_states[joint].velocity
00159
00160 return resp
00161
00162 def process_trajectory_action(self, goal):
00163 traj = goal.trajectory
00164
00165
00166 if set(self.joint_names) != set(traj.joint_names):
00167 msg = "Incoming trajectory joints don't match our joints"
00168 rospy.logerr(msg)
00169 self.action_server.set_aborted(text=msg)
00170 return
00171
00172
00173 if not traj.points:
00174 msg = "Incoming trajectory is empty"
00175 rospy.logerr(msg)
00176 self.action_server.set_aborted(text=msg)
00177 return
00178
00179
00180
00181 try:
00182 lookup = [traj.joint_names.index(joint) for joint in self.joint_names]
00183 except ValueError as val:
00184 msg = 'Joint in the controller and those specified in the trajectory do not match.'
00185 rospy.logerr(msg)
00186 self.action_server.set_aborted(text=msg)
00187 return
00188
00189 durations = [0.0] * len(traj.points)
00190
00191
00192 if len(traj.points) > 0:
00193 durations[0] = traj.points[0].time_from_start.to_sec()
00194
00195 for i in range(1, len(traj.points)):
00196 durations[i] = (traj.points[i].time_from_start - traj.points[i-1].time_from_start).to_sec()
00197
00198 if not traj.points[0].positions:
00199 msg = 'First point of trajectory has no positions'
00200 rospy.logerr(msg)
00201 self.action_server.set_aborted(text=msg)
00202 return
00203
00204 trajectory = []
00205 time = rospy.Time.now() + rospy.Duration(0.01)
00206
00207 for i in range(len(traj.points)):
00208 seg = Segment(self.num_joints)
00209
00210 if traj.header.stamp == rospy.Time(0.0):
00211 seg.start_time = (time + traj.points[i].time_from_start).to_sec() - durations[i]
00212 else:
00213 seg.start_time = (traj.header.stamp + traj.points[i].time_from_start).to_sec() - durations[i]
00214
00215 seg.duration = durations[i]
00216
00217
00218 if traj.points[i].velocities and len(traj.points[i].velocities) != self.num_joints:
00219 msg = 'Command point %d has %d elements for the velocities' % (i, len(traj.points[i].velocities))
00220 rospy.logerr(msg)
00221 self.action_server.set_aborted(text=msg)
00222 return
00223
00224 if len(traj.points[i].positions) != self.num_joints:
00225 msg = 'Command point %d has %d elements for the positions' % (i, len(traj.points[i].positions))
00226 rospy.logerr(msg)
00227 self.action_server.set_aborted(text=msg)
00228 return
00229
00230 for j in range(self.num_joints):
00231 if traj.points[i].velocities:
00232 seg.velocities[j] = traj.points[i].velocities[lookup[j]]
00233 if traj.points[i].positions:
00234 seg.positions[j] = traj.points[i].positions[lookup[j]]
00235
00236 trajectory.append(seg)
00237
00238 rospy.loginfo('Trajectory start requested at %.3lf, waiting...', traj.header.stamp.to_sec())
00239 rate = rospy.Rate(self.update_rate)
00240
00241 while traj.header.stamp > time:
00242 time = rospy.Time.now()
00243 rate.sleep()
00244
00245 end_time = traj.header.stamp + rospy.Duration(sum(durations))
00246 seg_end_times = [rospy.Time.from_sec(trajectory[seg].start_time + durations[seg]) for seg in range(len(trajectory))]
00247
00248 rospy.loginfo('Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf', time.to_sec(), end_time.to_sec(), sum(durations))
00249
00250 q = [0.0] * self.num_joints
00251 qd = [0.0] * self.num_joints
00252
00253 self.trajectory = trajectory
00254 traj_start_time = rospy.Time.now()
00255
00256 for seg in range(len(trajectory)):
00257 rospy.logdebug('current segment is %d time left %f cur time %f' % (seg, durations[seg] - (time.to_sec() - trajectory[seg].start_time), time.to_sec()))
00258 rospy.logdebug('goal positions are: %s' % str(trajectory[seg].positions))
00259
00260
00261 if durations[seg] == 0:
00262 rospy.logdebug('skipping segment %d with duration of 0 seconds' % seg)
00263 continue
00264
00265
00266
00267 for joint in reversed(self.joint_names):
00268 j = self.joint_names.index(joint)
00269
00270 start_position = self.joint_states[joint].current_pos
00271 if seg != 0: start_position = trajectory[seg-1].positions[j]
00272
00273 q[j] = trajectory[seg].positions[j]
00274 qd[j] = max(self.min_velocity, abs(q[j] - start_position) / durations[seg])
00275
00276 self.msg.desired.positions[j] = q[j]
00277 self.msg.desired.velocities[j] = qd[j]
00278
00279 self.set_joint_velocity(joint, qd[j])
00280 self.set_joint_angle(joint, q[j])
00281
00282 while time < seg_end_times[seg]:
00283
00284 if self.action_server.is_preempt_requested():
00285 msg = 'New trajectory received. Aborting old trajectory.'
00286
00287 for j in range(self.num_joints):
00288 cur_pos = self.joint_states[self.joint_names[j]].current_pos
00289 self.set_joint_angle(self.joint_names[j], cur_pos)
00290
00291 self.action_server.set_preempted(text=msg)
00292 rospy.logwarn(msg)
00293 return
00294
00295 rate.sleep()
00296 time = rospy.Time.now()
00297
00298
00299 for j, joint in enumerate(self.joint_names):
00300 if self.trajectory_constraints[j] > 0 and self.msg.error.positions[j] > self.trajectory_constraints[j]:
00301 msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \
00302 (joint, seg, self.msg.error.positions[j], self.trajectory_constraints[j])
00303 rospy.logwarn(msg)
00304 self.action_server.set_aborted(text=msg)
00305 return
00306
00307 traj_actual_time = (rospy.Time.now() - traj_start_time).to_sec()
00308 traj_time_err = traj_actual_time - sum(durations)
00309 rospy.loginfo('Trajectory execution took %f seconds, error is %f' % (traj_actual_time, traj_time_err))
00310
00311
00312 rospy.sleep(self.goal_time_constraint)
00313
00314 for i, j in enumerate(self.joint_names):
00315 rospy.logdebug('desired pos was %f, actual pos is %f, error is %f' % (trajectory[-1].positions[i], self.joint_states[j].current_pos, self.joint_states[j].current_pos - trajectory[-1].positions[i]))
00316
00317
00318 for (joint, pos_error, pos_constraint) in zip(self.joint_names, self.msg.error.positions, self.goal_constraints):
00319 if pos_constraint > 0 and abs(pos_error) > pos_constraint:
00320 msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \
00321 (joint, pos_error, pos_constraint)
00322 rospy.logwarn(msg)
00323 self.action_server.set_aborted(text=msg)
00324 break
00325 else:
00326 rospy.loginfo('Trajectory execution successfully completed')
00327 self.action_server.set_succeeded()
00328
00329
00330
00331
00332
00333
00334
00335 def process_joint_states(self, msg):
00336 self.msg.header.stamp = rospy.Time.now()
00337
00338 self.joint_states[msg.name] = msg
00339
00340
00341 for i, joint in enumerate(self.joint_names):
00342 state = self.joint_states[joint]
00343 self.msg.actual.positions[i] = state.current_pos
00344 self.msg.actual.velocities[i] = abs(state.velocity)
00345 self.msg.error.positions[i] = self.msg.actual.positions[i] - self.msg.desired.positions[i]
00346 self.msg.error.velocities[i] = self.msg.actual.velocities[i] - self.msg.desired.velocities[i]
00347
00348 self.state_pub.publish(self.msg)
00349
00350 def command_will_update(self, command, joint, value):
00351 current_state = None
00352 command_resolution = None
00353
00354 if command == 'position':
00355 current_state = self.joint_states[joint].current_pos
00356 command_resolution = 0.006
00357 elif command == 'velocity':
00358 current_state = self.joint_states[joint].velocity
00359 command_resolution = 0.05
00360 else:
00361 rospy.logerr('Unrecognized motor command %s while setting %s to %f', command, joint, value)
00362 return False
00363
00364 last_commanded = self.last_commanded[joint][command]
00365
00366
00367 if last_commanded is not None and (abs(value - last_commanded) < command_resolution): return False
00368 if current_state is not None and (abs(value - current_state) < command_resolution): return False
00369
00370 self.last_commanded[joint][command] = value
00371
00372 return True
00373
00374 def set_joint_velocity(self, joint, velocity):
00375 if self.command_will_update('velocity', joint, velocity):
00376 self.joint_velocity_srvs[self.joint_to_idx[joint]](velocity)
00377
00378 def set_joint_angle(self, joint, angle):
00379 if self.command_will_update('position', joint, angle):
00380 self.joint_position_pubs[self.joint_to_idx[joint]].publish(angle)
00381
00382
00383 if __name__ == '__main__':
00384 try:
00385 rospy.init_node('joint_trajectory_action_controller', anonymous=True)
00386 traj_controller = JointTrajectoryActionController()
00387 rospy.spin()
00388 except rospy.ROSInterruptException: pass
00389