joint_trajectory_action_controller.py
Go to the documentation of this file.
00001 # -*- coding: utf-8 -*-
00002 #
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2010-2011, Antons Rebguns.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of University of Arizona nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 from __future__ import division
00036 
00037 
00038 __author__ = 'Antons Rebguns'
00039 __copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
00040 
00041 __license__ = 'BSD'
00042 __maintainer__ = 'Antons Rebguns'
00043 __email__ = 'anton@email.arizona.edu'
00044 
00045 
00046 from threading import Thread
00047 
00048 import rospy
00049 import actionlib
00050 
00051 from std_msgs.msg import Float64
00052 from trajectory_msgs.msg import JointTrajectory
00053 from control_msgs.msg import FollowJointTrajectoryAction
00054 from control_msgs.msg import FollowJointTrajectoryFeedback
00055 from control_msgs.msg import FollowJointTrajectoryResult
00056 
00057 
00058 class Segment():
00059     def __init__(self, num_joints):
00060         self.start_time = 0.0  # trajectory segment start time
00061         self.duration = 0.0  # trajectory segment duration
00062         self.positions = [0.0] * num_joints
00063         self.velocities = [0.0] * num_joints
00064 
00065 class JointTrajectoryActionController():
00066     def __init__(self, controller_namespace, controllers):
00067         self.update_rate = 1000
00068         self.state_update_rate = 50
00069         self.trajectory = []
00070         
00071         self.controller_namespace = controller_namespace
00072         self.joint_names = [c.joint_name for c in controllers]
00073         
00074         self.joint_to_controller = {}
00075         for c in controllers:
00076             self.joint_to_controller[c.joint_name] = c
00077             
00078         self.port_to_joints = {}
00079         for c in controllers:
00080             if c.port_namespace not in self.port_to_joints: self.port_to_joints[c.port_namespace] = []
00081             self.port_to_joints[c.port_namespace].append(c.joint_name)
00082             
00083         self.port_to_io = {}
00084         for c in controllers:
00085             if c.port_namespace in self.port_to_io: continue
00086             self.port_to_io[c.port_namespace] = c.dxl_io
00087             
00088         self.joint_states = dict(zip(self.joint_names, [c.joint_state for c in controllers]))
00089         self.num_joints = len(self.joint_names)
00090         self.joint_to_idx = dict(zip(self.joint_names, range(self.num_joints)))
00091 
00092     def initialize(self):
00093         ns = self.controller_namespace + '/joint_trajectory_action_node/constraints'
00094         self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0)
00095         self.stopped_velocity_tolerance = rospy.get_param(ns + '/stopped_velocity_tolerance', 0.01)
00096         self.goal_constraints = []
00097         self.trajectory_constraints = []
00098         self.min_velocity = rospy.get_param(self.controller_namespace + '/joint_trajectory_action_node/min_velocity', 0.1)
00099         
00100         for joint in self.joint_names:
00101             self.goal_constraints.append(rospy.get_param(ns + '/' + joint + '/goal', -1.0))
00102             self.trajectory_constraints.append(rospy.get_param(ns + '/' + joint + '/trajectory', -1.0))
00103             
00104         # Message containing current state for all controlled joints
00105         self.msg = FollowJointTrajectoryFeedback()
00106         self.msg.joint_names = self.joint_names
00107         self.msg.desired.positions = [0.0] * self.num_joints
00108         self.msg.desired.velocities = [0.0] * self.num_joints
00109         self.msg.desired.accelerations = [0.0] * self.num_joints
00110         self.msg.actual.positions = [0.0] * self.num_joints
00111         self.msg.actual.velocities = [0.0] * self.num_joints
00112         self.msg.error.positions = [0.0] * self.num_joints
00113         self.msg.error.velocities = [0.0] * self.num_joints
00114         
00115         return True
00116 
00117 
00118     def start(self):
00119         self.running = True
00120         
00121         self.command_sub = rospy.Subscriber(self.controller_namespace + '/command', JointTrajectory, self.process_command)
00122         self.state_pub = rospy.Publisher(self.controller_namespace + '/state', FollowJointTrajectoryFeedback, queue_size=1)
00123         self.action_server = actionlib.SimpleActionServer(self.controller_namespace + '/follow_joint_trajectory',
00124                                                           FollowJointTrajectoryAction,
00125                                                           execute_cb=self.process_follow_trajectory,
00126                                                           auto_start=False)
00127         self.action_server.start()
00128         Thread(target=self.update_state).start()
00129 
00130     def stop(self):
00131         self.running = False
00132 
00133     def process_command(self, msg):
00134         if self.action_server.is_active(): self.action_server.set_preempted()
00135         
00136         while self.action_server.is_active():
00137             rospy.sleep(0.01)
00138             
00139         self.process_trajectory(msg)
00140 
00141     def process_follow_trajectory(self, goal):
00142         self.process_trajectory(goal.trajectory)
00143 
00144     def process_trajectory(self, traj):
00145         num_points = len(traj.points)
00146         
00147         # make sure the joints in the goal match the joints of the controller
00148         if set(self.joint_names) != set(traj.joint_names):
00149             res = FollowJointTrajectoryResult()
00150             res.error_code=FollowJointTrajectoryResult.INVALID_JOINTS
00151             msg = 'Incoming trajectory joints do not match the joints of the controller'
00152             rospy.logerr(msg)
00153             rospy.logerr(' self.joint_names={%s}' % (set(self.joint_names)))
00154             rospy.logerr(' traj.joint_names={%s}' % (set(traj.joint_names)))
00155             self.action_server.set_aborted(result=res, text=msg)
00156             return
00157             
00158         # make sure trajectory is not empty
00159         if num_points == 0:
00160             msg = 'Incoming trajectory is empty'
00161             rospy.logerr(msg)
00162             self.action_server.set_aborted(text=msg)
00163             return
00164             
00165         # correlate the joints we're commanding to the joints in the message
00166         # map from an index of joint in the controller to an index in the trajectory
00167         lookup = [traj.joint_names.index(joint) for joint in self.joint_names]
00168         durations = [0.0] * num_points
00169         
00170         # find out the duration of each segment in the trajectory
00171         durations[0] = traj.points[0].time_from_start.to_sec()
00172         
00173         for i in range(1, num_points):
00174             durations[i] = (traj.points[i].time_from_start - traj.points[i - 1].time_from_start).to_sec()
00175             
00176         if not traj.points[0].positions:
00177             res = FollowJointTrajectoryResult()
00178             res.error_code=FollowJointTrajectoryResult.INVALID_GOAL
00179             msg = 'First point of trajectory has no positions'
00180             rospy.logerr(msg)
00181             self.action_server.set_aborted(result=res, text=msg)
00182             return
00183             
00184         trajectory = []
00185         time = rospy.Time.now() + rospy.Duration(0.01)
00186         
00187         for i in range(num_points):
00188             seg = Segment(self.num_joints)
00189             
00190             if traj.header.stamp == rospy.Time(0.0):
00191                 seg.start_time = (time + traj.points[i].time_from_start).to_sec() - durations[i]
00192             else:
00193                 seg.start_time = (traj.header.stamp + traj.points[i].time_from_start).to_sec() - durations[i]
00194                 
00195             seg.duration = durations[i]
00196             
00197             # Checks that the incoming segment has the right number of elements.
00198             if traj.points[i].velocities and len(traj.points[i].velocities) != self.num_joints:
00199                 res = FollowJointTrajectoryResult()
00200                 res.error_code=FollowJointTrajectoryResult.INVALID_GOAL
00201                 msg = 'Command point %d has %d elements for the velocities' % (i, len(traj.points[i].velocities))
00202                 rospy.logerr(msg)
00203                 self.action_server.set_aborted(result=res, text=msg)
00204                 return
00205                 
00206             if len(traj.points[i].positions) != self.num_joints:
00207                 res = FollowJointTrajectoryResult()
00208                 res.error_code=FollowJointTrajectoryResult.INVALID_GOAL
00209                 msg = 'Command point %d has %d elements for the positions' % (i, len(traj.points[i].positions))
00210                 rospy.logerr(msg)
00211                 self.action_server.set_aborted(result=res, text=msg)
00212                 return
00213                 
00214             for j in range(self.num_joints):
00215                 if traj.points[i].velocities:
00216                     seg.velocities[j] = traj.points[i].velocities[lookup[j]]
00217                 if traj.points[i].positions:
00218                     seg.positions[j] = traj.points[i].positions[lookup[j]]
00219                     
00220             trajectory.append(seg)
00221             
00222         rospy.loginfo('Trajectory start requested at %.3lf, waiting...', traj.header.stamp.to_sec())
00223         rate = rospy.Rate(self.update_rate)
00224         
00225         while traj.header.stamp > time:
00226             time = rospy.Time.now()
00227             rate.sleep()
00228             
00229         end_time = traj.header.stamp + rospy.Duration(sum(durations))
00230         seg_end_times = [rospy.Time.from_sec(trajectory[seg].start_time + durations[seg]) for seg in range(len(trajectory))]
00231         
00232         rospy.loginfo('Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf', time.to_sec(), end_time.to_sec(), sum(durations))
00233         
00234         self.trajectory = trajectory
00235         traj_start_time = rospy.Time.now()
00236         
00237         for seg in range(len(trajectory)):
00238             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()))
00239             rospy.logdebug('goal positions are: %s' % str(trajectory[seg].positions))
00240             
00241             # first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it
00242             if durations[seg] == 0:
00243                 rospy.logdebug('skipping segment %d with duration of 0 seconds' % seg)
00244                 continue
00245                 
00246             multi_packet = {}
00247             
00248             for port, joints in self.port_to_joints.items():
00249                 vals = []
00250                 
00251                 for joint in joints:
00252                     j = self.joint_names.index(joint)
00253                     
00254                     start_position = self.joint_states[joint].current_pos
00255                     if seg != 0: start_position = trajectory[seg - 1].positions[j]
00256                         
00257                     desired_position = trajectory[seg].positions[j]
00258                     desired_velocity = max(self.min_velocity, abs(desired_position - start_position) / durations[seg])
00259                     
00260                     self.msg.desired.positions[j] = desired_position
00261                     self.msg.desired.velocities[j] = desired_velocity
00262                     
00263                     # probably need a more elegant way of figuring out if we are dealing with a dual controller
00264                     if hasattr(self.joint_to_controller[joint], "master_id"):
00265                         master_id = self.joint_to_controller[joint].master_id
00266                         slave_id = self.joint_to_controller[joint].slave_id
00267                         master_pos, slave_pos = self.joint_to_controller[joint].pos_rad_to_raw(desired_position)
00268                         spd = self.joint_to_controller[joint].spd_rad_to_raw(desired_velocity)
00269                         vals.append((master_id, master_pos, spd))
00270                         vals.append((slave_id, slave_pos, spd))
00271                     else:
00272                         motor_id = self.joint_to_controller[joint].motor_id
00273                         pos = self.joint_to_controller[joint].pos_rad_to_raw(desired_position)
00274                         spd = self.joint_to_controller[joint].spd_rad_to_raw(desired_velocity)
00275                         vals.append((motor_id, pos, spd))
00276                     
00277                 multi_packet[port] = vals
00278                 
00279             for port, vals in multi_packet.items():
00280                 self.port_to_io[port].set_multi_position_and_speed(vals)
00281                 
00282             while time < seg_end_times[seg]:
00283                 # check if new trajectory was received, if so abort current trajectory execution
00284                 # by setting the goal to the current position
00285                 if self.action_server.is_preempt_requested():
00286                     msg = 'New trajectory received. Aborting old trajectory.'
00287                     multi_packet = {}
00288                     
00289                     for port, joints in self.port_to_joints.items():
00290                         vals = []
00291                         
00292                         for joint in joints:
00293                             cur_pos = self.joint_states[joint].current_pos
00294                             
00295                             motor_id = self.joint_to_controller[joint].motor_id
00296                             pos = self.joint_to_controller[joint].pos_rad_to_raw(cur_pos)
00297                             
00298                             vals.append((motor_id, pos))
00299                             
00300                         multi_packet[port] = vals
00301                         
00302                     for port, vals in multi_packet.items():
00303                         self.port_to_io[port].set_multi_position(vals)
00304                         
00305                     self.action_server.set_preempted(text=msg)
00306                     rospy.logwarn(msg)
00307                     return
00308                     
00309                 rate.sleep()
00310                 time = rospy.Time.now()
00311                 
00312             # Verifies trajectory constraints
00313             for j, joint in enumerate(self.joint_names):
00314                 if self.trajectory_constraints[j] > 0 and self.msg.error.positions[j] > self.trajectory_constraints[j]:
00315                     res = FollowJointTrajectoryResult()
00316                     res.error_code=FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
00317                     msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \
00318                            (joint, seg, self.msg.error.positions[j], self.trajectory_constraints[j])
00319                     rospy.logwarn(msg)
00320                     self.action_server.set_aborted(result=res, text=msg)
00321                     return
00322                     
00323         # let motors roll for specified amount of time
00324         rospy.sleep(self.goal_time_constraint)
00325         
00326         for i, j in enumerate(self.joint_names):
00327             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]))
00328             
00329         # Checks that we have ended inside the goal constraints
00330         for (joint, pos_error, pos_constraint) in zip(self.joint_names, self.msg.error.positions, self.goal_constraints):
00331             if pos_constraint > 0 and abs(pos_error) > pos_constraint:
00332                 res = FollowJointTrajectoryResult()
00333                 res.error_code=FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED
00334                 msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \
00335                       (joint, pos_error, pos_constraint)
00336                 rospy.logwarn(msg)
00337                 self.action_server.set_aborted(result=res, text=msg)
00338                 break
00339         else:
00340             msg = 'Trajectory execution successfully completed'
00341             rospy.loginfo(msg)
00342             res = FollowJointTrajectoryResult()  
00343             res.error_code=FollowJointTrajectoryResult.SUCCESSFUL
00344             self.action_server.set_succeeded(result=res, text=msg)
00345 
00346     def update_state(self):
00347         rate = rospy.Rate(self.state_update_rate)
00348         while self.running and not rospy.is_shutdown():
00349             self.msg.header.stamp = rospy.Time.now()
00350             
00351             # Publish current joint state
00352             for i, joint in enumerate(self.joint_names):
00353                 state = self.joint_states[joint]
00354                 self.msg.actual.positions[i] = state.current_pos
00355                 self.msg.actual.velocities[i] = abs(state.velocity)
00356                 self.msg.error.positions[i] = self.msg.actual.positions[i] - self.msg.desired.positions[i]
00357                 self.msg.error.velocities[i] = self.msg.actual.velocities[i] - self.msg.desired.velocities[i]
00358                 
00359             self.state_pub.publish(self.msg)
00360             rate.sleep()


dynamixel_controllers
Author(s): Antons Rebguns, Cody Jorgensen, Cara Slutter
autogenerated on Wed Feb 22 2017 16:47:35