示例#1
0
  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)
示例#2
0
 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)
示例#3
0
 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)
示例#4
0
 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)
示例#5
0
 def Deactivate(self):
     cmd = robotiq_msgs.CModel_robot_output()
     cmd.rACT = 0
     self.pub_grip.publish(cmd)
示例#6
0
 def StopGripper(self):
     cmd = robotiq_msgs.CModel_robot_output()
     cmd.rACT = 1
     cmd.rGTO = 0
     self.pub_grip.publish(cmd)
示例#7
0
 def Stop(self):
   cmd= robotiq_msgs.CModel_robot_output();
   cmd.rACT= 1
   cmd.rGTO= 0
   self.pub.grip.publish(cmd)
示例#8
0
        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()
示例#9
0
文件: rbt_rq.py 项目: akihikoy/ay_py
 def Stop(self, blocking=False):
     cmd = robotiq_msgs.CModel_robot_output()
     cmd.rACT = 1
     cmd.rGTO = 0
     self.pub.grip.publish(cmd)