def Move(self, pos, max_effort=50.0, speed=50.0, blocking=False):
clip= lambda c: max(0,min(255,int(c)))
pos= clip(self.rqg_pos2cmd(pos))
max_effort= clip(max_effort*(255.0/100.0))
speed= clip(speed*(255.0/100.0))
cmd= robotiq_msgs.CModel_robot_output();
cmd.rACT= 1
cmd.rGTO= 1
cmd.rPR= pos #Position Request
cmd.rSP= speed
cmd.rFR= max_effort
self.pub.grip.publish(cmd)
if blocking:
while pos!=self.status.gPR and not rospy.is_shutdown():
#self.PrintStatus(self.status)
rospy.sleep(0.001)
prev_PO= None
CMAX= 500
counter= CMAX
while not (self.status.gGTO==0 or self.status.gOBJ==3) and not rospy.is_shutdown():
#self.PrintStatus(self.status)
if self.status.gPO==prev_PO: counter-= 1
else: counter= CMAX
if counter==0: break
prev_PO= self.status.gPO
rospy.sleep(0.001)
def Activate(self):
cmd = robotiq_msgs.CModel_robot_output()
cmd.rACT = 1
cmd.rGTO = 1
cmd.rSP = 255 #SPeed
cmd.rFR = 150 #FoRce
self.pub_grip.publish(cmd)
def MoveGripper(self, pos, max_effort, speed=255, blocking=False):
cmd = robotiq_msgs.CModel_robot_output()
cmd.rACT = 1
cmd.rGTO = 1
cmd.rPR = pos #Position Request
cmd.rSP = speed
cmd.rFR = max_effort
self.pub_grip.publish(cmd)
def MoveGripper(self, pos, max_effort, speed=255, blocking=False):
pos = max(0, min(255, int(pos)))
cmd = robotiq_msgs.CModel_robot_output()
cmd.rACT = 1
cmd.rGTO = 1
cmd.rPR = pos #Position Request
cmd.rSP = speed
cmd.rFR = max_effort
self.pub_grip.publish(cmd)
if blocking:
while pos != self.status.gPR and not rospy.is_shutdown():
#self.PrintStatus(self.status)
rospy.sleep(0.001)
prev_PO = None
CMAX = 500
counter = CMAX
while not (self.status.gGTO == 0
or self.status.gOBJ == 3) and not rospy.is_shutdown():
#self.PrintStatus(self.status)
if self.status.gPO == prev_PO: counter -= 1
else: counter = CMAX
if counter == 0: break
prev_PO = self.status.gPO
rospy.sleep(0.001)
def Deactivate(self):
cmd = robotiq_msgs.CModel_robot_output()
cmd.rACT = 0
self.pub_grip.publish(cmd)
def StopGripper(self):
cmd = robotiq_msgs.CModel_robot_output()
cmd.rACT = 1
cmd.rGTO = 0
self.pub_grip.publish(cmd)
def Stop(self): cmd= robotiq_msgs.CModel_robot_output(); cmd.rACT= 1 cmd.rGTO= 0 self.pub.grip.publish(cmd)
cmd.rACT = 1
cmd.rGTO = 1
cmd.rPR = pos #Position Request
cmd.rSP = speed
cmd.rFR = max_effort
self.pub_grip.publish(cmd)
#TODO:FIXME:blocking is not implemented yet
if __name__ == '__main__':
rospy.init_node('robotiq_test')
rq = TRobotiq()
rq.Init()
time.sleep(1)
raw_input('wait activation>')
print robotiq_msgs.CModel_robot_output()
print 'closing gripper'
rq.CloseGripper()
time.sleep(1)
print 'opening gripper'
rq.OpenGripper()
time.sleep(1)
for t in FRange1(0.0, 5.0, 500):
p = 255 * 0.5 * (1.0 - math.cos(t * 2.0 * math.pi / 5.0))
#print t,p
rq.MoveGripper(p, 100)
time.sleep(0.01)
rq.Cleanup()
def Stop(self, blocking=False):
cmd = robotiq_msgs.CModel_robot_output()
cmd.rACT = 1
cmd.rGTO = 0
self.pub.grip.publish(cmd)
