-
Notifications
You must be signed in to change notification settings - Fork 0
/
pick_and_place_arm_sm.py
241 lines (185 loc) · 8.53 KB
/
pick_and_place_arm_sm.py
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
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
# state machine for pick and place based on
# https://python-3-patterns-idioms-test.readthedocs.io/en/latest/StateMachine.html
from enum import Enum
import math
from rospy import loginfo, logwarn
import PyKDL
import pprint
# TODO: failed pickup state transition from APPROACH_DEST to APPROACH_OBJECT
# TODO: pass entire estimated world into the run_once function
class PickAndPlaceState(Enum):
OPEN_JAW = 1,
APPROACH_OBJECT = 2,
GRAB_OBJECT = 3,
CLOSE_JAW = 4,
APPROACH_DEST = 5,
DROP_OBJECT = 6,
HOME = 7,
DONE = 8
VECTOR_EPS = 0.005
DOWN_JAW_ORIENTATION = PyKDL.Rotation.RPY(math.pi, 0, - math.pi / 2.0)
PSM_HOME_POS = PyKDL.Vector(0., 0., -0.1)
def vector_eps_eq(lhs, rhs):
return bool((lhs - rhs).Norm() < 0.005)
class PickAndPlaceStateMachine:
def jaw_fully_open(self):
return True if self.psm.get_current_jaw_position() >= math.pi / 3 else False
def jaw_fully_closed(self):
return True if self.psm.get_current_jaw_position() <= 0 else False
def _open_jaw(self):
if self.psm.get_desired_jaw_position() < math.pi / 3:
self.psm.open_jaw(blocking=False)
def _open_jaw_next(self):
# open_jaw() sets jaw to 80 deg, we check if we're open past 60 deg
if self.psm.get_current_jaw_position() < math.pi / 3:
return PickAndPlaceState.OPEN_JAW
else:
return PickAndPlaceState.APPROACH_OBJECT
def _approach_object(self):
if self.log_verbose:
loginfo("Picking Object {}".format(self.object.pos))
self._set_arm_dest(self._obj_pos())
def _approach_object_next(self):
if self.psm._arm__goal_reached and \
vector_eps_eq(self.psm.get_current_position().p, self._obj_pos()):
return PickAndPlaceState.GRAB_OBJECT
else:
return PickAndPlaceState.APPROACH_OBJECT
def _grab_object(self):
self._set_arm_dest(self._obj_pos() + self._approach_vec())
def _grab_object_next(self):
if self.psm._arm__goal_reached and \
vector_eps_eq(self.psm.get_current_position().p, self._obj_pos() + self._approach_vec()):
return PickAndPlaceState.CLOSE_JAW
else:
return PickAndPlaceState.GRAB_OBJECT
def _close_jaw(self):
if self.psm.get_desired_jaw_position() > 0:
self.psm.close_jaw(blocking=False)
def _close_jaw_next(self):
if self.psm.get_current_jaw_position() > 0:
return PickAndPlaceState.CLOSE_JAW
else:
return PickAndPlaceState.APPROACH_DEST
def _approach_dest(self):
self._set_arm_dest(self._obj_dest())
def _approach_dest_next(self):
if self.psm._arm__goal_reached and \
vector_eps_eq(self.psm.get_current_position().p, self._obj_dest()):
return PickAndPlaceState.DROP_OBJECT
else:
return PickAndPlaceState.APPROACH_DEST
def _drop_object(self):
if self.psm.get_desired_jaw_position() < math.pi / 3:
self.psm.open_jaw(blocking=False)
def _drop_object_next(self):
# open_jaw() sets jaw to 80 deg, we check if we're open past 60 deg
if self.psm.get_current_jaw_position() > math.pi / 3:
# early out if this is being controlled by the parent state machine
if not self.closed_loop:
if self.home_when_done:
return PickAndPlaceState.HOME
else:
return PickAndPlaceState.DONE
elif len(self.world.objects) > 0:
# there are objects left, find one and go to APPROACH_OBJECT
closest_object = None
if self.pick_closest_to_base_frame:
# closest object to base frame
closest_object = min(self.world.objects,
key=lambda obj : (self.world_to_psm_tf * obj.pos).Norm())
else:
# closest object to current position, only if we're running
closest_object = min(self.world.objects,
key=lambda obj : (self.world_to_psm_tf * obj.pos \
- self.psm.get_current_position().p).Norm())
self.object = closest_object
return PickAndPlaceState.APPROACH_OBJECT
else:
return PickAndPlaceState.HOME
else:
return PickAndPlaceState.DROP_OBJECT
def _home(self):
self._set_arm_dest(PSM_HOME_POS)
def _home_next(self):
# the home state is used for arm state machines that are completely
# finished executing as determined by the parent state machine
return PickAndPlaceState.HOME
def _obj_pos(self):
return self.world_to_psm_tf * self.object.pos
def _approach_vec(self):
return self.world_to_psm_tf.M * self.approach_vec
def _obj_dest(self):
return self.world_to_psm_tf * self.obj_dest
def _set_arm_dest(self, dest):
if self.log_verbose:
loginfo("Setting {} dest to {}".format(self.psm.name(), dest))
if self.psm.get_desired_position().p != dest:
self.psm.move(PyKDL.Frame(DOWN_JAW_ORIENTATION, dest), blocking=False)
def __init__(self, psm, world, world_to_psm_tf, object, approach_vec,
closed_loop=False, use_down_facing_jaw=True, home_when_done=False, pick_closest_to_base_frame=False,
log_verbose=False):
self.log_verbose = log_verbose
self.home_when_done = home_when_done
self.pick_closest_to_base_frame = pick_closest_to_base_frame
if self.log_verbose:
loginfo("PickAndPlaceStateMachine:__init__")
loginfo("psm: {}, world: {}, world_to_psm_tf: {}, object: {}".format(
psm.name(), world, world_to_psm_tf, object))
loginfo("home_when_done: {}".format(self.home_when_done))
self.state = PickAndPlaceState.OPEN_JAW
self.psm = psm
self.world = world
self.world_to_psm_tf = world_to_psm_tf
if object is not None:
self.object = object
else:
self.object = min(self.world.objects,
key=lambda obj : (self.world_to_psm_tf * obj.pos \
- self.psm.get_current_position().p).Norm())
# approching position to pick up
self.approach_vec = approach_vec
# if this is False, we don't check if we successfully picked up the object
# and go straight to the done state
self.closed_loop = closed_loop
self.obj_dest = world.bowl.pos + PyKDL.Vector(0, 0, 0.05) # extra height over the bowl
self.state_functions = {
PickAndPlaceState.OPEN_JAW : self._open_jaw,
PickAndPlaceState.APPROACH_OBJECT : self._approach_object,
PickAndPlaceState.GRAB_OBJECT : self._grab_object,
PickAndPlaceState.CLOSE_JAW : self._close_jaw,
PickAndPlaceState.APPROACH_DEST : self._approach_dest,
PickAndPlaceState.DROP_OBJECT : self._drop_object,
PickAndPlaceState.HOME : self._home,
}
self.next_functions = {
PickAndPlaceState.OPEN_JAW : self._open_jaw_next,
PickAndPlaceState.APPROACH_OBJECT : self._approach_object_next,
PickAndPlaceState.GRAB_OBJECT : self._grab_object_next,
PickAndPlaceState.CLOSE_JAW : self._close_jaw_next,
PickAndPlaceState.APPROACH_DEST : self._approach_dest_next,
PickAndPlaceState.DROP_OBJECT : self._drop_object_next,
PickAndPlaceState.HOME : self._home_next
}
def update_world(self, world):
self.world = world
def run_once(self):
if self.log_verbose:
loginfo("Running state {}".format(self.state))
if self.is_done():
return
# execute the current state
self.state_functions[self.state]()
self.state = self.next_functions[self.state]()
def is_done(self):
if self.home_when_done:
return self.state == PickAndPlaceState.HOME and vector_eps_eq(self.psm.get_current_position().p, PSM_HOME_POS)
else:
return self.state == PickAndPlaceState.DONE
def halt(self):
# this sets the desired joint position to the current joint position
self.psm.move(self.psm.get_current_position(), blocking=False)
def __str__(self):
return str(self.__dict__)
def __repr__(self):
return str(self.__dict__)