baseSModel.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2012, Robotiq, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Robotiq, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 #
00033 # Copyright (c) 2012, Robotiq, Inc.
00034 # Revision $Id$
00035 
00036 """@package docstring
00037 Module baseSModel: defines a base class for handling command and status of the Robotiq S-Model gripper. 
00038 
00039 After being instanciated, a 'client' member must be added to the object. This client depends on the communication protocol used by the Gripper. As an example, the ROS node 'SModelTcpNode.py' instanciate a robotiqBaseSModel and adds a client defined in the module comModbusTcp.
00040 """
00041 
00042 from   robotiq_s_model_control.msg import _SModel_robot_input  as inputMsg
00043 from   robotiq_s_model_control.msg import _SModel_robot_output as outputMsg
00044 
00045 class robotiqBaseSModel:
00046     """Base class (communication protocol agnostic) for sending commands and receiving the status of the Robotic S-Model gripper."""
00047     
00048     def __init__(self):
00049 
00050         #Initiate output message as an empty list
00051         self.message = []
00052 
00053         #Note: after the instantiation, a ".client" member must be added to the object
00054         
00055         
00056     def verifyCommand(self, command):
00057         """Function to verify that the value of each variable satisfy its limits."""
00058                 
00059         #Verify that each variable is in its correct range
00060         command.rACT = max(0, command.rACT)
00061         command.rACT = min(1, command.rACT)
00062         
00063         command.rMOD = max(0, command.rMOD)
00064         command.rMOD = min(3, command.rMOD)
00065         
00066         command.rGTO = max(0, command.rGTO)
00067         command.rGTO = min(1, command.rGTO)
00068 
00069         command.rATR = max(0, command.rATR)
00070         command.rATR = min(1, command.rATR)
00071 
00072         command.rGLV = max(0, command.rGLV)
00073         command.rGLV = min(1, command.rGLV)
00074 
00075         command.rICF = max(0, command.rICF)
00076         command.rICF = min(1, command.rICF)
00077 
00078         command.rICS = max(0, command.rICS)
00079         command.rICS = min(1, command.rICS)
00080         
00081         command.rPRA = max(0,   command.rPRA)
00082         command.rPRA = min(255, command.rPRA)           
00083 
00084         command.rSPA = max(0,   command.rSPA)
00085         command.rSPA = min(255, command.rSPA)           
00086 
00087         command.rFRA = max(0,   command.rFRA)
00088         command.rFRA = min(255, command.rFRA)           
00089 
00090         command.rPRB = max(0,   command.rPRB)
00091         command.rPRB = min(255, command.rPRB)           
00092 
00093         command.rSPB = max(0,   command.rSPB)
00094         command.rSPB = min(255, command.rSPB)           
00095 
00096         command.rFRB = max(0,   command.rFRB)
00097         command.rFRB = min(255, command.rFRB)  
00098 
00099         command.rPRC = max(0,   command.rPRC)
00100         command.rPRC = min(255, command.rPRC)           
00101 
00102         command.rSPC = max(0,   command.rSPC)
00103         command.rSPC = min(255, command.rSPC)           
00104 
00105         command.rFRC = max(0,   command.rFRC)
00106         command.rFRC = min(255, command.rFRC)   
00107         
00108         command.rPRS = max(0,   command.rPRS)
00109         command.rPRS = min(255, command.rPRS)           
00110 
00111         command.rSPS = max(0,   command.rSPS)
00112         command.rSPS = min(255, command.rSPS)           
00113 
00114         command.rFRS = max(0,   command.rFRS)
00115         command.rFRS = min(255, command.rFRS) 
00116         
00117         #Return the modified command
00118         return command
00119         
00120     def refreshCommand(self, command):
00121         """Function to update the command which will be sent during the next sendCommand() call."""
00122         
00123         #Limit the value of each variable
00124         command = self.verifyCommand(command)
00125 
00126         #Initiate command as an empty list
00127         self.message = []
00128 
00129         #Build the command with each output variable
00130         self.message.append(command.rACT + (command.rMOD << 1) + (command.rGTO << 3) + (command.rATR << 4))
00131         self.message.append(command.rGLV + (command.rICF << 2) + (command.rICS << 3))
00132         self.message.append(0)
00133         self.message.append(command.rPRA)
00134         self.message.append(command.rSPA)
00135         self.message.append(command.rFRA)
00136         self.message.append(command.rPRB)
00137         self.message.append(command.rSPB)
00138         self.message.append(command.rFRB)
00139         self.message.append(command.rPRC)
00140         self.message.append(command.rSPC)
00141         self.message.append(command.rFRC)
00142         self.message.append(command.rPRS)
00143         self.message.append(command.rSPS)
00144         self.message.append(command.rFRS)        
00145 
00146     def sendCommand(self):
00147         """Send the command to the Gripper."""    
00148         
00149         self.client.sendCommand(self.message)
00150 
00151     def getStatus(self):
00152         """Request the status from the gripper and return it in the SModel_robot_input msg type."""    
00153 
00154         #Acquire status from the Gripper
00155         status = self.client.getStatus(16);
00156 
00157         #Message to output
00158         message = inputMsg.SModel_robot_input()
00159 
00160         #Assign the values to their respective variables
00161         message.gACT = (status[0] >> 0) & 0x01;
00162         message.gMOD = (status[0] >> 1) & 0x03;
00163         message.gGTO = (status[0] >> 3) & 0x01;
00164         message.gIMC = (status[0] >> 4) & 0x03;
00165         message.gSTA = (status[0] >> 6) & 0x03;
00166         message.gDTA = (status[1] >> 0) & 0x03;
00167         message.gDTB = (status[1] >> 2) & 0x03;
00168         message.gDTC = (status[1] >> 4) & 0x03;
00169         message.gDTS = (status[1] >> 6) & 0x03;
00170         message.gFLT =  status[2]
00171         message.gPRA =  status[3]
00172         message.gPOA =  status[4]
00173         message.gCUA =  status[5]
00174         message.gPRB =  status[6]
00175         message.gPOB =  status[7]
00176         message.gCUB =  status[8]
00177         message.gPRC =  status[9]
00178         message.gPOC =  status[10]
00179         message.gCUC =  status[11]
00180         message.gPRS =  status[12]
00181         message.gPOS =  status[13]
00182         message.gCUS =  status[14]
00183 
00184         return message
00185         
00186 
00187 


robotiq_s_model_control
Author(s): Nicolas Lauzier (Robotiq inc.)
autogenerated on Thu Aug 27 2015 14:44:26