robotiq_c_ctrl.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import numpy as np
00004 
00005 import roslib
00006 roslib.load_manifest('robotiq_c_model_control')
00007 
00008 import rospy
00009 from robotiq_c_model_control.msg import CModel_robot_output as outputMsg
00010 from robotiq_c_model_control.msg import CModel_robot_input as inputMsg
00011 
00012 class RobotiqCGripper(object):
00013     def __init__(self):
00014         self.cur_status = None
00015         self.status_sub = rospy.Subscriber('CModelRobotInput', inputMsg, 
00016                                            self._status_cb)
00017         self.cmd_pub = rospy.Publisher('CModelRobotOutput', outputMsg)
00018 
00019     def _status_cb(self, msg):
00020         self.cur_status = msg
00021 
00022     def wait_for_connection(self, timeout=-1):
00023         rospy.sleep(0.1)
00024         r = rospy.Rate(30)
00025         start_time = rospy.get_time()
00026         while not rospy.is_shutdown():
00027             if (timeout >= 0. and rospy.get_time() - start_time > timeout):
00028                 return False
00029             if self.cur_status is not None:
00030                 return True
00031             r.sleep()
00032         return False
00033 
00034     def is_ready(self):
00035         return self.cur_status.gSTA == 3 and self.cur_status.gACT == 1
00036 
00037     def is_reset(self):
00038         return self.cur_status.gSTA == 0 or self.cur_status.gACT == 0
00039 
00040     def is_moving(self):
00041         return self.cur_status.gGTO == 1 and self.cur_status.gOBJ == 0
00042 
00043     def is_stopped(self):
00044         return self.cur_status.gOBJ != 0
00045 
00046     def object_detected(self):
00047         return self.cur_status.gOBJ == 1 or self.cur_status.gOBJ == 2
00048 
00049     def get_fault_status(self):
00050         return self.cur_status.gFLT
00051 
00052     def get_pos(self):
00053         po = self.cur_status.gPO
00054         return np.clip(0.087/(13.-230.)*(po-230.), 0, 0.087)
00055 
00056     def get_req_pos(self):
00057         pr = self.cur_status.gPR
00058         return np.clip(0.087/(13.-230.)*(pr-230.), 0, 0.087)
00059 
00060     def is_closed(self):
00061         return self.cur_status.gPO >= 230
00062 
00063     def is_opened(self):
00064         return self.cur_status.gPO <= 13
00065 
00066     # in mA
00067     def get_current(self):
00068         return self.cur_status.gCU * 0.1
00069 
00070     # if timeout is negative, wait forever
00071     def wait_until_stopped(self, timeout=-1):
00072         r = rospy.Rate(30)
00073         start_time = rospy.get_time()
00074         while not rospy.is_shutdown():
00075             if (timeout >= 0. and rospy.get_time() - start_time > timeout) or self.is_reset():
00076                 return False
00077             if self.is_stopped():
00078                 return True
00079             r.sleep()
00080         return False
00081 
00082     def wait_until_moving(self, timeout=-1):
00083         r = rospy.Rate(30)
00084         start_time = rospy.get_time()
00085         while not rospy.is_shutdown():
00086             if (timeout >= 0. and rospy.get_time() - start_time > timeout) or self.is_reset():
00087                 return False
00088             if not self.is_stopped():
00089                 return True
00090             r.sleep()
00091         return False
00092 
00093     def reset(self):
00094         cmd = outputMsg()
00095         cmd.rACT = 0
00096         self.cmd_pub.publish(cmd)
00097 
00098     def activate(self, timeout=-1):
00099         cmd = outputMsg()
00100         cmd.rACT = 1
00101         cmd.rGTO = 1
00102         cmd.rPR = 0
00103         cmd.rSP = 255
00104         cmd.rFR = 150
00105         self.cmd_pub.publish(cmd)
00106         r = rospy.Rate(30)
00107         start_time = rospy.get_time()
00108         while not rospy.is_shutdown():
00109             if timeout >= 0. and rospy.get_time() - start_time > timeout:
00110                 return False
00111             if self.is_ready():
00112                 return True
00113             r.sleep()
00114         return False
00115 
00116     def auto_release(self):
00117         cmd = outputMsg()
00118         cmd.rACT = 1
00119         cmd.rATR = 1
00120         self.cmd_pub.publish(cmd)
00121 
00122     ##
00123     # Goto position with desired force and velocity
00124     # @param pos Gripper width in meters. [0, 0.087]
00125     # @param vel Gripper speed in m/s. [0.013, 0.100]
00126     # @param force Gripper force in N. [30, 100] (not precise)
00127     def goto(self, pos, vel, force, block=False, timeout=-1):
00128         cmd = outputMsg()
00129         cmd.rACT = 1
00130         cmd.rGTO = 1
00131         cmd.rPR = int(np.clip((13.-230.)/0.087 * pos + 230., 0, 255))
00132         cmd.rSP = int(np.clip(255./(0.1-0.013) * (vel-0.013), 0, 255))
00133         cmd.rFR = int(np.clip(255./(100.-30.) * (force-30.), 0, 255))
00134         self.cmd_pub.publish(cmd)
00135         rospy.sleep(0.1)
00136         if block:
00137             if not self.wait_until_moving(timeout):
00138                 return False
00139             return self.wait_until_stopped(timeout)
00140         return True
00141 
00142     def stop(self, block=False, timeout=-1):
00143         cmd = outputMsg()
00144         cmd.rACT = 1
00145         cmd.rGTO = 0
00146         self.cmd_pub.publish(cmd)
00147         rospy.sleep(0.1)
00148         if block:
00149             return self.wait_until_stopped(timeout)
00150         return True
00151 
00152     def open(self, vel=0.1, force=100, block=False, timeout=-1):
00153         if self.is_opened():
00154             return True
00155         return self.goto(1.0, vel, force, block=block, timeout=timeout)
00156 
00157     def close(self, vel=0.1, force=100, block=False, timeout=-1):
00158         if self.is_closed():
00159             return True
00160         return self.goto(-1.0, vel, force, block=block, timeout=timeout)
00161 
00162 def main():
00163     rospy.init_node("robotiq_c_ctrl_test")
00164     gripper = RobotiqCGripper()
00165     gripper.wait_for_connection()
00166     if gripper.is_reset():
00167         gripper.reset()
00168         gripper.activate()
00169     print gripper.close(block=True)
00170     while not rospy.is_shutdown():
00171         print gripper.open(block=False)
00172         rospy.sleep(0.11)
00173         gripper.stop()
00174         print gripper.close(block=False)
00175         rospy.sleep(0.1)
00176         gripper.stop()
00177 
00178 if __name__ == '__main__':
00179     main()


robotiq_c_model_control
Author(s): Nicolas Lauzier (Robotiq inc.)
autogenerated on Thu Jun 6 2019 17:58:00