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)