Go to the documentation of this file.00001
00002 """
00003 @file reactor_gripper.py
00004
00005
00006 Subscribes:
00007 -
00008
00009 Publishes:
00010 -
00011
00012 Actions:
00013
00014
00015 @author: Robotnik Automation
00016
00017 Software License Agreement (BSD License)
00018
00019 Copyright (c) 2015 Robotnik Automation SLL. All Rights Reserved.
00020
00021 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
00022
00023 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
00024
00025 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
00026 in the documentation and/or other materials provided with the distribution.
00027
00028 3. The name of the author may not be used to endorse or promote products derived from this software without specific prior written permission.
00029
00030 THIS SOFTWARE IS PROVIDED BY ROBOTNIK AUTOMATION SLL "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
00031 AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
00032 OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
00033 AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
00034 EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00035 """
00036
00037 import roslib; roslib.load_manifest('phantomx_reactor_arm_controller')
00038 import rospy
00039 import actionlib
00040 from control_msgs.msg import *
00041 from std_msgs.msg import *
00042 from sensor_msgs.msg import JointState
00043
00044
00045
00046 class WidowxGripper:
00047 """
00048 Class to communicate with the dinamyxel controllers of the arm
00049 """
00050 def __init__(self):
00051
00052 self.distance = 0.0;
00053
00054 try:
00055 self.name = rospy.get_param('~name', default = 'reactor')
00056 self.revolute_command = rospy.get_param('~revolute_command', default = '/gripper_revolute_joint/command')
00057 self.joint_name = rospy.get_param('~joint_name', default = 'gripper_right_joint')
00058 self.prismatic_command = rospy.get_param('~prismatic_command', default = '/gripper_prismatic_joint/command')
00059 self.gripper_revolute_joint_name = rospy.get_param('~revolute_joint_name', default = 'gripper_revolute_joint')
00060
00061
00062 self.open_gripper_angle = rospy.get_param('~open_gripper_angle', default = 0.0)
00063 self.open_gripper_distance = rospy.get_param('~open_gripper_distance', default = 0.03)
00064 self.close_gripper_angle = rospy.get_param('~close_gripper_angle', default = -2.5)
00065 self.close_gripper_distance = rospy.get_param('~close_gripper_distance', default = 0.0)
00066
00067
00068 self.angle_to_distance_param_a = (self.open_gripper_angle - self.close_gripper_angle) / (self.open_gripper_distance - self.close_gripper_distance)
00069 self.angle_to_distance_param_b = self.close_gripper_angle - self.angle_to_distance_param_a*self.close_gripper_distance
00070
00071
00072 self.gripper_revolute_joint_state_subscriber = rospy.Subscriber('/joint_states', JointState, self.jointStateCb)
00073 self.gripper_prismatic_joint_subscriber = rospy.Subscriber(self.prismatic_command, Float64, self.gripperPrismaticJointCommandCb)
00074 self.gripper_prismatic_joint_state_publisher = rospy.Publisher('/joint_states', JointState, queue_size=10)
00075 self.gripper_revolute_joint_publisher = rospy.Publisher(self.revolute_command, Float64, queue_size=10)
00076
00077
00078 self.desired_freq = rospy.get_param('~desired_freq', default = 10.0)
00079
00080 except rospy.ROSException, e:
00081 rospy.logerr('%s: error getting params %s'%(rospy.get_name(), e))
00082 exit()
00083
00084 def angleToDistance(self, angle):
00085 """
00086 Receives angle in rads and return distance in meters
00087 """
00088
00089 return (angle - self.angle_to_distance_param_b) / self.angle_to_distance_param_a
00090
00091 def distanceToAngle(self, distance):
00092 """
00093 Receives distance in meters and returns angle in rads
00094 """
00095
00096 return self.angle_to_distance_param_a*distance + self.angle_to_distance_param_b
00097
00098
00099 def jointStateCb(self, msg):
00100
00101 if self.gripper_revolute_joint_name in msg.name:
00102 index = msg.name.index(self.gripper_revolute_joint_name)
00103 position = msg.position[index]
00104 velocity = msg.velocity[index]
00105
00106 self.distance = self.angleToDistance(position)
00107
00108 def gripperPrismaticJointCommandCb(self, msg):
00109 """
00110 Receives joint command and send it to the controller
00111 """
00112
00113
00114 gripper_goal = Float64()
00115
00116 if(msg.data >= 0 and msg.data <= 0.03):
00117 self.distance = msg.data;
00118 rad = self.distanceToAngle(self.distance)
00119 gripper_goal.data = rad
00120 self.gripper_revolute_joint_publisher.publish(gripper_goal)
00121
00122
00123 def controlLoop(self):
00124 """
00125 Runs the control loop
00126 """
00127 joint_state_gripper = JointState()
00128
00129 joint_state_gripper.header = Header()
00130 joint_state_gripper.header.stamp = rospy.Time.now()
00131 joint_state_gripper.name = [self.joint_name]
00132 joint_state_gripper.position = [self.distance]
00133 joint_state_gripper.velocity = [0.0]
00134 joint_state_gripper.effort = [0.0]
00135
00136 self.gripper_prismatic_joint_state_publisher.publish(joint_state_gripper)
00137
00138 t_sleep = 1.0/self.desired_freq
00139
00140 while not rospy.is_shutdown():
00141 joint_state_gripper.header.stamp = rospy.Time.now()
00142 joint_state_gripper.position = [self.distance/2.0]
00143
00144 self.gripper_prismatic_joint_state_publisher.publish(joint_state_gripper)
00145 rospy.sleep(t_sleep)
00146
00147 def start(self):
00148 """
00149 Starts the action server and runs spin
00150 """
00151 try:
00152 self.controlLoop()
00153 except rospy.ROSInterruptException:
00154 rospy.loginfo('%s: Bye!'%rospy.get_name())
00155
00156 def main():
00157
00158 rospy.init_node('reactor_gripper_node')
00159
00160 widowx_node = WidowxGripper()
00161
00162 widowx_node.start()
00163
00164
00165 if __name__=='__main__':
00166 main()
00167 exit()
00168