qfar[2] = -5 #~ AFTER loading obstacles rLegId = 'hrp2_rleg_rom' lLegId = 'hrp2_lleg_rom' tStart = time.time() rLeg = 'RLEG_JOINT0' rLegOffset = [0,0,-0.0955] rLegLimbOffset=[0,0,-0.035]#0.035 rLegNormal = [0,0,1] rLegx = 0.09; rLegy = 0.05 #fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward") fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=rLegLimbOffset) fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True) #fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db") lLeg = 'LLEG_JOINT0' lLegOffset = [0,0,-0.0955] lLegLimbOffset=[0,0,0.035] lLegNormal = [0,0,1] lLegx = 0.09; lLegy = 0.05 #fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward") fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset) fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True) #fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db") fullBody.setReferenceConfig (q_ref) ## Add arms (not used for contact) :
# Randomly generating a contact configuration at q_end fullBody.setCurrentConfig (q_goal) q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,robTreshold) r(q_goal) # specifying the full body configurations as start and goal state of the problem r.addLandmark('hrp2_14/BODY',0.3) r(q_init) fullBody.setStartState(q_init,[rLegId,lLegId]) fullBody.setEndState(q_goal,[rLegId,lLegId]) """ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True) fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True) """ from hpp.gepetto import PathPlayer pp = PathPlayer (fullBody.client.basic, r) import fullBodyPlayerHrp2 configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 1, filterStates = True) print "number of configs :", len(configs)
lArmOffset = [0,0,-0.1075] lArmNormal = [0,0,1] lArmx = 0.024; lArmy = 0.024 #~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", False,grasp = True) fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", False) #~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF") #~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF") rarmId = 'hrp2_rarm_rom' rarm = 'RARM_JOINT0' rHand = 'RARM_JOINT5' rArmOffset = [0,0,-0.1075] rArmNormal = [0,0,1] rArmx = 0.024; rArmy = 0.024 #disabling collision for hook #~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", False,grasp = True) fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", False) #~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF") fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True) fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True) limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'}, lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}, rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand}, larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }
rKnee = 'RLEG_JOINT3' rLegOffset = [0.105,0.055,0.017] rLegNormal = [-1,0,0] rLegx = 0.05; rLegy = 0.05 #~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01) #~ lKneeId = '1LKnee' lLeg = 'LLEG_JOINT0' lKnee = 'LLEG_JOINT3' lLegOffset = [0.105,0.055,0.017] lLegNormal = [-1,0,0] lLegx = 0.05; lLegy = 0.05 #~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01) #~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True) fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True) #~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1]) #~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1]) q_0 = fullBody.getCurrentConfig(); #~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,) q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7] q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7] fullBody.setCurrentConfig (q_init) q_init = [ 0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6 0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10