forked from CURG-archive/trajectory_planner
-
Notifications
You must be signed in to change notification settings - Fork 0
/
grasp_message_robot_server.py
executable file
·244 lines (196 loc) · 9.96 KB
/
grasp_message_robot_server.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
242
243
#!/usr/bin/env python
import roslib
roslib.load_manifest( "trajectory_planner" )
import rospy
import time
import graspit_msgs.msg
import geometry_msgs.msg
import tf, tf_conversions, tf.transformations
from numpy import pi, eye, dot, cross, linalg, sqrt, ceil, size
from numpy import hstack, vstack, mat, array, arange, fabs
import tf_conversions.posemath as pm
from time import sleep
import trajectory_planner as tp
import pdb
from object_filename_dict import file_name_dict
from std_msgs.msg import String, Empty
#from IPython.Shell import IPShellEmbed
from adjust_message_robot_server import AdjustExecutor, ShakeExecutor, MoveExecutor
from time import time
from model_rec_manager import *
import HaoUtil as hu
Hao = False
class GraspExecutor():
"""@brief - Generates a persistent converter between grasps published by a graspit commander and
the trajectory planning module to actually lift objects off the table
@member grasp_listener - subscriber to the graspit grasp channel. Expects graspit_msgs Grasp types
@member name_listener - subscribes to an object name channel. Expects a string which corresponds
to an entry in the filename dictionary
@member global_data - Planning environment
@member target_object_name - the current grasp target
"""
def __init__(self, init_planner = True):
self.grasp_listener = rospy.Subscriber("/graspit/grasps", graspit_msgs.msg.Grasp, self.process_grasp_msg)
self.name_listener = rospy.Subscriber("/graspit/target_name", String, self.process_object_name)
self.refresh_models_listener = rospy.Subscriber("/graspit/refresh_models", Empty, self.refresh_model_list)
self.global_data = tp.SetupStaubliEnv(True, init_planner)
self.model_manager = ModelRecManager(self.global_data.listener)
self.graspit_status_publisher = rospy.Publisher("/graspit/status", graspit_msgs.msg.GraspStatus)
self.graspit_target_publisher = rospy.Publisher("/graspit/target_name", String)
self.graspit_object_publisher = rospy.Publisher("/graspit/object_name", graspit_msgs.msg.ObjectInfo)
self.remove_object_publisher = rospy.Publisher("/graspit/remove_objects", String)
self.target_object_name = "flask"
self.last_grasp_time = 0
self.table_cube=[geometry_msgs.msg.Point(-0.7,0,-0.02), geometry_msgs.msg.Point(0.2,1,1)]
def process_object_name(self, string):
self.target_object_name = string.data
def refresh_model_list(self, empty_msg):
self.model_manager.refresh()
self.model_manager()
#self.publish_target()
self.remove_object_publisher.publish('ALL')
self.publish_table_models()
self.remove_all_objects_from_planner()
self.add_all_objects_to_planner()
def publish_target(self):
self.graspit_target_publisher.publish(self.model_manager.get_model_names()[0])
def publish_table_models(self):
self.model_manager()
table_models = [model for model in self.model_manager.model_list if self.point_within_table_cube(model.get_world_pose().position)]
print '\n'.join(['Model rejected %s'%(model.object_name) for model in self.model_manager.model_list if model not in table_models])
for model in table_models:
model()
object_name = "%s %s"%(model.model_name, model.object_name)
print "Sending object: %s \n"%(object_name)
p = model.get_world_pose()
print "Model name: %s"%(model.object_name)
print p
self.graspit_object_publisher.publish(object_name, p)
self.graspit_status_publisher.publish(0, '')
def clear_objects(self):
model_name_list = []
model_name_list = [model.object_name for model in self.model_manager.model_list]
self.remove_objects_publisher.publish(' '.join(model_name_list))
def add_object_to_planner(self, model):
tp.add_object_to_planner(self.global_data, model.object_name, file_name_dict[model.model_name])
def add_all_objects_to_planner(self):
for model in self.model_manager.model_list:
self.add_object_to_planner(model)
def remove_all_objects_from_planner(self):
body_list = self.global_data.or_env.GetBodies()
ignored_body_list = ['StaubliRobot', 'table']
for body in body_list:
if body.GetName() not in ignored_body_list:
self.global_data.or_env.Remove(body)
del body
def process_grasp_msg(self, grasp_msg):
"""@brief - Attempt to grasp the object and lift it
First moves the arm to pregrasp the object, then attempts to grasp and lift it.
The grasp and lifting phase is currently completely open loop
"""
if (time() - self.last_grasp_time) < 30:
return [], []
self.last_grasp_time = time()
print grasp_msg
grasp_status = graspit_msgs.msg.GraspStatus.SUCCESS
grasp_status_msg = "grasp_succeeded"
success = 1
if not tp.is_home():
print 'go home'
tp.go_home(self.global_data)
# if not success:
# grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
# grasp_status_msg = "Unable to go home"
if success:
success, grasp_status_msg, positions = tp.open_barrett()
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR
if success:
grasp_tran = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
grasp_tran[0:3,3] /=1000 #mm to meters
tp.MoveHandSrv(1, [0,0,0, grasp_msg.pre_grasp_dof[0]])
tp.update_robot(self.global_data.or_env.GetRobots()[0])
print 'pre-grasp'
self.model_manager()
# success, final_tran, dof_list, j = tp.pregrasp_object(self.global_data, file_name_dict[self.target_object_name], grasp_tran)
print 'after model_manager()'
success, final_tran, dof_list, j = tp.pregrasp_object(self.global_data, self.target_object_name, False, grasp_tran)
print 'after pre-grasp'
#raw_input("Press enter...")
tp.update_robot(self.global_data.or_env.GetRobots()[0])
if not success:
if not j:
grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
grasp_status_msg = "Pregrasp tran Out of range!"
else:
grasp_status = graspit_msgs.msg.GraspStatus.FAILED
grasp_status_msg = "Unknown planner failure!"
if success:
if not Hao:
success = tp.move_forward(0.05, True)
else:
hu.GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20)
success = True
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
grasp_status_msg = "Unable to move forward to grasp!"
if Hao:
"""new routine"""
hu.GuardedCloseHand(self.global_data)
else:
"""old routine"""
if success:
success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.pre_grasp_dof[1],grasp_msg.pre_grasp_dof[2], grasp_msg.pre_grasp_dof[3], grasp_msg.pre_grasp_dof[0]])
if success:
success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.final_grasp_dof[1],grasp_msg.final_grasp_dof[2], grasp_msg.final_grasp_dof[3], grasp_msg.final_grasp_dof[0]])
if success:
success, grasp_status_msg, joint_angles = tp.close_barrett()
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR
if success:
selection = int(raw_input('Lift up (1) or not (0): '))
if selection == 1:
print 'lift up the object'
success = tp.lift_arm(.05, True)
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
grasp_status_msg = "Couldn't lift object"
else:
print 'not lift up the object'
#Maybe decide if it has been successfully lifted here...
if success:
rospy.logwarn(grasp_status_msg)
else:
rospy.logfatal(grasp_status_msg)
self.graspit_status_publisher.publish(grasp_status, grasp_status_msg)
print grasp_status_msg
return grasp_status, grasp_status_msg
def point_within_table_cube(self, test_point):
[min_corner_point , max_corner_point ] = self.table_cube
keys = ['x', 'y', 'z']
for k in keys:
t = getattr(test_point, k)
min_test = getattr(min_corner_point, k)
max_test = getattr(max_corner_point, k)
if t < min_test or t > max_test:
print 'Failed to be inside table in key %s - min - %f max - %f value %f'%(k, min_test, max_test, t)
return False
return True
if __name__ == '__main__':
try:
rospy.init_node('graspit_message_robot_server')
init_planner = rospy.get_param('init_planner', True)
print "init planner value %d \n"%(init_planner)
ge = GraspExecutor(init_planner = init_planner)
if Hao:
ae = AdjustExecutor(ge.global_data)
se = ShakeExecutor(ge.global_data)
me = MoveExecutor(ge.global_data)
loop = rospy.Rate(10)
# ipshell = IPShellEmbed(banner = 'Dropping into IPython',
# exit_msg = 'Leaving Interpreter, back to program.')
# ipshell(local_ns = locals())
while not rospy.is_shutdown():
ge.model_manager.rebroadcast_object_tfs()
loop.sleep()
except rospy.ROSInterruptException: pass