follow_controller.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004   follow_controller.py - controller for a kinematic chain
00005   Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
00006 
00007   Redistribution and use in source and binary forms, with or without
00008   modification, are permitted provided that the following conditions are met:
00009       * Redistributions of source code must retain the above copyright
00010         notice, this list of conditions and the following disclaimer.
00011       * Redistributions in binary form must reproduce the above copyright
00012         notice, this list of conditions and the following disclaimer in the
00013         documentation and/or other materials provided with the distribution.
00014       * Neither the name of Vanadium Labs LLC nor the names of its 
00015         contributors may be used to endorse or promote products derived 
00016         from this software without specific prior written permission.
00017   
00018   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019   ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020   WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021   DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00022   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00023   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00024   OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00025   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00026   OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00027   ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 """
00029 
00030 import rospy, actionlib
00031 
00032 from control_msgs.msg import FollowJointTrajectoryAction
00033 from trajectory_msgs.msg import JointTrajectory
00034 from diagnostic_msgs.msg import *
00035 
00036 from ax12 import *
00037 from controllers import *
00038 
00039 class FollowController(Controller):
00040     """ A controller for joint chains, exposing a FollowJointTrajectory action. """
00041 
00042     def __init__(self, device, name):
00043         Controller.__init__(self, device, name)
00044         self.interpolating = 0
00045 
00046         # parameters: rates and joints
00047         self.rate = rospy.get_param('~controllers/'+name+'/rate',50.0)
00048         self.joints = rospy.get_param('~controllers/'+name+'/joints')
00049         self.index = rospy.get_param('~controllers/'+name+'/index', len(device.controllers))
00050         for joint in self.joints:
00051             self.device.joints[joint].controller = self
00052 
00053         # action server
00054         name = rospy.get_param('~controllers/'+name+'/action_name','follow_joint_trajectory')
00055         self.server = actionlib.SimpleActionServer(name, FollowJointTrajectoryAction, execute_cb=self.actionCb, auto_start=False)
00056 
00057         # good old trajectory
00058         rospy.Subscriber(self.name+'/command', JointTrajectory, self.commandCb)
00059         self.executing = False
00060 
00061         rospy.loginfo("Started FollowController ("+self.name+"). Joints: " + str(self.joints) + " on C" + str(self.index))
00062 
00063     def startup(self):
00064         self.server.start()
00065 
00066     def actionCb(self, goal):
00067         rospy.loginfo(self.name + ": Action goal recieved.")
00068         traj = goal.trajectory
00069 
00070         if set(self.joints) != set(traj.joint_names):
00071             for j in self.joints:
00072                 if j not in traj.joint_names:
00073                     msg = "Trajectory joint names does not match action controlled joints." + str(traj.joint_names)
00074                     rospy.logerr(msg)
00075                     self.server.set_aborted(text=msg)
00076                     return
00077             rospy.logwarn("Extra joints in trajectory")
00078 
00079         if not traj.points:
00080             msg = "Trajectory empy."
00081             rospy.logerr(msg)
00082             self.server.set_aborted(text=msg)
00083             return
00084 
00085         try:
00086             indexes = [traj.joint_names.index(joint) for joint in self.joints]
00087         except ValueError as val:
00088             msg = "Trajectory invalid."
00089             rospy.logerr(msg)
00090             self.server.set_aborted(text=msg)
00091             return
00092 
00093         if self.executeTrajectory(traj):   
00094             self.server.set_succeeded()
00095         else:
00096             self.server.set_aborted(text="Execution failed.")
00097 
00098         rospy.loginfo(self.name + ": Done.")
00099     
00100     def commandCb(self, msg):
00101         # don't execute if executing an action
00102         if self.server.is_active():
00103             rospy.loginfo(self.name+": Received trajectory, but action is active")
00104             return
00105         self.executing = True
00106         self.executeTrajectory(msg)
00107         self.executing = False    
00108 
00109     def executeTrajectory(self, traj):
00110         rospy.loginfo("Executing trajectory")
00111         rospy.logdebug(traj)
00112         # carry out trajectory
00113         try:
00114             indexes = [traj.joint_names.index(joint) for joint in self.joints]
00115         except ValueError as val:
00116             rospy.logerr("Invalid joint in trajectory.")
00117             return False
00118 
00119         # get starting timestamp, MoveIt uses 0, need to fill in
00120         start = traj.header.stamp
00121         if start.secs == 0 and start.nsecs == 0:
00122             start = rospy.Time.now()
00123 
00124         r = rospy.Rate(self.rate)
00125         last = [ self.device.joints[joint].position for joint in self.joints ]
00126         for point in traj.points:
00127             while rospy.Time.now() + rospy.Duration(0.01) < start:
00128                 rospy.sleep(0.01)
00129             desired = [ point.positions[k] for k in indexes ]
00130             endtime = start + point.time_from_start
00131             while rospy.Time.now() + rospy.Duration(0.01) < endtime:
00132                 err = [ (d-c) for d,c in zip(desired,last) ]
00133                 velocity = [ abs(x / (self.rate * (endtime - rospy.Time.now()).to_sec())) for x in err ]
00134                 rospy.logdebug(err)
00135                 for i in range(len(self.joints)):
00136                     if err[i] > 0.001 or err[i] < -0.001:
00137                         cmd = err[i] 
00138                         top = velocity[i]
00139                         if cmd > top:
00140                             cmd = top
00141                         elif cmd < -top:
00142                             cmd = -top
00143                         last[i] += cmd
00144                         self.device.joints[self.joints[i]].setControlOutput(last[i])
00145                     else:
00146                         velocity[i] = 0
00147                 r.sleep()
00148         return True
00149 
00150     def active(self):
00151         """ Is controller overriding servo internal control? """
00152         return self.server.is_active() or self.executing
00153 
00154     def getDiagnostics(self):
00155         """ Get a diagnostics status. """
00156         msg = DiagnosticStatus()
00157         msg.name = self.name
00158         msg.level = DiagnosticStatus.OK
00159         msg.message = "OK"
00160         if self.active():
00161             msg.values.append(KeyValue("State", "Active"))
00162         else:
00163             msg.values.append(KeyValue("State", "Not Active"))
00164         return msg
00165 


arbotix_python
Author(s): Michael Ferguson
autogenerated on Wed Aug 26 2015 10:44:20