def execute(self, userdata): sss.set_light("blue") wi = WorldInterface() wi.reset_attached_objects() # add floor floor_extent = [3.0, 3.0, 0.1] floor_pose = conversions.create_pose_stamped( [0, 0, floor_extent[2] / 2.0, 0, 0, 0, 1], "/base_link" ) # magic numbers are cool wi.add_collision_box(floor_pose, floor_extent, "floor") if not self.mp.plan(5).success: sss.set_light("green") return "not_succeeded" sss.set_light("yellow") # run, handle errors i = 0 for ex in self.mp.execute(): if not ex.wait(80.0).success: sss.set_light("red") return "failed" i += 1 sss.set_light("green") return "succeeded"
def execute(self, userdata): sss.set_light('blue') wi = WorldInterface() wi.reset_attached_objects() # add floor floor_extent = [3.0, 3.0, 0.1] floor_pose = conversions.create_pose_stamped( [0, 0, floor_extent[2] / 2.0, 0, 0, 0, 1], '/base_link') # magic numbers are cool wi.add_collision_box(floor_pose, floor_extent, "floor") if not self.mp.plan(5).success: sss.set_light('green') return "not_succeeded" sss.set_light('yellow') # run, handle errors i = 0 for ex in self.mp.execute(): if not ex.wait(80.0).success: sss.set_light('red') return 'failed' i += 1 sss.set_light('green') return 'succeeded'
def execute(self, userdata, overwrite_presets=dict()): presets = self.default_presets presets.update(overwrite_presets) sss.set_light('blue') wi = WorldInterface() wi.reset_attached_objects() graspdata = dict() print userdata.object print "time difference = ", ( rospy.Time.now() - userdata.object.pose.header.stamp).to_sec() # add wall wall_extent = [3.0, 0.1, 2.5] wall_pose = conversions.create_pose_stamped( [0, -0.99 - wall_extent[1], wall_extent[2] / 2.0, 0, 0, 0, 1], '/base_link') # magic numbers are cool wi.add_collision_box(wall_pose, wall_extent, "wall") # add floor floor_extent = [3.0, 3.0, 0.1] floor_pose = conversions.create_pose_stamped( [0, 0, floor_extent[2] / 2.0, 0, 0, 0, 1], '/base_link') # magic numbers are cool wi.add_collision_box(floor_pose, floor_extent, "floor") #transform into base_link grasp_pose = transform_listener.transform_pose_stamped( '/base_link', userdata.object.pose, use_most_recent=False) print grasp_pose # add object bounding box obj_pose = deepcopy(userdata.object.pose) lwh = userdata.object.bounding_box_lwh m1 = pm.toMatrix(pm.fromMsg(obj_pose.pose)) m2 = pm.toMatrix(pm.fromTf(((0, 0, lwh.z / 2.0), (0, 0, 0, 1)))) obj_pose.pose = pm.toMsg(pm.fromMatrix(numpy.dot(m1, m2))) wi.add_collision_box(obj_pose, (lwh.x * 2.0, lwh.y * 2.0, lwh.z), "grasp_object") # compute minimal height of object bounding box edges in base_link min_z = grasp_pose.pose.position.z for k in [(0.5, 0.5, 1.0), (0.5, 0.5, 0.0), (0.5, -0.5, 1.0), (0.5, -0.5, 0.0), (-0.5, 0.5, 1.0), (-0.5, 0.5, 0.0), (-0.5, -0.5, 1.0), (-0.5, -0.5, 0.0)]: m1 = pm.toMatrix(pm.fromMsg(grasp_pose.pose)) # BFromO m2 = pm.toMatrix( pm.fromTf(((k[0] * lwh.x, k[1] * lwh.y, k[2] * lwh.z), (0, 0, 0, 1)))) # inO min_z = min(min_z, pm.toMsg(pm.fromMatrix(numpy.dot( m1, m2))).position.z) #min_z inB # add table table_extent = (2.0, 2.0, min_z) # base_link # x - points towards front of robot # y - follow right hand rule # z - points upwards table_pose = conversions.create_pose_stamped([ -0.5 - table_extent[0] / 2.0, 0, table_extent[2] / 2.0, 0, 0, 0, 1 ], '/base_link') # center of table, 0.5 meter behind the robot wi.add_collision_box(table_pose, table_extent, "table") # calculate grasp and lift pose grasp_pose.pose.position.x += presets['grasp_offset'][0] grasp_pose.pose.position.y += presets['grasp_offset'][1] grasp_pose.pose.position.z += presets['grasp_offset'][2] grasp_pose.pose.orientation = Quaternion( *quaternion_from_euler(*presets['fixed_orientation_euler']) ) # orientation of sdh_grasp_link in base_link for 'grasp' joint goal graspdata['height'] = grasp_pose.pose.position.z - table_extent[2] pregrasp_pose = deepcopy(grasp_pose) pregrasp_pose.pose.position.x += presets['pregrasp_offset'][0] pregrasp_pose.pose.position.y += presets['pregrasp_offset'][1] pregrasp_pose.pose.position.z += presets['pregrasp_offset'][2] lift_pose = deepcopy(grasp_pose) lift_pose.pose.position.x += presets['lift_offset'][0] lift_pose.pose.position.y += presets['lift_offset'][1] lift_pose.pose.position.z += presets['lift_offset'][2] mp = MotionPlan() # open hand mp += CallFunction(sss.move, 'sdh', 'cylopen', False) mp += MoveArm('arm', [pregrasp_pose, [presets['tcp_link']]], seed=presets['pregrasp_seed']) mp += MoveComponent('sdh', 'cylopen', True) # allow collison hand/object for l in hand_description.HandDescription('arm').touch_links: mp += EnableCollision("grasp_object", l) # goto grasp mp += MoveArm('arm', [grasp_pose, [presets['tcp_link']] ]) # Move sdh_grasp_link to grasp_pose # close hand mp += MoveComponent('sdh', 'cylclosed') # check grasp #mp += CheckService('/sdh_controller/is_cylindric_grasped', Trigger, lambda res: res.success.data) # disable collison mp += ResetCollisions() # attach object mp += AttachObject('arm', "grasp_object") # enable collision mp += EnableCollision("grasp_object", "table") # lift motion mp += MoveArm('arm', [lift_pose, [presets['tcp_link']] ]) # Move sdh_grasp_link to lift_pose # disable collison mp += ResetCollisions() # move away #mp += MoveArm('arm', [pregrasp_pose,[presets['tcp_link']]]) # goto hold mp += MoveArm('arm', 'hold') userdata.graspdata = graspdata if not mp.plan(5).success: sss.set_light('green') return "not_grasped" sss.set_light('yellow') sss.say(["I am grasping " + userdata.object.label + " now."], False) # run, handle errors i = 0 for ex in mp.execute(): if not ex.wait(80.0).success: sss.set_light('red') return 'failed' i += 1 sss.set_light('green') return 'grasped'
def execute(self, userdata): sss.set_light("blue") wi = WorldInterface() wi.reset_attached_objects() graspdata = dict() print userdata.object # add wall wall_extent = [3.0, 0.1, 2.5] wall_pose = conversions.create_pose_stamped( [0, -0.99 - wall_extent[1], wall_extent[2] / 2.0, 0, 0, 0, 1], "base_link" ) # magic numbers are cool wi.add_collision_box(wall_pose, wall_extent, "wall") # add floor floor_extent = [3.0, 3.0, 0.1] floor_pose = conversions.create_pose_stamped( [0, 0, floor_extent[2] / 2.0, 0, 0, 0, 1], "/base_link" ) # magic numbers are cool wi.add_collision_box(floor_pose, floor_extent, "floor") # transform into base_link grasp_pose = transform_listener.transform_pose_stamped("base_link", userdata.object.pose, use_most_recent=False) # add object bounding box obj_pose = deepcopy(grasp_pose) lwh = userdata.object.bounding_box_lwh m1 = pm.toMatrix(pm.fromMsg(obj_pose.pose)) m2 = pm.toMatrix(pm.fromTf(((0, 0, lwh.z / 2.0), (0, 0, 0, 1)))) obj_pose.pose = pm.toMsg(pm.fromMatrix(numpy.dot(m1, m2))) wi.add_collision_box(obj_pose, (lwh.x * 2.0, lwh.y * 2.0, lwh.z), "grasp_object") print grasp_pose # add table table_extent = (2.0, 2.0, grasp_pose.pose.position.z) table_pose = conversions.create_pose_stamped( [-0.5 - table_extent[0] / 2.0, 0, table_extent[2] / 2.0, 0, 0, 0, 1], "base_link" ) wi.add_collision_box(table_pose, table_extent, "table") # calculate grasp and lift pose grasp_pose.pose.position.x += 0.02 grasp_pose.pose.position.y += 0.02 grasp_pose.pose.position.z += 0.1 # 0.03 + 0.05 grasp_pose.pose.orientation = Quaternion( *quaternion_from_euler(-1.706, 0.113, 2.278) ) # orientation of sdh_grasp_link in base_link for 'grasp' joint goal graspdata["height"] = grasp_pose.pose.position.z - table_extent[2] pregrasp_pose = deepcopy(grasp_pose) pregrasp_pose.pose.position.x += 0.20 pregrasp_pose.pose.position.y += 0.11 pregrasp_pose.pose.position.z += 0.1 lift_pose = deepcopy(grasp_pose) lift_pose.pose.position.z += 0.03 mp = MotionPlan() # open hand mp += CallFunction(sss.move, "sdh", "cylopen", False) mp += MoveArm("arm", [pregrasp_pose, ["sdh_grasp_link"]], seed="pregrasp") mp += MoveComponent("sdh", "cylopen", True) # allow collison hand/object # for l in hand_description.HandDescription('arm').touch_links: # mp += EnableCollision("grasp_object", l) # # disable collison # mp += ResetCollisions() # goto grasp mp += MoveArmUnplanned("arm", [grasp_pose, ["sdh_grasp_link"]]) # close hand mp += MoveComponent("sdh", "cylclosed") # check grasp # mp += CheckService('/sdh_controller/is_cylindric_grasped', Trigger, lambda res: res.success.data) # attach object mp += AttachObject("arm", "grasp_object") # enable collision mp += EnableCollision("grasp_object", "table") # lift motion mp += MoveArmUnplanned("arm", [lift_pose, ["sdh_grasp_link"]]) # disable collison mp += ResetCollisions() # move away mp += MoveArm("arm", [pregrasp_pose, ["sdh_grasp_link"]]) # goto hold mp += MoveArm("arm", "hold") userdata.graspdata = graspdata if not mp.plan(5).success: sss.set_light("green") return "not_grasped" sss.set_light("yellow") sss.say(["I am grasping " + userdata.object.label + " now."], False) # run, handle errors i = 0 for ex in mp.execute(): if not ex.wait(80.0).success: sss.set_light("red") return "failed" i += 1 sss.set_light("green") return "grasped"
def execute(self, userdata, overwrite_presets = dict()): presets = self.default_presets presets.update(overwrite_presets) sss.set_light('blue') wi = WorldInterface() wi.reset_attached_objects() graspdata = dict() print userdata.object print "time difference = ", (rospy.Time.now() - userdata.object.pose.header.stamp).to_sec() # add wall wall_extent = [3.0,0.1,2.5] wall_pose = conversions.create_pose_stamped([ 0, -0.99 - wall_extent[1], wall_extent[2]/2.0 ,0,0,0,1], '/base_link') # magic numbers are cool wi.add_collision_box(wall_pose, wall_extent, "wall") # add floor floor_extent = [3.0,3.0,0.1] floor_pose = conversions.create_pose_stamped([ 0, 0, floor_extent[2]/2.0 ,0,0,0,1], '/base_link') # magic numbers are cool wi.add_collision_box(floor_pose, floor_extent, "floor") #transform into base_link grasp_pose = transform_listener.transform_pose_stamped('/base_link', userdata.object.pose, use_most_recent=False) print grasp_pose # add object bounding box obj_pose = deepcopy(userdata.object.pose) lwh = userdata.object.bounding_box_lwh m1 = pm.toMatrix( pm.fromMsg(obj_pose.pose) ) m2 = pm.toMatrix( pm.fromTf( ((0,0, lwh.z/2.0),(0,0,0,1)) ) ) obj_pose.pose = pm.toMsg( pm.fromMatrix(numpy.dot(m1,m2)) ) wi.add_collision_box(obj_pose,(lwh.x*2.0,lwh.y*2.0,lwh.z) , "grasp_object") # compute minimal height of object bounding box edges in base_link min_z = grasp_pose.pose.position.z for k in [(0.5,0.5,1.0),(0.5,0.5,0.0),(0.5,-0.5,1.0),(0.5,-0.5,0.0),(-0.5,0.5,1.0),(-0.5,0.5,0.0),(-0.5,-0.5,1.0),(-0.5,-0.5,0.0)]: m1 = pm.toMatrix( pm.fromMsg(grasp_pose.pose) ) # BFromO m2 = pm.toMatrix( pm.fromTf( ((k[0]*lwh.x,k[1]*lwh.y, k[2]*lwh.z),(0,0,0,1)) ) ) # inO min_z = min(min_z,pm.toMsg( pm.fromMatrix(numpy.dot(m1,m2))).position.z) #min_z inB # add table table_extent = (2.0, 2.0, min_z) # base_link # x - points towards front of robot # y - follow right hand rule # z - points upwards table_pose = conversions.create_pose_stamped([ -0.5 - table_extent[0]/2.0, 0 ,table_extent[2]/2.0 ,0,0,0,1], '/base_link') # center of table, 0.5 meter behind the robot wi.add_collision_box(table_pose, table_extent, "table") # calculate grasp and lift pose grasp_pose.pose.position.x += presets['grasp_offset'][0] grasp_pose.pose.position.y += presets['grasp_offset'][1] grasp_pose.pose.position.z += presets['grasp_offset'][2] grasp_pose.pose.orientation = Quaternion(*quaternion_from_euler(*presets['fixed_orientation_euler'])) # orientation of sdh_grasp_link in base_link for 'grasp' joint goal graspdata['height'] = grasp_pose.pose.position.z - table_extent[2] pregrasp_pose = deepcopy(grasp_pose) pregrasp_pose.pose.position.x += presets['pregrasp_offset'][0] pregrasp_pose.pose.position.y += presets['pregrasp_offset'][1] pregrasp_pose.pose.position.z += presets['pregrasp_offset'][2] lift_pose = deepcopy(grasp_pose) lift_pose.pose.position.x += presets['lift_offset'][0] lift_pose.pose.position.y += presets['lift_offset'][1] lift_pose.pose.position.z += presets['lift_offset'][2] mp = MotionPlan() # open hand mp += CallFunction(sss.move, 'sdh','cylopen', False) mp += MoveArm('arm',[pregrasp_pose,[presets['tcp_link']]], seed = presets['pregrasp_seed']) mp += MoveComponent('sdh','cylopen', True) # allow collison hand/object for l in hand_description.HandDescription('arm').touch_links: mp += EnableCollision("grasp_object", l) # goto grasp mp += MoveArm('arm', [grasp_pose,[presets['tcp_link']]]) # Move sdh_grasp_link to grasp_pose # close hand mp += MoveComponent('sdh','cylclosed') # check grasp #mp += CheckService('/sdh_controller/is_cylindric_grasped', Trigger, lambda res: res.success.data) # disable collison mp += ResetCollisions() # attach object mp += AttachObject('arm', "grasp_object") # enable collision mp += EnableCollision("grasp_object", "table") # lift motion mp += MoveArm('arm', [lift_pose,[presets['tcp_link']]]) # Move sdh_grasp_link to lift_pose # disable collison mp += ResetCollisions() # move away #mp += MoveArm('arm', [pregrasp_pose,[presets['tcp_link']]]) # goto hold mp += MoveArm('arm', 'hold') userdata.graspdata = graspdata if not mp.plan(5).success: sss.set_light('green') return "not_grasped" sss.set_light('yellow') sss.say(["I am grasping " + userdata.object.label + " now."],False) # run, handle errors i = 0 for ex in mp.execute(): if not ex.wait(80.0).success: sss.set_light('red') return 'failed' i+=1 sss.set_light('green') return 'grasped'
def execute(self, target=PoseStamped(), blocking=True): ah = ActionHandle("pick_up", "dummy", "", blocking) rospy.loginfo("Picking up object...") #ah = self.actions.grasp_object(target, blocking) wi = WorldInterface() wi.reset_attached_objects() wi.reset_collision_objects() # add table table_extent = (2.0, 2.0, 1.0) table_pose = conversions.create_pose_stamped([ -0.5 - table_extent[0]/2.0, 0 ,table_extent[2]/2.0 ,0,0,0,1], 'base_link') wi.add_collision_box(table_pose, table_extent, "table") mp = MotionPlan() mp += CallFunction(sss.move, 'torso','front') #mp += MoveArm('arm',['pregrasp']) mp += CallFunction(sss.move, 'sdh','cylopen', False) # OpenIssues: # - where to place the sdh_grasp_link to grasp the object located at target? # - there is no orientation in the target? -> hardcoded taken from pregrasp grasp_pose = PoseStamped() grasp_pose = deepcopy(target) grasp_pose.header.stamp = rospy.Time.now() grasp_pose.pose.orientation.x = 0.220 grasp_pose.pose.orientation.y = 0.670 grasp_pose.pose.orientation.z = -0.663 grasp_pose.pose.orientation.w = -0.253 mp += MoveArm('arm',[grasp_pose,['sdh_grasp_link']], seed = 'pregrasp') mp += CallFunction(sss.move, 'sdh','cylclosed', True) #ah = self.actions.lift_object(target, blocking) lift_pose = PoseStamped() lift_pose = deepcopy(target) lift_pose.header.stamp = rospy.Time.now() lift_pose.pose.position.z += 0.08 lift_pose.pose.orientation.x = 0.220 lift_pose.pose.orientation.y = 0.670 lift_pose.pose.orientation.z = -0.663 lift_pose.pose.orientation.w = -0.253 mp += MoveArm('arm',[lift_pose,['sdh_grasp_link']]) #ah = self.actions.retrieve_object(target, blocking) #mp += MoveArm('arm',['pregrasp']) mp += MoveArm('arm',['wavein']) planning_res = mp.plan(2) print planning_res if planning_res.success: for e in mp.execute(): exec_res = e.wait() print exec_res if not exec_res.success: #rospy.logerr("Execution of MotionExecutable %s failed", e.name) ah.set_failed(4) break ah.set_succeeded() else: rospy.logerr("Planning failed") ah.set_failed(4) return ah