r(q_init) q_init = fullBody.generateContacts(q_init,dir_init,acc_init,robTreshold) r(q_init) # 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(q_init) fullBody.setStartState(q_init,[rLegId,lLegId]) fullBody.setEndState(q_goal,[rLegId,lLegId]) configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 0, filterStates = True) print "number of configs :", len(configs) r(configs[-1]) time.sleep(3) """ from hpp.gepetto import PathPlayer pp = PathPlayer (fullBody.client.basic, r) import fullBodyPlayerHrp2 player = fullBodyPlayerHrp2.Player(fullBody,pp,tp,configs,draw=False,use_window=1,optim_effector=False,use_velocity=True,pathId = pId)
q_init[configSize + 3:configSize + 6] = acc_init[::] q_goal[configSize:configSize + 3] = dir_goal[::] q_goal[configSize + 3:configSize + 6] = acc_goal[::] fullBody.setStaticStability(False) # Randomly generating a contact configuration at q_init fullBody.setCurrentConfig(q_init) q_init = fullBody.generateContacts(q_init, dir_init, acc_init, 2) # Randomly generating a contact configuration at q_end fullBody.setCurrentConfig(q_goal) q_goal = fullBody.generateContacts(q_goal, dir_goal, acc_goal, 2) # specifying the full body configurations as start and goal state of the problem fullBody.setStartState(q_init, [larmId, rLegId, rarmId, lLegId]) fullBody.setEndState(q_goal, [larmId, rLegId, rarmId, lLegId]) r(q_init) # computing the contact sequence configs = fullBody.interpolate(0.001, pathId=pathId, robustnessTreshold=0, filterStates=True) """ print "number of configs =", len(configs) r(configs[-1]) from hpp.gepetto import PathPlayer pp = PathPlayer (fullBody.client.basic, r)
fullBody.setCurrentConfig (q_init) q_init = fullBody.generateContacts(q_init, [0,0,1]) #~ from pickle import load #~ f = open("config_"+str(tp.config_i), 'r') #~ q_init = load(f) #~ f.close() # Randomly generating a contact configuration at q_end fullBody.setCurrentConfig (q_goal) q_goal = fullBody.generateContacts(q_goal, [0,0,1]) # specifying the full body configurations as start and goal state of the problem fullBody.setStartState(q_init,[]) #~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId]) fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId]) r(q_init) # computing the contact sequence #~ configs = fullBody.interpolate(0.05, 10, 1, True) configs = fullBody.interpolate(0.08, 10, 10, True) #~ configs = fullBody.interpolate(0.11, 7, 10, True) #~ configs = fullBody.interpolate(0.1, 1, 10, True) #~ configs = fullBody.interpolate(0.02, 10, 10, True) import time try: time.sleep(2) fullBody.dumpProfile() except Exception as e: