class MoveArm(smach.State): def __init__(self, *args, **kwargs): smach.State.__init__(self, outcomes=["succeeded", "not_succeeded", "failed"]) self.mp = MotionPlan() # move arm self.mp += cob_arm_navigation_python.MoveArm.MoveArm(*args, **kwargs) 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"
class MoveArm(smach.State): def __init__(self, *args, **kwargs): smach.State.__init__(self, outcomes=['succeeded', 'not_succeeded', 'failed']) self.mp = MotionPlan() # move arm self.mp += cob_arm_navigation_python.MoveArm.MoveArm(*args, **kwargs) 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"
mp += EnableCollision('milk','tray_link') #mp += MoveArm('arm', 'home') mp += MoveArm('arm', [['base_link',[0.64-0.161, -0.11+0.056,0.832+0.22],[-1.441, 0.118, -0.078]],['sdh_grasp_link']]) mp += MoveComponent('sdh', 'cylclosed') mp += AttachObject('arm', 'milk') mp += EnableCollision('milk','tray_link') mp += MoveArm('arm', [['base_link',[0.64-0.161, -0.11+0.056,0.832+0.5],[-1.441, 0.118, -0.078]],['sdh_grasp_link']]) mp += ResetCollisions() #mp += MoveArm('arm', 'home') mp += MoveArm("arm",[lift_pose,['sdh_grasp_link']]) mp += MoveComponent('sdh', 'cylopen') mp += DetachObject('arm', 'milk') mp += MoveArm('arm', 'folded') planning_res = mp.plan(2) print planning_res if planning_res.success: print "For execution please press ENTER!" raw_input() for e in mp.execute(): exec_res = e.wait() print exec_res if not exec_res.success: print "Execution failed" break else: print "Will not execute, since planning failed" get_planning_scene_interface().reset()
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'
], ['sdh_grasp_link']]) mp += MoveComponent('sdh', 'cylclosed') mp += AttachObject('arm', 'milk') mp += EnableCollision('milk', 'tray_link') mp += MoveArm('arm', [[ 'base_link', [0.64 - 0.161, -0.11 + 0.056, 0.832 + 0.5], [-1.441, 0.118, -0.078] ], ['sdh_grasp_link']]) mp += ResetCollisions() #mp += MoveArm('arm', 'home') mp += MoveArm("arm", [lift_pose, ['sdh_grasp_link']]) mp += MoveComponent('sdh', 'cylopen') mp += DetachObject('arm', 'milk') mp += MoveArm('arm', 'folded') planning_res = mp.plan(2) print planning_res if planning_res.success: print "For execution please press ENTER!" raw_input() for e in mp.execute(): exec_res = e.wait() print exec_res if not exec_res.success: print "Execution failed" break else: print "Will not execute, since planning failed" get_planning_scene_interface().reset()