q_init[configSize:configSize + 3] = dir_init[::]
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) 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
# Randomly generating a contact configuration at q_init
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)
"forward", 0.05, cType)
q_0 = fullBody.getCurrentConfig()
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 = fullBody.generateContacts(q_init, [0, 0, 1])
q_0 = fullBody.getCurrentConfig()
fullBody.setCurrentConfig(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0, 0, 1])
fullBody.setStartState(q_init, [])
fullBody.setEndState(q_goal, [rLegId, lLegId, rarmId, larmId])
r(q_init)
configs = fullBody.interpolate(0.1, 1, 10) #hole
#~ configs = fullBody.interpolate(0.08,1,5) # bridge
r.loadObstacleModel('hpp-rbprm-corba', "groundcrouch", "contact")
#~ fullBody.exportAll(r, configs, 'obstacle_hyq_robust_10');
i = 0
r(configs[i])
i = i + 1
i - 1
from hpp.gepetto import PathPlayer
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
q_0 = fullBody.getCurrentConfig();
q_init = [
0.0, 0.0, 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
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # RARM 18-24
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
]; r (q_init)
q_init[0:7] = tp.q_init[0:7]
q_goal = q_init [::]
q_goal[0:7] = tp.q_goal[0:7]
fullBody.setCurrentConfig (q_goal)
fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
#~
#~ configs = fullBody.interpolate(0.1)
configs = fullBody.interpolate(0.1)
#~ configs = fullBody.interpolate(0.08)
i = 0;
r (configs[i]); i=i+1; i-1
#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
