def ExampleSendGripperCommands(self):
# Create the GripperCommand we will send
gripper_command = Base_pb2.GripperCommand()
finger = gripper_command.gripper.finger.add()
# Close the gripper with position increments
print("Performing gripper test in position...")
gripper_command.mode = Base_pb2.GRIPPER_POSITION
position = 0.00
finger.finger_identifier = 1
while position < 1.0:
finger.value = position
print("Going to position {:0.2f}...".format(finger.value))
self.base.SendGripperCommand(gripper_command)
position += 0.1
time.sleep(1)
# Set speed to open gripper
print("Opening gripper using speed command...")
gripper_command.mode = Base_pb2.GRIPPER_SPEED
finger.value = 0.1
self.base.SendGripperCommand(gripper_command)
gripper_request = Base_pb2.GripperRequest()
# Wait for reported position to be opened
gripper_request.mode = Base_pb2.GRIPPER_POSITION
while True:
gripper_measure = self.base.GetMeasuredGripperMovement(
gripper_request)
if len(gripper_measure.finger):
print("Current position is : {0}".format(
gripper_measure.finger[0].value))
if gripper_measure.finger[0].value < 0.01:
break
else: # Else, no finger present in answer, end loop
break
# Set speed to close gripper
print("Closing gripper using speed command...")
gripper_command.mode = Base_pb2.GRIPPER_SPEED
finger.value = -0.1
self.base.SendGripperCommand(gripper_command)
# Wait for reported speed to be 0
gripper_request.mode = Base_pb2.GRIPPER_SPEED
while True:
gripper_measure = self.base.GetMeasuredGripperMovement(
gripper_request)
if len(gripper_measure.finger):
print("Current speed is : {0}".format(
gripper_measure.finger[0].value))
if gripper_measure.finger[0].value == 0.0:
break
else: # Else, no finger present in answer, end loop
break
def send_gripper_command(base,value=0.5):
# desired opening value 1=close, 0 is open
opening_val=value
speed_val_=0.1
last_gripper_val=666
t0=time.time()
#check the finger value
# Create the GripperCommand we will send
gripper_command = Base_pb2.GripperCommand()
finger = gripper_command.gripper.finger.add()
gripper_request = Base_pb2.GripperRequest()
# Wait for reported position to be opened
gripper_request.mode = Base_pb2.GRIPPER_POSITION
gripper_measure = base.GetMeasuredGripperMovement(gripper_request)
if gripper_measure.finger[0].value < opening_val: #close
speed_val=-speed_val_
gr_action="close"
else: #open
speed_val=speed_val_
gr_action="open"
# Create the GripperCommand we will send
print("moving to ",str(opening_val))
gripper_command = Base_pb2.GripperCommand()
finger = gripper_command.gripper.finger.add()
# Set speed to open gripper
if gr_action=="open":
print ("Opening gripper using speed command...")
gripper_command.mode = Base_pb2.GRIPPER_SPEED
finger.value = speed_val
base.SendGripperCommand(gripper_command)
gripper_request = Base_pb2.GripperRequest()
# Wait for reported position to be opened
gripper_request.mode = Base_pb2.GRIPPER_POSITION
while True:
gripper_measure = base.GetMeasuredGripperMovement(gripper_request)
if len (gripper_measure.finger):
act_gripper_val=gripper_measure.finger[0].value
delta=abs(last_gripper_val-act_gripper_val)
print("Current pos is : {0}".format(act_gripper_val))
time.sleep(0.1)
if delta==0: #gripper cant move anymore
break
elif act_gripper_val < opening_val+0.01 or time.time()-t0>5:
break
last_gripper_val=act_gripper_val
else: # Else, no finger present in answer, end loop
break
else:
# Set speed to close gripper
print ("Closing gripper using speed command...")
gripper_command.mode = Base_pb2.GRIPPER_SPEED
finger.value = speed_val
base.SendGripperCommand(gripper_command)
# Wait for reported speed to be 0
gripper_request.mode = Base_pb2.GRIPPER_POSITION
while True:
gripper_measure = base.GetMeasuredGripperMovement(gripper_request)
if len (gripper_measure.finger):
act_gripper_val=gripper_measure.finger[0].value
delta=abs(last_gripper_val-act_gripper_val)
print("Current pos is : {0}".format(act_gripper_val))
time.sleep(0.1)
if delta==0: #gripper cant move anymore
break
elif act_gripper_val > opening_val-0.05:
break
elif time.time()-t0>5:
break
last_gripper_val=act_gripper_val
else: # Else, no finger present in answer, end loop
break
base.Stop()
return True