follow_controller.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 """
4  follow_controller.py - controller for a kinematic chain
5  Copyright (c) 2011 Vanadium Labs LLC. All right reserved.
6 
7  Redistribution and use in source and binary forms, with or without
8  modification, are permitted provided that the following conditions are met:
9  * Redistributions of source code must retain the above copyright
10  notice, this list of conditions and the following disclaimer.
11  * Redistributions in binary form must reproduce the above copyright
12  notice, this list of conditions and the following disclaimer in the
13  documentation and/or other materials provided with the distribution.
14  * Neither the name of Vanadium Labs LLC nor the names of its
15  contributors may be used to endorse or promote products derived
16  from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
19  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
20  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21  DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
22  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
24  OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
25  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
26  OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
27  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 """
29 
30 import rospy, actionlib
31 
32 from control_msgs.msg import FollowJointTrajectoryAction
33 from trajectory_msgs.msg import JointTrajectory
34 from diagnostic_msgs.msg import *
35 
36 from ax12 import *
37 from controllers import *
38 
40  """ A controller for joint chains, exposing a FollowJointTrajectory action. """
41 
42  def __init__(self, device, name):
43  Controller.__init__(self, device, name)
44  self.interpolating = 0
45 
46  # parameters: rates and joints
47  self.rate = rospy.get_param('~controllers/'+name+'/rate',50.0)
48  self.joints = rospy.get_param('~controllers/'+name+'/joints')
49  self.index = rospy.get_param('~controllers/'+name+'/index', len(device.controllers))
50  for joint in self.joints:
51  self.device.joints[joint].controller = self
52 
53  # action server
54  name = rospy.get_param('~controllers/'+name+'/action_name','follow_joint_trajectory')
55  self.server = actionlib.SimpleActionServer(name, FollowJointTrajectoryAction, execute_cb=self.actionCb, auto_start=False)
56 
57  # good old trajectory
58  rospy.Subscriber(self.name+'/command', JointTrajectory, self.commandCb)
59  self.executing = False
60 
61  rospy.loginfo("Started FollowController ("+self.name+"). Joints: " + str(self.joints) + " on C" + str(self.index))
62 
63  def startup(self):
64  self.server.start()
65 
66  def actionCb(self, goal):
67  rospy.loginfo(self.name + ": Action goal recieved.")
68  traj = goal.trajectory
69 
70  if set(self.joints) != set(traj.joint_names):
71  for j in self.joints:
72  if j not in traj.joint_names:
73  msg = "Trajectory joint names does not match action controlled joints." + str(traj.joint_names)
74  rospy.logerr(msg)
75  self.server.set_aborted(text=msg)
76  return
77  rospy.logwarn("Extra joints in trajectory")
78 
79  if not traj.points:
80  msg = "Trajectory empy."
81  rospy.logerr(msg)
82  self.server.set_aborted(text=msg)
83  return
84 
85  try:
86  indexes = [traj.joint_names.index(joint) for joint in self.joints]
87  except ValueError as val:
88  msg = "Trajectory invalid."
89  rospy.logerr(msg)
90  self.server.set_aborted(text=msg)
91  return
92 
93  retval = self.executeTrajectory(traj) # retval: 1: successful, 0: canceled, -1: failed
94  if retval == 1:
95  self.server.set_succeeded()
96  rospy.loginfo(self.name + ": Done.")
97  elif retval == 0:
98  self.server.set_preempted(text="Goal canceled.")
99  rospy.loginfo(self.name + ": Goal canceled.")
100  else:
101  self.server.set_aborted(text="Execution failed.")
102  rospy.loginfo(self.name + ": Execution failed.")
103 
104 
105  def commandCb(self, msg):
106  # don't execute if executing an action
107  if self.server.is_active():
108  rospy.loginfo(self.name+": Received trajectory, but action is active")
109  return
110  self.executing = True
111  self.executeTrajectory(msg)
112  self.executing = False
113 
114  def executeTrajectory(self, traj):
115  rospy.loginfo("Executing trajectory")
116  rospy.logdebug(traj)
117  # carry out trajectory
118  try:
119  indexes = [traj.joint_names.index(joint) for joint in self.joints]
120  except ValueError as val:
121  rospy.logerr("Invalid joint in trajectory.")
122  return -1
123 
124  # get starting timestamp, MoveIt uses 0, need to fill in
125  start = traj.header.stamp
126  if start.secs == 0 and start.nsecs == 0:
127  start = rospy.Time.now()
128 
129  r = rospy.Rate(self.rate)
130  last = [ self.device.joints[joint].position for joint in self.joints ]
131  for point in traj.points:
132  while rospy.Time.now() + rospy.Duration(0.01) < start:
133  if self.server.is_preempt_requested():
134  return 0
135  rospy.sleep(0.01)
136  desired = [ point.positions[k] for k in indexes ]
137  endtime = start + point.time_from_start
138  while rospy.Time.now() + rospy.Duration(0.01) < endtime:
139  # check that preempt has not been requested by the client
140  if self.server.is_preempt_requested():
141  return 0
142 
143  err = [ (d-c) for d,c in zip(desired,last) ]
144  velocity = [ abs(x / (self.rate * (endtime - rospy.Time.now()).to_sec())) for x in err ]
145  rospy.logdebug(err)
146  for i in range(len(self.joints)):
147  if err[i] > 0.001 or err[i] < -0.001:
148  cmd = err[i]
149  top = velocity[i]
150  if cmd > top:
151  cmd = top
152  elif cmd < -top:
153  cmd = -top
154  last[i] += cmd
155  self.device.joints[self.joints[i]].setControlOutput(last[i])
156  else:
157  velocity[i] = 0
158  r.sleep()
159  return 1
160 
161  def active(self):
162  """ Is controller overriding servo internal control? """
163  return self.server.is_active() or self.executing
164 
165  def getDiagnostics(self):
166  """ Get a diagnostics status. """
167  msg = DiagnosticStatus()
168  msg.name = self.name
169  msg.level = DiagnosticStatus.OK
170  msg.message = "OK"
171  if self.active():
172  msg.values.append(KeyValue("State", "Active"))
173  else:
174  msg.values.append(KeyValue("State", "Not Active"))
175  return msg
176 
Controllers interact with ArbotiX hardware.
Definition: controllers.py:32
def active(self)
Is the controller actively sending commands to joints?
Definition: controllers.py:63


arbotix_python
Author(s): Michael Ferguson
autogenerated on Mon Feb 28 2022 21:37:42