← Back to branch summary

~shadowrobot/sr-ros-interface/syntouch_release

« back to all changes in this revision

Viewing changes to sr_dremmeling_wall/src/wall_dremmeler.py

  • Committer: Ugo Cupcic
  • Date: 2011-12-12 08:20:10 UTC
  • mfrom: (321.1.1 shadow_robot)
  • Revision ID: ugo@shadowrobot.com-20111212082010-gpw15bf8mu9no9li
TSB demos added.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sr_dremmeling_wall/src/wall_dremmeler.py
 
1
#!/usr/bin/env python
 
2
#
 
3
# Copyright 2011 Shadow Robot Company Ltd.
 
4
#
 
5
# This program is free software: you can redistribute it and/or modify it
 
6
# under the terms of the GNU General Public License as published by the Free
 
7
# Software Foundation, either version 2 of the License, or (at your option)
 
8
# any later version.
 
9
#
 
10
# This program is distributed in the hope that it will be useful, but WITHOUT
 
11
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 
12
# FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 
13
# more details.
 
14
#
 
15
# You should have received a copy of the GNU General Public License along
 
16
# with this program.  If not, see <http://www.gnu.org/licenses/>.
 
17
#
 
18
 
 
19
import roslib; roslib.load_manifest('sr_dremmeling_wall')
 
20
import rospy
 
21
import actionlib
 
22
import tf
 
23
import time
 
24
import math
 
25
import denso_msgs.msg
 
26
from geometry_msgs.msg import Pose, Quaternion, PoseStamped, Point
 
27
from kinect_color_segmentation.srv import WallNormale, SurfaceToDremmel
 
28
from tf.transformations import quaternion_from_euler
 
29
from rospy.core import rospyerr
 
30
 
 
31
from interactive_markers.interactive_marker_server import *
 
32
from interactive_marker import InteractiveConnectorSelector
 
33
 
 
34
class WallDremmeler(object):
 
35
    """
 
36
    Controls the process of dremelling certain regions of a wall with a dremel mounted on a Denso arm
 
37
    
 
38
    When the user clicks on the interactive marker, the current sequence of 3D points to be drilled is obtained, as well
 
39
    as the normal to the surface of the wall.
 
40
    
 
41
    For every point, a pose for the tool is calculated, which is inside the wall, following the normal, to ensure that the tool 
 
42
    will remove the first layer of the wall.
 
43
    If consecutive points are farther than a set distance, an intermediate position which is above the surface of the wall is calculated,
 
44
    so that the positions between the two points are not drilled.
 
45
    
 
46
    The calculated poses are sent to the arm.
 
47
    
 
48
    The tool we are using must be configured in the Denso arm's tool menu, and selected in set_tooltip(x) in DensoArm::init() (denso_arm package)   
 
49
    """
 
50
 
 
51
    def __init__(self, ):
 
52
        """
 
53
        """
 
54
        rospy.init_node('sr_dremmeling_wall')
 
55
 
 
56
        self.trajectory_client = actionlib.SimpleActionClient('/denso_arm/trajectory', denso_msgs.msg.TrajectoryAction)
 
57
        #self.trajectory_client.wait_for_server()
 
58
 
 
59
        self.listener = tf.TransformListener()
 
60
        self.br = tf.TransformBroadcaster()
 
61
 
 
62
        self.surface_to_dremmel_server = rospy.ServiceProxy( '/kinect_segmentation/PointSequenceDetection/segment', SurfaceToDremmel)
 
63
        self.wall_orientation_server = rospy.ServiceProxy( '/kinect_segmentation/PointSequenceDetection/get_wall_normale', WallNormale)
 
64
 
 
65
        self.interactive_marker_server = InteractiveMarkerServer("dremmel_wall")
 
66
        self.interactive_markers = InteractiveConnectorSelector(["camera_link"], self.run, self.interactive_marker_server)
 
67
 
 
68
        #rospy.loginfo("go to initial pose 1")
 
69
        #self.go_to_initial_position()
 
70
        
 
71
        #time.sleep(5)
 
72
 
 
73
        rospy.loginfo("go to initial pose 2")        
 
74
        self.go_to_initial_position_2()
 
75
 
 
76
 
 
77
    def run(self, name):
 
78
        """
 
79
        """
 
80
        #First we get the segmented points.
 
81
        rospy.loginfo("segmenting the point cloud")
 
82
        segmented_points = []
 
83
        try:
 
84
            res = self.surface_to_dremmel_server()
 
85
            segmented_points = res.points
 
86
            rospy.loginfo("cloud length:" + str(len(segmented_points)))
 
87
        except:
 
88
            rospy.logerr("Couldn't segment the point cloud")
 
89
            return
 
90
 
 
91
        #Then we get the normal for the wall
 
92
        rospy.loginfo("Getting the wall normale")
 
93
        wall_normale = Quaternion()
 
94
        wall_link = ""
 
95
        try:
 
96
            res = self.wall_orientation_server()
 
97
            wall_normale = res.normale
 
98
            wall_link = res.frame_name
 
99
        except:
 
100
            rospy.logerr("Couldn't get the wall normale")
 
101
 
 
102
 
 
103
        wall_normale = self.rotate_normal(segmented_points, wall_normale, wall_link)
 
104
 
 
105
        #Substitution of the surface normal for the tooltip normal to test the dremmelling
 
106
        rospy.logerr("TODO: remove this. Using tooltip normal.")
 
107
        tooltip_pose = self.get_pose("/denso_arm/tooltip", wall_link)
 
108
        wall_normale = tooltip_pose.orientation
 
109
 
 
110
        #We build a list of poses to send to the hand.
 
111
        list_of_poses, list_of_speeds = self.build_poses( segmented_points, wall_normale, wall_link )
 
112
 
 
113
        #now we send this to the arm
 
114
        rospy.loginfo("Sending the list of poses to the arm")
 
115
        goal = denso_msgs.msg.TrajectoryGoal()
 
116
        goal.trajectory = list_of_poses
 
117
        goal.speed = list_of_speeds
 
118
 
 
119
        self.trajectory_client.send_goal( goal )
 
120
        self.trajectory_client.wait_for_result()
 
121
 
 
122
        rospy.loginfo( "Finished Dremmeling the surface: " + str( self.trajectory_client.get_result() ) )
 
123
 
 
124
        #move the arm out of the way
 
125
        self.go_to_initial_position_2()
 
126
 
 
127
    def rotate_normal(self, segmented_points, quaternion, rotation_link):
 
128
        pose = Pose()
 
129
        pose.position.x = segmented_points[0].x
 
130
        pose.position.y = segmented_points[0].y
 
131
        pose.position.z = segmented_points[0].z
 
132
        pose.orientation = quaternion
 
133
 
 
134
        normal_name = "/surface_normal"
 
135
 
 
136
        pose_rotated = Pose()
 
137
        #position is the same, we just want to rotate
 
138
        pose_rotated.position.x = 0.0
 
139
        pose_rotated.position.y = 0.0
 
140
        pose_rotated.position.z = 0.0
 
141
        rotation_tmp_array = tf.transformations.quaternion_from_euler(0.0, 0.0, 3.14159266)
 
142
        pose_rotated.orientation.x = rotation_tmp_array[0]
 
143
        pose_rotated.orientation.y = rotation_tmp_array[1]
 
144
        pose_rotated.orientation.z = rotation_tmp_array[2]
 
145
        pose_rotated.orientation.w = rotation_tmp_array[3]
 
146
 
 
147
        rotated_normal_name = "/surface_normal_rotated"
 
148
 
 
149
        rate = rospy.Rate(100.0)
 
150
        pose_rotated_base = None
 
151
        success = False
 
152
 
 
153
        for i in range(0,100):
 
154
            self.br.sendTransform((pose.position.x, pose.position.y, pose.position.z),
 
155
                                  (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w),
 
156
                                  rospy.Time.now(), normal_name, rotation_link)
 
157
 
 
158
            self.br.sendTransform((pose_rotated.position.x, pose_rotated.position.y, pose_rotated.position.z),
 
159
                                  (pose_rotated.orientation.x, pose_rotated.orientation.y, pose_rotated.orientation.z, pose_rotated.orientation.w),
 
160
                                  rospy.Time.now(), rotated_normal_name, normal_name)
 
161
            rate.sleep()
 
162
            try:
 
163
                (trans,rot) = self.listener.lookupTransform(rotation_link, rotated_normal_name, rospy.Time())
 
164
                pose_rotated_base = PoseStamped()
 
165
                pose_rotated_base.pose.position.x = trans[0]
 
166
                pose_rotated_base.pose.position.y = trans[1]
 
167
                pose_rotated_base.pose.position.z = trans[2]
 
168
                pose_rotated_base.pose.orientation.x = rot[0]
 
169
                pose_rotated_base.pose.orientation.y = rot[1]
 
170
                pose_rotated_base.pose.orientation.z = rot[2]
 
171
                pose_rotated_base.pose.orientation.w = rot[3]
 
172
                pose_rotated_base.header.frame_id = rotation_link
 
173
                pose_rotated_base.header.stamp = rospy.Time.now()
 
174
                success = True
 
175
                break
 
176
            except:
 
177
                pass
 
178
                #rospy.logerr("Could not transform from " + pose_above_pose_frame.header.frame_id + " to /base_link")
 
179
 
 
180
        if success == False:
 
181
            rospy.logerr("Could not transform from " + rotated_normal_name + " to " + rotation_link)
 
182
 
 
183
        return pose_rotated_base.pose.orientation
 
184
 
 
185
    def calculate_distance(self, point1, point2):
 
186
        distance = math.sqrt((point1.x - point2.x) * (point1.x - point2.x)
 
187
                             + (point1.y - point2.y) * (point1.y - point2.y)
 
188
                             + (point1.z - point2.z) * (point1.z - point2.z))
 
189
        return distance
 
190
 
 
191
 
 
192
    def get_pose(self, link_name, base_link):
 
193
        trans = None
 
194
        rot = None
 
195
 
 
196
        #try to get the pose of the given link_name
 
197
        # in the base_link frame.
 
198
        for i in range (0, 500):
 
199
            try:
 
200
                (trans, rot) = self.listener.lookupTransform( base_link, link_name,
 
201
                                                              rospy.Time(0) )
 
202
                break
 
203
            except Exception, e:
 
204
                time.sleep(0.01)
 
205
                continue
 
206
 
 
207
        pose = None
 
208
        pose = Pose()
 
209
        pose.position.x = trans[0]
 
210
        pose.position.y = trans[1]
 
211
        pose.position.z = trans[2]
 
212
        pose.orientation.x = rot[0]
 
213
        pose.orientation.y = rot[1]
 
214
        pose.orientation.z = rot[2]
 
215
        pose.orientation.w = rot[3]
 
216
 
 
217
        return pose
 
218
 
 
219
    def build_poses(self, segmented_points, quaternion, rotation_link):
 
220
        list_of_poses = []
 
221
        list_of_speeds = []
 
222
        previous_point = None
 
223
        last_orientation_in_base_link = None
 
224
 
 
225
        for index,point in enumerate(segmented_points):
 
226
            #create a pose above the point (in the wall frame)
 
227
            rospy.loginfo("Doing point: " + str(index))
 
228
 
 
229
            send_pose_above = False
 
230
 
 
231
            #Create a pose above the surface only if it's the first time, or the point is at more than x meters from the previous
 
232
            if (previous_point == None):
 
233
                previous_point = Point()
 
234
                send_pose_above = True
 
235
            elif (self.calculate_distance(previous_point, point) > 0.035):
 
236
                send_pose_above = True
 
237
            else:
 
238
                send_pose_above = False
 
239
 
 
240
            #Save current point as previous
 
241
            previous_point.x = point.x
 
242
            previous_point.y = point.y
 
243
            previous_point.z = point.z
 
244
            
 
245
            pose_on_plane = Pose()
 
246
            pose_on_plane.position.x = point.x
 
247
            pose_on_plane.position.y = point.y
 
248
            pose_on_plane.position.z = point.z
 
249
            pose_on_plane.orientation = quaternion
 
250
            pose_on_plane_name = "/pose_on_plane_" + str(index)
 
251
 
 
252
            if send_pose_above == True:
 
253
                pose_above = Pose()
 
254
                pose_above.position.x = 0.0
 
255
                pose_above.position.y = 0.0
 
256
                pose_above.position.z = -0.05
 
257
                pose_above.orientation = Quaternion(0,0,0,1)
 
258
                pose_above_name = "/pose_above_" + str(index)
 
259
 
 
260
            #then create a pose below the point (inside the wall)
 
261
            pose_inside = Pose()
 
262
            pose_inside.position.x = -0.025
 
263
            pose_inside.position.y = 0.02
 
264
            pose_inside.position.z = 0.042
 
265
            pose_inside.orientation = Quaternion(0,0,0,1)
 
266
            pose_inside_name = "/pose_inside_"  + str(index)
 
267
 
 
268
            #then transform those two poses in the base_link frame
 
269
            #print "TODO: transform from ", rotation_link, " to /base_link"
 
270
 
 
271
 
 
272
            pose_above_base_frame = None
 
273
            pose_inside_base_frame = None
 
274
            success = False
 
275
            rate = rospy.Rate(100.0)
 
276
 
 
277
            if send_pose_above == True:
 
278
                for i in range(0,100):
 
279
                    self.br.sendTransform((pose_on_plane.position.x, pose_on_plane.position.y, pose_on_plane.position.z),
 
280
                                          (pose_on_plane.orientation.x, pose_on_plane.orientation.y, pose_on_plane.orientation.z, pose_on_plane.orientation.w),
 
281
                                          rospy.Time.now(), pose_on_plane_name, rotation_link)
 
282
                    rate.sleep()
 
283
                    self.br.sendTransform((pose_above.position.x, pose_above.position.y, pose_above.position.z),
 
284
                                          (pose_above.orientation.x, pose_above.orientation.y, pose_above.orientation.z, pose_above.orientation.w),
 
285
                                          rospy.Time.now(), pose_above_name, pose_on_plane_name)
 
286
                    rate.sleep()
 
287
                    try:
 
288
                        (trans,rot) = self.listener.lookupTransform("/base_link", pose_above_name, rospy.Time())
 
289
                        pose_above_base_frame = PoseStamped()
 
290
                        pose_above_base_frame.pose.position.x = trans[0]
 
291
                        pose_above_base_frame.pose.position.y = trans[1]
 
292
                        pose_above_base_frame.pose.position.z = trans[2]
 
293
                        pose_above_base_frame.pose.orientation.x = rot[0]
 
294
                        pose_above_base_frame.pose.orientation.y = rot[1]
 
295
                        pose_above_base_frame.pose.orientation.z = rot[2]
 
296
                        pose_above_base_frame.pose.orientation.w = rot[3]
 
297
                        pose_above_base_frame.header.frame_id = "/base_link"
 
298
                        pose_above_base_frame.header.stamp = rospy.Time.now()
 
299
                        success = True
 
300
                        break
 
301
                    except:
 
302
                        pass
 
303
                        #rospy.logerr("Could not transform from " + pose_above_pose_frame.header.frame_id + " to /base_link")
 
304
 
 
305
 
 
306
                if success == False:
 
307
                    rospy.logerr("Could not transform from " + pose_above_name + " to /base_link")
 
308
 
 
309
            success = False
 
310
            for i in range(0,100):
 
311
                self.br.sendTransform((pose_on_plane.position.x, pose_on_plane.position.y, pose_on_plane.position.z),
 
312
                                      (pose_on_plane.orientation.x, pose_on_plane.orientation.y, pose_on_plane.orientation.z, pose_on_plane.orientation.w),
 
313
                                      rospy.Time.now(), pose_on_plane_name, rotation_link)
 
314
                rate.sleep()
 
315
                self.br.sendTransform((pose_inside.position.x, pose_inside.position.y, pose_inside.position.z),
 
316
                                      (pose_inside.orientation.x, pose_inside.orientation.y, pose_inside.orientation.z, pose_inside.orientation.w),
 
317
                                      rospy.Time.now(), pose_inside_name, pose_on_plane_name)
 
318
                rate.sleep()
 
319
 
 
320
                try:
 
321
                    (trans,rot) = self.listener.lookupTransform("/base_link", pose_inside_name, rospy.Time())
 
322
                    pose_inside_base_frame = PoseStamped()
 
323
                    pose_inside_base_frame.pose.position.x = trans[0]
 
324
                    pose_inside_base_frame.pose.position.y = trans[1]
 
325
                    pose_inside_base_frame.pose.position.z = trans[2]
 
326
                    pose_inside_base_frame.pose.orientation.x = rot[0]
 
327
                    pose_inside_base_frame.pose.orientation.y = rot[1]
 
328
                    pose_inside_base_frame.pose.orientation.z = rot[2]
 
329
                    pose_inside_base_frame.pose.orientation.w = rot[3]
 
330
                    pose_inside_base_frame.header.frame_id = "/base_link"
 
331
                    pose_inside_base_frame.header.stamp = rospy.Time.now()
 
332
                    success = True
 
333
 
 
334
                    #Copy orientaion
 
335
                    last_orientation_in_base_link = pose_inside_base_frame.pose.orientation
 
336
                    break
 
337
                except:
 
338
                    pass
 
339
                    #rospy.logerr("Could not transform from " + pose_inside_pose_frame.header.frame_id + " to /base_link")
 
340
                rate.sleep()
 
341
 
 
342
            if success == False:
 
343
                rospy.logerr("Could not transform from " + pose_inside_name + " to /base_link")
 
344
 
 
345
 
 
346
            #add those two poses to the list of poses sent to the arm
 
347
            if send_pose_above == True:
 
348
                if (pose_above_base_frame != None):
 
349
                    list_of_poses.append( pose_above_base_frame.pose )
 
350
                    list_of_speeds.append( 30.0 )
 
351
                else:
 
352
                    rospy.logerr("Could not convert pose above " + pose_above_name + " to /base_link")
 
353
 
 
354
            if (pose_inside_base_frame != None):
 
355
                list_of_poses.append( pose_inside_base_frame.pose )
 
356
                list_of_speeds.append( 20.0 )
 
357
            else:
 
358
                rospy.logerr("Could not convert pose inside " + pose_inside_name + " to /base_link")
 
359
 
 
360
        #Enqueue a last position far from the target
 
361
        pose_out = Pose()
 
362
        pose_out.position.x = 0.644
 
363
        pose_out.position.y = 0.279
 
364
        pose_out.position.z = 0.581
 
365
        pose_out.orientation = last_orientation_in_base_link
 
366
        list_of_poses.append( pose_out )
 
367
        list_of_speeds.append( 25.0 )
 
368
 
 
369
        return list_of_poses, list_of_speeds
 
370
 
 
371
    def go_to_initial_position(self):
 
372
        #Move the arm to a fixed initial position
 
373
        goal = denso_msgs.msg.TrajectoryGoal()
 
374
        traj = []
 
375
        speed = []
 
376
        pose_tmp = Pose()
 
377
        pose_tmp.position.x = 0.639
 
378
        pose_tmp.position.y = -0.005
 
379
        pose_tmp.position.z = 0.356
 
380
        pose_tmp.orientation.x = -0.659
 
381
        pose_tmp.orientation.y = 0.256
 
382
        pose_tmp.orientation.z = -0.2466
 
383
        pose_tmp.orientation.w = 0.6626
 
384
 
 
385
        traj.append( pose_tmp )
 
386
        speed.append( 15. )
 
387
 
 
388
        goal.trajectory = traj
 
389
        goal.speed = speed
 
390
 
 
391
        self.trajectory_client.send_goal( goal )
 
392
        self.trajectory_client.wait_for_result()
 
393
        res =  self.trajectory_client.get_result()
 
394
        if res.val != denso_msgs.msg.TrajectoryResult.SUCCESS:
 
395
            rospy.logerr("Failed to move the arm to the given position.")
 
396
            return False
 
397
 
 
398
        return True
 
399
 
 
400
    def go_to_initial_position_2(self):
 
401
        #Move the arm to a fixed initial position
 
402
        goal = denso_msgs.msg.TrajectoryGoal()
 
403
        traj = []
 
404
        speed = []
 
405
        pose_tmp = Pose()
 
406
        pose_tmp.position.x = 0.61
 
407
        pose_tmp.position.y = 0.036
 
408
        pose_tmp.position.z = 0.669
 
409
        pose_tmp.orientation.x = -0.653
 
410
        pose_tmp.orientation.y = 0.271
 
411
        pose_tmp.orientation.z = -0.261
 
412
        pose_tmp.orientation.w = 0.657
 
413
 
 
414
        #pose_tmp.orientation.x = -0.659
 
415
        #pose_tmp.orientation.y = 0.256
 
416
        #pose_tmp.orientation.z = -0.2466
 
417
        #pose_tmp.orientation.w = 0.6626
 
418
 
 
419
 
 
420
        traj.append( pose_tmp )
 
421
        speed.append( 35. )
 
422
 
 
423
        goal.trajectory = traj
 
424
        goal.speed = speed
 
425
 
 
426
        self.trajectory_client.send_goal( goal )
 
427
        self.trajectory_client.wait_for_result()
 
428
        res =  self.trajectory_client.get_result()
 
429
        if res.val != denso_msgs.msg.TrajectoryResult.SUCCESS:
 
430
            rospy.logerr("Failed to move the arm to the given position.")
 
431
            return False
 
432
 
 
433
        return True