def manip_from_pose_grasp(pose, grasp): # This is the robot wrist manip_trans = pose.value.compose(grasp.grasp_trans) # Ad-hoc backoff strategy if abs(manip_trans.matrix[2,0]) < 0.1: # horizontal offset = hu.Pose(-glob.approachBackoff,0.,glob.approachPerpBackoff,0.) else: # vertical offset = hu.Pose(-glob.approachBackoff,0.,0.,0.) manip_trans_approach = manip_trans.compose(offset) # The grasp and the approach return manip_trans, manip_trans_approach
def test0(): var = tinyVar # bigVar | tinyVar exp = Experiment( {'tableIkea1': (hu.Pose(1.3, 0.0, 0.0, math.pi / 2.0), var)}, {'objA': (hu.Pose(1.1, 0.0, ikZ, 0.0), var)}, ['tableIkea1Top', 'tableIkea1Left'], easy=False) # easy replaces the variance #goal = emptyHand(hand) #goal = holding('objA', hand=hand, graspType=grasp_type) goal = inRegion(['objA'], 'tableIkea1Left') return exp, goal
def testBusy(hardSwap=True, **args): glob.rebindPenalty = 40 # Put this back to make the problem harder #back = hu.Pose(1.1, 0.0, tZ, 0.0) back = hu.Pose(1.45, 0.0, tZ, 0.0) #parking1 = hu.Pose(1.15, 0.3, tZ, 0.0) #parking2 = hu.Pose(1.15, -0.3, tZ, 0.0) exp = Experiment( { 'table1': (table1Pose, smallVar), 'table2': (table2Pose, smallVar) }, { 'objA': (back, medVar), 'objB': (hu.Pose(1.15, -0.4, tZ, 0.0), medVar), 'objC': (hu.Pose(0.65, -1.2, tZ, 0.0), medVar), 'objD': (hu.Pose(1.15, -0.2, tZ, 0.0), medVar), 'objE': (hu.Pose(1.15, 0.0, tZ, 0.0), medVar), 'objF': (hu.Pose(1.15, 0.2, tZ, 0.0), medVar), 'objG': (hu.Pose(1.15, 0.4, tZ, 0.0), medVar) }, ['table1Top', 'table2Top', 'table1MidFront', 'table1MidRear'], easy=args.get('easy', False)) goal = inRegion(['objA', 'objB'], ['table1MidFront', 'table1MidRear']) # A on other table goal1 = inRegion('objA', 'table2Top') # A and B on other table goal2 = inRegion(['objA', 'objB'], 'table2Top') # B in back goal3 = inRegion('objB', 'table1MidRear') actualGoal = goal if hardSwap else goal3 return exp, actualGoal
def box_body(env, length, width, height, name='box', color='black'): def trans(graspIndex): # We're using the origin of object, so don't need to specify restFace wrist = objectGraspFrameAux(gd, graspIndex, graspMu, None, None, Ident, robot, hand, origin=True) return wrist shape, _ = glob.constructor[shape_name](dx=length/2., dy=width/2., dz=height, name=name, color=color) if length <= 0.7 and width <= 0.7: # graspable gd = env.world.getObjData(shape_name, 'graspDesc', []) hand = env.arm robot = env.robot graspMu = hu.Pose(0.0, -0.025, 0.0, 0.0) # Note !! constant Ident = hu.Pose(0,0,0,0) env.grasps[name] = [Grasp(name, i, trans(i)) for i,_ in enumerate(gd)] else: env.grasps[name] = [] return Body(env, shape)
def make_place(hand, obj, rest_face, mu_pose, grasp_conf, approach_conf, var=tinyVar): # rest_face is just for updating discrete distribution over resting face #face_frames = world.getFaceFrames(obj) face_frames = None # Not actually used in the operator pb = ObjPlaceB(obj, face_frames, rest_face, PoseD(hu.Pose(*mu_pose), var)) return ('Place', (approach_conf, grasp_conf, hand, pb))
def openrave_test( env): # NOTE - cannot do Leslie and Tomas's viewers with openrave test = testBusy # test0 | testBusy exp, _ = test() test = PlanTest(exp, noWindow=True) test.load() conf, (leftHeld, leftObjGraspType, leftObjGrasp), \ (rightHeld, rightObjGraspType, rightObjGrasp), objects = makeMLS(test.bs) set_or_state(env, test.world, conf, {name: hu.Pose(*pose) for name, _, _, pose in objects}) raw_input('Finish?')
def make_pick(hand, obj, grasp_index, mu_grasp, grasp_conf, approach_conf, var=tinyVar): # grasp_index is just for updating discrete distribution over grasps #grasp_desc = glob.objectDataBase['graspDesc'][obj] #gb = ObjGraspB(obj, grasp_desc, grasp, None, PoseD(hu.Pose(*mu), var)) grasp_descs = None gb = ObjGraspB(obj, grasp_descs, grasp_index, None, PoseD(hu.Pose(*mu_grasp), var)) return ('Pick', (approach_conf, grasp_conf, hand, gb))
def sample_base_pose_from_hand_pose( robot, hand_pose, attempts=20): #Hand pose represents the tip of the hand base_limits = map(lambda pair: ranges.realRange(*pair), list(robot.limits(['pr2Base']))) for i in range(attempts): base_pose = hand_pose.compose(choice(HORIZONTAL).inversePose()).pose() if not (base_limits[0].inside(base_pose.x) and base_limits[1].inside( base_pose.y) and base_limits[2].inside(base_pose.theta)): continue base_pose = hu.Pose(base_pose.x, base_pose.y, 0.0, base_pose.theta) #if not object_collides(base.applyTrans(base_pose), obstacles = self.obstacles): return base_pose return None
def pose_from_quat_point(quat, xyz): angle = euler_from_quaternion(quat)[2] # z rotation return hu.Pose(xyz[0], xyz[1], xyz[2], angle)
(0.,0.,1.,-0.025), (1.,0.,0.,0.02), (0.,0.,0.,1.)])) gMat0h = hu.Transform(np.array([(0.,1.,0.,0.0), (0.,0.,1.,-0.025), (1.,0.,0.,0.05), (0.,0.,0.,1.)])) gMat1 = hu.Transform(np.array([(0.,-1.,0.,0.), (0.,0.,-1.,0.025), (1.,0.,0.,0.02), (0.,0.,0.,1.)])) gMat1h = hu.Transform(np.array([(0.,-1.,0.,0.), (0.,0.,-1.,0.025), (1.,0.,0.,0.05), (0.,0.,0.,1.)])) gMat4 = hu.Pose(0,0,0,math.pi/2).compose(gMat0) gMat5 = hu.Pose(0,0,0,-math.pi/2).compose(gMat0) # from the top gMat2= hu.Transform(np.array([(-1.,0.,0.,0.), (0.,0.,-1.,0.025), (0.,-1.,0.,0.), (0.,0.,0.,1.)])) gMat3= hu.Transform(np.array([(1.,0.,0.,0.), (0.,0.,1.,-0.025), (0.,-1.,0.,0.), (0.,0.,0.,1.)])) gdesc0 = lambda obj: GDesc(obj, gMat0, 0.05, 0.05, 0.025) gdesc0h = lambda obj: GDesc(obj, gMat0h, 0.05, 0.05, 0.025) gdesc1 = lambda obj: GDesc(obj, gMat1, 0.05, 0.05, 0.025) gdesc1h = lambda obj: GDesc(obj, gMat1h, 0.05, 0.05, 0.025)
def grasp_test(env, no_window=True, name='objA', hand='right', window_name='W'): exp, goal = test0() test = PlanTest(exp, noWindow=no_window) test.load() # TODO - do I need to pass regions here? belief = test.bs print SEPARATOR # NOTE - should cascade functions to get values # NOTE - I need to be able to generate from the current placement #domainProbs.maxGraspVar #graspV = pbs.beliefContext.domainProbs.maxGraspVar hand = 'left' grasp_index = 2 inputs = [ 'objA', grasp_index, (0, 0, 0, 0), typicalErrProbs.placeVar, typicalErrProbs.pickVar, typicalErrProbs.placeDelta, typicalErrProbs.graspDelta, hand, probForGenerators ] for outputs in pick_fn.fun(inputs, goal.fluents, belief): print outputs raw_input('Continue?') return print SEPARATOR conf, _, _, objects = makeMLS(belief) # Extract MLS from b objects_dict = {props[0]: props[1:] for props in objects} type_name, support_face, pose = objects_dict[name] wrapped_pose = hu.Pose(*pose) grasp_desc = belief.objects[name].attrs[ 'graspDesc'] # 1 is side grasp and 3 is top grasp set_or_state(env, belief.pbs.getWorld(), conf, {name: hu.Pose(*pose) for name, _, _, pose in objects}) robot = env.GetRobots()[0] placeB = belief.pbs.makePlaceB( name, support=support_face, pose=wrapped_pose, var=tinyVar, delta=typicalErrProbs.placeDelta) # ObjPlaceB for grasp_index in range(len(grasp_desc)): print '\nGrasp index:', grasp_index graspB = belief.pbs.makeGraspB( name, grasp=grasp_index, support=None, pose=IDENTITY_POSE, var=tinyVar, delta=typicalErrProbs.graspDelta) # ObjGraspB for conf1, conf2, _ in islice( potentialGraspConfGen(belief.pbs, placeB, graspB, None, hand, None, prob=0.001, nMax=100, findApproach=True), 10): set_full_config(robot, or_from_hpn_conf(robot, conf1)) robot.SetDOFValues( np.array([0.54800022]), robot.GetManipulator(hand + 'arm').GetGripperIndices()) conf1.draw(window_name) raw_input('One?') set_full_config(robot, or_from_hpn_conf(robot, conf2)) conf2.draw(window_name) raw_input('Two?')
def policy(self, belief, goal): if satisfies(belief, goal): return [] #if self.iterations != 0: # return [] self.iterations += 1 if self.plan is not None: if len(self.plan) == 0: return [] return [self.plan.pop(0)] print print belief.pbs.__class__ # PBS (Planning Belief State?) print belief.pbs.getWorld().__class__ # World # TODO - realWorld is RealWorld or RobotEnv within PlanTest. Has executePrim within it print 'Regions', self.exp.regions USE_HAND = 'right' print belief.makeObjGraspB(USE_HAND) # Gets the current grasp? conf, (leftHeld, leftObjGraspType, leftObjGrasp), \ (rightHeld, rightObjGraspType, rightObjGrasp), objects = makeMLS(belief) # Extract MLS from b objects_dict = {props[0]: props[1:] for props in objects} robot = conf.robot #print conf.cartConf() print name = 'objA' type_name, support_face, pose = objects_dict[name] wrapped_pose = hu.Pose(*pose) identity_pose = hu.Pose(0, 0, 0, 0) print wrapped_pose.matrix print type_name, support_face, pose geom, _ = glob.constructor[type_name](name=name) grasp_desc = belief.objects[name].attrs['graspDesc'] grasp_index = 0 grasp = grasp_desc[grasp_index] # G * M = P print grasp.frame # Transform print grasp.dx, grasp.dy, grasp.dz # Don't know what these do... grasp_trans = grasp.frame.inverse().compose(wrapped_pose) print robot.potentialBasePosesGen print robot.inverseKinWristGen # Allows choosing base pose print graspConfHypGen #grasp_cart = RobotCartConf(grasp_trans, robot) # First argument must be a conf? grasp_cart = conf.getCart() grasp_cart = grasp_cart.set('pr2RightArm', grasp_trans) print print 'Kinematics' print conf.getCart() #print conf #print robot.inverseKin(get_manip(conf), conf) print robot.inverseKin(grasp_cart, conf) print # TODO - need to sample the base? # Return plan with one move operation currConf = copy.copy(conf) goalConf = currConf.setBaseConf((0.2, 0.2, 0.2)) self.plan = [ make_move(currConf, goalConf), make_pick(USE_HAND, name, grasp_desc, grasp_index, pose, tinyVar, currConf, currConf), ] print print conf print leftHeld, leftObjGraspType, leftObjGrasp print rightHeld, rightObjGraspType, rightObjGrasp print objects #print goal.satisfies(belief) # Nope #print belief.satisfies(goal) # Nope print goal print self.plan raw_input('Found plan! Continue?') print return None #return None # Fail #return [] # Sucess return self.plan[:1] # NOTE - only the first step really matters
def sample_pose(pose_belief, dist): face_frame = pose_belief.faceFrames[pose_belief.restFace] return hu.Pose(*dist.draw()).compose(face_frame.inverse())
if plan is None: return print plan if no_window: visualize_plan(Plan( convert_state(oracle, stream_problem.initial_atoms), executable_plan(oracle, plan)), oracle, display=True, save=None) ################################################################# IDENTITY_POSE = hu.Pose(0, 0, 0, 0) from mm_operators.mmOps import PickGen, PlaceGen, probForGenerators, PoseInRegionGen from mm_operators.mmGen import pickGenTop, potentialGraspConfGen, placeInRegionGenGen, placeInGenTop, potentialRegionPoseGen # NOTE - Leslie and Tomas's primitives try placing in the belief of something from mm_operators.mmOps import moveNBPrim, movePrim # These produce motions # NOTE - potentialGraspConfGen is used for both pick and place (duh) pick_fn = PickGen([ 'Pose', 'PoseFace', 'PickConf', 'PreConf', 'ConfDelta', 'SupportObj', 'SupportObjPoseFace' ], [ 'Obj', 'GraspFace', 'GraspMu', 'PoseVar', 'RealGraspVar', 'PoseDelta',