Skip to content

Commit eb91130

Browse files
zhenyan2001zhenyan2001
committed
fix behaviour
1 parent 3901413 commit eb91130

File tree

3 files changed

+61
-46
lines changed

3 files changed

+61
-46
lines changed

‎caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/Gripper_whole_process_sm.py‎

Lines changed: 58 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -45,8 +45,9 @@
4545
"""
4646

4747

48+
from caro_skills_flexbe_states.Calibrate_state import CalibratetoOrigin
4849
from caro_skills_flexbe_states.gripper_gigatino_state import GripperState
49-
from caro_skills_flexbe_states.home_state import Home
50+
from caro_skills_flexbe_states.home_state import BackToOrigin
5051
from caro_skills_flexbe_states.move_gripper_state import GripperMove
5152
from caro_skills_flexbe_states.move_gripper_up_state import GripperMoveUp
5253
from flexbe_core import Autonomy
@@ -95,15 +96,15 @@ def __init__(self, node):
9596
def create(self):
9697
"""Create state machine."""
9798
# Root state machine
98-
# x:379 y:654, x:130 y:400
99+
# x:909 y:591, x:130 y:400
99100
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
100101
_state_machine.userdata.x = 0.15
101102
_state_machine.userdata.y = 0.08
102103
_state_machine.userdata.z = 0.03
103104
_state_machine.userdata.z_offset = 0.05
104105
_state_machine.userdata.gripper_state = False
105106
_state_machine.userdata.use_gripper = False
106-
_state_machine.userdata.target_frame = 'robotinobase2/end_effector_home'
107+
_state_machine.userdata.target_frame = 'robotinobase1/end_effector_home'
107108
_state_machine.userdata.relative = False
108109
_state_machine.userdata.open = False
109110

@@ -114,14 +115,28 @@ def create(self):
114115
# [/MANUAL_CREATE]
115116

116117
with _state_machine:
117-
# x:30 y:40
118-
OperatableStateMachine.add('move above',
119-
GripperMoveUp(timeout=5,
120-
action_topic='robotinobase2/gigatino/move'),
121-
transitions={'reached': 'move down' # 238 72 -1 -1 -1 -1
122-
, 'failed': 'failed' # 102 274 -1 -1 -1 -1
123-
, 'canceled': 'failed' # 102 274 -1 -1 -1 -1
124-
, 'timeout': 'failed' # 102 274 -1 -1 -1 -1
118+
# x:105 y:42
119+
OperatableStateMachine.add('calibrate',
120+
CalibratetoOrigin(timeout=10,
121+
action_topic='robotinobase1/gigatino/calibrate'),
122+
transitions={'pose_reached': 'move_above' # 314 98 -1 -1 -1 -1
123+
, 'failed': 'failed' # 143 272 -1 -1 -1 -1
124+
, 'canceled': 'failed' # 143 272 -1 -1 -1 -1
125+
, 'timeout': 'failed' # 143 272 -1 -1 -1 -1
126+
},
127+
autonomy={'pose_reached': Autonomy.Off,
128+
'failed': Autonomy.Off,
129+
'canceled': Autonomy.Off,
130+
'timeout': Autonomy.Off})
131+
132+
# x:616 y:51
133+
OperatableStateMachine.add('down',
134+
GripperMove(timeout=10,
135+
action_topic='robotinobase1/gigatino/move'),
136+
transitions={'reached': 'grip' # 782 129 -1 -1 -1 -1
137+
, 'failed': 'failed' # 380 255 -1 -1 -1 -1
138+
, 'canceled': 'failed' # 380 255 -1 -1 -1 -1
139+
, 'timeout': 'failed' # 380 255 -1 -1 -1 -1
125140
},
126141
autonomy={'reached': Autonomy.Off,
127142
'failed': Autonomy.Off,
@@ -133,46 +148,45 @@ def create(self):
133148
'y': 'y',
134149
'z': 'z',
135150
'gripper_state': 'gripper_state',
136-
'use_gripper': 'use_gripper',
137-
'z_offset': 'z_offset'})
151+
'use_gripper': 'use_gripper'})
138152

139-
# x:407 y:237
153+
# x:797 y:122
140154
OperatableStateMachine.add('grip',
141-
GripperState(timeout=3000,
155+
GripperState(timeout=10,
142156
action_topic='robotinobase2/gigatino/gripper'),
143-
transitions={'success': 'move up' # 566 312 -1 -1 -1 -1
144-
, 'failed': 'failed' # 274 333 -1 -1 -1 -1
145-
, 'canceled': 'failed' # 274 333 -1 -1 -1 -1
146-
, 'timeout': 'failed' # 274 333 -1 -1 -1 -1
157+
transitions={'success': 'up' # 889 272 -1 -1 -1 -1
158+
, 'failed': 'failed' # 467 287 -1 -1 -1 -1
159+
, 'canceled': 'failed' # 467 287 -1 -1 -1 -1
160+
, 'timeout': 'failed' # 467 287 -1 -1 -1 -1
147161
},
148162
autonomy={'success': Autonomy.Off,
149163
'failed': Autonomy.Off,
150164
'canceled': Autonomy.Off,
151165
'timeout': Autonomy.Off},
152166
remapping={'open': 'open'})
153167

154-
# x:447 y:456
168+
# x:860 y:430
155169
OperatableStateMachine.add('home',
156-
Home(timeout=1000,
157-
action_topic='robotinobase2/gigatino/calibrate'),
158-
transitions={'pose_reached': 'finished' # 416 597 -1 -1 -1 -1
159-
, 'failed': 'failed' # 291 455 -1 -1 -1 -1
160-
, 'canceled': 'failed' # 291 455 -1 -1 -1 -1
161-
, 'timeout': 'failed' # 291 455 -1 -1 -1 -1
170+
BackToOrigin(timeout=10,
171+
action_topic='robotinobase1/gigatino/home'),
172+
transitions={'pose_reached': 'finished' # 931 568 -1 -1 -1 -1
173+
, 'failed': 'failed' # 496 445 -1 -1 -1 -1
174+
, 'canceled': 'failed' # 496 445 -1 -1 -1 -1
175+
, 'timeout': 'failed' # 496 445 -1 -1 -1 -1
162176
},
163177
autonomy={'pose_reached': Autonomy.Off,
164178
'failed': Autonomy.Off,
165179
'canceled': Autonomy.Off,
166180
'timeout': Autonomy.Off})
167181

168-
# x:258 y:62
169-
OperatableStateMachine.add('move down',
170-
GripperMove(timeout=3000,
171-
action_topic='robotinobase2/gigatino/move'),
172-
transitions={'reached': 'grip' # 438 175 -1 -1 -1 -1
173-
, 'failed': 'failed' # 197 261 -1 -1 -1 -1
174-
, 'canceled': 'failed' # 197 261 -1 -1 -1 -1
175-
, 'timeout': 'failed' # 197 261 -1 -1 -1 -1
182+
# x:342 y:72
183+
OperatableStateMachine.add('move_above',
184+
GripperMoveUp(timeout=10,
185+
action_topic='robotinobase1/gigatino/move'),
186+
transitions={'reached': 'down' # 563 91 -1 -1 -1 -1
187+
, 'failed': 'failed' # 240 262 -1 -1 -1 -1
188+
, 'canceled': 'failed' # 240 262 -1 -1 -1 -1
189+
, 'timeout': 'failed' # 240 262 -1 -1 -1 -1
176190
},
177191
autonomy={'reached': Autonomy.Off,
178192
'failed': Autonomy.Off,
@@ -184,16 +198,17 @@ def create(self):
184198
'y': 'y',
185199
'z': 'z',
186200
'gripper_state': 'gripper_state',
187-
'use_gripper': 'use_gripper'})
201+
'use_gripper': 'use_gripper',
202+
'z_offset': 'z_offset'})
188203

189-
# x:503 y:349
190-
OperatableStateMachine.add('move up',
191-
GripperMoveUp(timeout=500,
192-
action_topic='robotinobase2/gigatino/move'),
193-
transitions={'reached': 'home' # 458 433 -1 -1 -1 -1
194-
, 'failed': 'failed' # 320 400 -1 -1 -1 -1
195-
, 'canceled': 'failed' # 320 400 -1 -1 -1 -1
196-
, 'timeout': 'failed' # 320 400 -1 -1 -1 -1
204+
# x:883 y:288
205+
OperatableStateMachine.add('up',
206+
GripperMoveUp(timeout=10,
207+
action_topic='robotinobase1/gigatino/move'),
208+
transitions={'reached': 'home' # 940 428 -1 -1 -1 -1
209+
, 'failed': 'failed' # 509 374 -1 -1 -1 -1
210+
, 'canceled': 'failed' # 509 374 -1 -1 -1 -1
211+
, 'timeout': 'failed' # 509 374 -1 -1 -1 -1
197212
},
198213
autonomy={'reached': Autonomy.Off,
199214
'failed': Autonomy.Off,

‎caro_skills_flexbe_states/caro_skills_flexbe_states/gripper_gigatino_state.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@ class GripperState(EventState):
2727
2828
Parameters
2929
-- timeout Maximum time allowed (seconds)
30-
-- action_topic Name of action to invoke
30+
-- action_topic Name of action to invoke example ='robotinobase2/gigatino/gripper'
3131
3232
Outputs
3333
<= success congrats we got the workpiece.
@@ -40,7 +40,7 @@ class GripperState(EventState):
4040
4141
"""
4242

43-
def __init__(self, timeout, action_topic='robotinobase2/gigatino/gripper'):
43+
def __init__(self, timeout, action_topic):
4444
# See example_state.py for basic explanations.
4545
super().__init__(outcomes=['success', 'failed', 'canceled', 'timeout'],
4646
input_keys=['open'], output_keys=[])

‎caro_skills_flexbe_states/caro_skills_flexbe_states/home_state.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ class BackToOrigin(EventState):
3333
<= timeout The action has timed out.
3434
3535
"""
36-
def __init__(self, timeout,action_topic='robotinobase2/gigatino/home'):
36+
def __init__(self, timeout,action_topic):
3737

3838
super().__init__(outcomes=['pose_reached', 'failed', 'canceled', 'timeout'],
3939
output_keys=[])

0 commit comments

Comments
 (0)