1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
|
from py_trees.behaviour import Behaviour
from py_trees.common import Status
from constants import GRIPPER_TIMEOUT, GRASP_THRESHOLD, JOINT_TOLERANCE
from setup import robot, motors, sensors
joints = ["gripper_left_finger_joint", "gripper_right_finger_joint", ]
# For this module, ensure motors and sensors only contain the relevant entries.
motors = [motors[joint] for joint in joints]
sensors = [sensors[joint] for joint in joints]
class CloseGripper(Behaviour):
def __init__(self, name):
super().__init__(name)
self.position = .045
def initialise(self):
for motor in motors:
motor.setPosition(self.position)
self.initial_time = robot.getTime()
print("%s.initialise()" % (self.name, ))
def update(self):
if robot.getTime() - self.initial_time > GRIPPER_TIMEOUT:
print(
"%s.update()[%s->%s], Timed out"
% (self.name, self.status, Status.FAILURE)
)
return Status.FAILURE
# Close the grip little by little, as setting the position to 0 all at
# once creates strain on the gripper.
self.position -= .003
for motor in motors:
motor.setPosition(max(0, self.position))
# Terminate with SUCCESS when both grippers are exerting enough force.
if all([motor.getForceFeedback() < GRASP_THRESHOLD for motor in motors]):
print(
"%s.update()[%s->%s], both ends grasped with force > %.2f"
% (self.name, self.status, Status.SUCCESS, abs(GRASP_THRESHOLD))
)
return Status.SUCCESS
# Both grippers reached 0 without enough force being exerted, meaning
# the jar was not grabbed.
if all([sensor.getValue() < JOINT_TOLERANCE for sensor in sensors]):
print(
"%s.update()[%s->%s], gripper completely closed (empty)"
% (self.name, self.status, Status.FAILURE)
)
return Status.FAILURE
return Status.RUNNING
def terminate(self, new_status):
print(
"%s.terminate()[%s->%s], gripper positions: %.2f, %.2f"
% (self.name, self.status, new_status, *[s.getValue() for s in sensors])
)
class OpenGripper(Behaviour):
def __init__(self, name):
super().__init__(name)
def initialise(self):
print("%s.initialise()" % (self.name, ))
for motor in motors:
motor.setPosition(0.045)
self.initial_time = robot.getTime()
def update(self):
if robot.getTime() - self.initial_time > GRIPPER_TIMEOUT:
print(
"%s.update()[%s->%s], Timed out"
% (self.name, self.status, Status.FAILURE)
)
return Status.FAILURE
# Both joints are within JOINT_TOLERANCE from their respective targets.
if all([abs(sensor.getValue() - .045) < JOINT_TOLERANCE for sensor in sensors]):
print(
"%s.update()[%s->%s], gripper completely opened"
% (self.name, self.status, Status.SUCCESS)
)
return Status.SUCCESS
return Status.RUNNING
|
