parallel_gripper_action_controller.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 """
4  parallel_gripper_controller.py - controls a gripper built of two servos
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 import thread
32 
33 from control_msgs.msg import GripperCommandAction
34 from std_msgs.msg import Float64
35 from math import asin
36 
38  """ A simple controller that operates two opposing servos to
39  open/close to a particular size opening. """
40  def __init__(self):
41  rospy.init_node('gripper_controller')
42  rospy.logwarn("parallel_gripper_action_controller.py is deprecated and will be removed in ROS Indigo, please use gripper_controller")
43 
44  # trapezoid model: base width connecting each gripper's rotation point
45  # + length of gripper fingers to computation point
46  # = compute angles based on a desired width at comp. point
47  self.pad_width = rospy.get_param('~pad_width', 0.01)
48  self.finger_length = rospy.get_param('~finger_length', 0.02)
49  self.min_opening = rospy.get_param('~min', 0.0)
50  self.max_opening = rospy.get_param('~max', 2*self.finger_length)
51 
52  self.center_l = rospy.get_param('~center_left', 0.0)
53  self.center_r = rospy.get_param('~center_right', 0.0)
54  self.invert_l = rospy.get_param('~invert_left', False)
55  self.invert_r = rospy.get_param('~invert_right', False)
56 
57  # publishers
58  self.l_pub = rospy.Publisher('l_gripper_joint/command', Float64, queue_size=5)
59  self.r_pub = rospy.Publisher('r_gripper_joint/command', Float64, queue_size=5)
60 
61  # subscribe to command and then spin
62  self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
63  self.server.start()
64  rospy.spin()
65 
66  def actionCb(self, goal):
67  """ Take an input command of width to open gripper. """
68  rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
69  command = goal.command.position
70  # check limits
71  if command > self.max_opening:
72  command = self.max_opening
73  if command < self.min_opening:
74  command = self.min_opening
75  # compute angles
76  angle = asin((command - self.pad_width)/(2*self.finger_length))
77  if self.invert_l:
78  l = -angle + self.center_l
79  else:
80  l = angle + self.center_l
81  if self.invert_r:
82  r = angle + self.center_r
83  else:
84  r = -angle + self.center_r
85  # publish msgs
86  lmsg = Float64(l)
87  rmsg = Float64(r)
88  self.l_pub.publish(lmsg)
89  self.r_pub.publish(rmsg)
90  rospy.sleep(5.0)
91  self.server.set_succeeded()
92  rospy.loginfo('Gripper Controller: Done.')
93 
94 if __name__=='__main__':
95  try:
97  except rospy.ROSInterruptException:
98  rospy.loginfo('Hasta la Vista...')
99 


arbotix_controllers
Author(s): Michael Ferguson
autogenerated on Mon Jun 10 2019 12:43:39