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             msg = "Trajectory joint names does not match action controlled joints." + str(traj.joint_names)
00072             rospy.logerr(msg)
00073             self.server.set_aborted(text=msg)
00074             return
00075 
00076         if not traj.points:
00077             msg = "Trajectory empy."
00078             rospy.logerr(msg)
00079             self.server.set_aborted(text=msg)
00080             return
00081 
00082         try:
00083             indexes = [traj.joint_names.index(joint) for joint in self.joints]
00084         except ValueError as val:
00085             msg = "Trajectory invalid."
00086             rospy.logerr(msg)
00087             self.server.set_aborted(text=msg)
00088             return
00089 
00090         if self.executeTrajectory(traj):   
00091             self.server.set_succeeded()
00092         else:
00093             self.server.set_aborted(text="Execution failed.")
00094 
00095         rospy.loginfo(self.name + ": Done.")
00096     
00097     def commandCb(self, msg):
00098         # don't execute if executing an action
00099         if self.server.is_active():
00100             rospy.loginfo(self.name+": Received trajectory, but action is active")
00101             return
00102         self.executing = True
00103         self.executeTrajectory(msg)
00104         self.executing = False    
00105 
00106     def executeTrajectory(self, traj):
00107         rospy.loginfo("Executing trajectory")
00108         rospy.loginfo(traj)
00109         # carry out trajectory
00110         try:
00111             indexes = [traj.joint_names.index(joint) for joint in self.joints]
00112         except ValueError as val:
00113             return False
00114         time = rospy.Time.now()
00115         start = traj.header.stamp
00116         r = rospy.Rate(self.rate)
00117         last = [ self.device.joints[joint].position for joint in self.joints ]
00118         for point in traj.points:
00119             while rospy.Time.now() + rospy.Duration(0.01) < start:
00120                 rospy.sleep(0.01)
00121             desired = [ point.positions[k] for k in indexes ]
00122             endtime = start + point.time_from_start
00123             while rospy.Time.now() + rospy.Duration(0.01) < endtime:
00124                 err = [ (d-c) for d,c in zip(desired,last) ]
00125                 velocity = [ abs(x / (self.rate * (endtime - rospy.Time.now()).to_sec())) for x in err ]
00126                 rospy.logdebug(err)
00127                 for i in range(len(self.joints)):
00128                     if err[i] > 0.001 or err[i] < -0.001:
00129                         cmd = err[i] 
00130                         top = velocity[i]
00131                         if cmd > top:
00132                             cmd = top
00133                         elif cmd < -top:
00134                             cmd = -top
00135                         last[i] += cmd
00136                         self.device.joints[self.joints[i]].setControlOutput(last[i])
00137                     else:
00138                         velocity[i] = 0
00139                 r.sleep()
00140         return True
00141 
00142     def active(self):
00143         """ Is controller overriding servo internal control? """
00144         return self.server.is_active() or self.executing
00145 
00146     def getDiagnostics(self):
00147         """ Get a diagnostics status. """
00148         msg = DiagnosticStatus()
00149         msg.name = self.name
00150         msg.level = DiagnosticStatus.OK
00151         msg.message = "OK"
00152         if self.active():
00153             msg.values.append(KeyValue("State", "Active"))
00154         else:
00155             msg.values.append(KeyValue("State", "Not Active"))
00156         return msg
00157 


arbotix_python
Author(s): Michael Ferguson
autogenerated on Sat Dec 28 2013 16:46:13