#~ 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: pass
q_goal = fullBody.getCurrentConfig(); q_goal[0:confsize] = tp.q_goal[0:confsize] fullBody.setCurrentConfig (q_init) #~ q_0 = fullBody.getCurrentConfig(); q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init) fullBody.setCurrentConfig (q_goal) #~ r(q_goal) q_goal = fullBody.generateContacts(q_goal, [0,0,1]) #~ r(q_goal) #~ gui = r.client.gui #~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId]) fullBody.setStartState(q_init,[rLegId,lLegId,larmId]) fullBody.setEndState(q_goal,[rLegId,lLegId]) #~ #~ r(q_init) configs = fullBody.interpolate(0.03, 10, 5, True) import time try: time.sleep(2) fullBody.dumpProfile() except Exception as e: pass
#~ 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)
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # 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)
fullBody.setCurrentConfig (q_goal)
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
#~ r(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.15)
i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1
r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
#~ fullBody.exportAll(r, configs, 'stair_bauzil_hrp2_robust_2');
#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT0',[-1])
#~ q_0 = fullBody.getCurrentConfig();
#~ fullBody.draw(q_0,r);
#~ print(fullBody.client.rbprm.rbprm.getOctreeTransform(rarmId, q_0))
#~
#~
#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT0',[1])
#~ q_0 = fullBody.getCurrentConfig();
#~ fullBody.draw(q_0,r);
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[::]
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartStateId(s_init.sId)
fullBody.setEndStateId(s_goal.sId)
q_far = q_init[::]
q_far[2] = -5
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 2, filterStates = True)
r(configs[-1])
#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
"""
from fullBodyPlayer import Player
player = Player(fullBody,pp,tp,configs,draw=True,optim_effector=False,use_velocity=dynamic,pathId = 0)
player.displayContactPlan()
#player.interpolate(5,len(configs)-2)
#player.play()
"""
fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
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.08, 1, 3) #hole
import time
try:
time.sleep(2)
fullBody.dumpProfile()
except Exception as e:
pass
r.addLandmark('hrp2_14/BODY', 0.3)
r(q_init)
fullBody.setStartState(q_init, [rLegId, lLegId])
fullBody.setEndState(q_goal, [rLegId, lLegId])
fullBody.setStaticStability(
False) # only set it after the init/goal configuration are computed
from hpp.gepetto import PathPlayer
pp = PathPlayer(fullBody.client.basic, r)
import fullBodyPlayerHrp2
tStart = time.time()
configs = fullBody.interpolate(0.01,
pathId=pId,
robustnessTreshold=robTreshold,
filterStates=True)
tInterpolate = time.time() - tStart
print("number of configs : ", len(configs))
print("generated in " + str(tInterpolate) + " s")
r(configs[len(configs) - 2])
player = fullBodyPlayerHrp2.Player(fullBody,
pp,
tp,
configs,
draw=False,
use_window=1,
optim_effector=True,
use_velocity=False,
pathId=pId)
#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('talos/base_link', 0.3)
r(q_init)
fullBody.setStartState(q_init, [rLegId, lLegId])
fullBody.setEndState(q_goal, [rLegId, lLegId])
from hpp.gepetto import PathPlayer
pp = PathPlayer(fullBody.client.basic, r)
tStart = time.time()
configsFull = fullBody.interpolate(0.01,
pathId=pId,
robustnessTreshold=2,
filterStates=False)
tInterpolateConfigs = time.time() - tStart
print "number of configs :", len(configsFull)
from configs.talos_flatGround import *
from generate_contact_sequence import *
beginState = 0
endState = 6
configs = configsFull[beginState:endState + 1]
cs = generateContactSequence(fullBody, configs, beginState, endState, r)
#player.displayContactPlan()
filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
cs.saveAsXML(filename, "ContactSequence")
r(q_init)
fullBody.setStartState(q_init, [rLegId, lLegId])
fullBody.setEndState(q_goal, [rLegId, lLegId])
fullBody.setStaticStability(
True) # only set it after the init/goal configuration are computed
from hpp.gepetto import PathPlayer
pp = PathPlayer(fullBody.client.basic, r)
import fullBodyPlayerHrp2
tStart = time.time()
configs = fullBody.interpolate(0.01,
pathId=pId,
robustnessTreshold=robTreshold,
filterStates=True,
testReachability=True,
quasiStatic=True)
tInterpolate = time.time() - tStart
print "number of configs : ", len(configs)
print "generated in " + str(tInterpolate) + " s"
r(configs[len(configs) - 2])
f = open(
"/local/fernbac/bench_iros18/kin_constraint_tog/without_kin_constraints.log",
"a")
global num_transition
global valid_transition
global length_traj
global valid_length
q_init = fullBody.generateContacts(q_init, [0, 0, -1])
r(q_init)
fullBody.setCurrentConfig(q_goal)
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0, 0, 1])
#~ r(q_goal)
#~ gui = r.client.gui
#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody.setStartState(q_init, [rLegId, lLegId, larmId])
fullBody.setEndState(q_goal, [rLegId, lLegId])
#~
#~ r(q_init)
configs = fullBody.interpolate(0.02, 10, 5, False)
#~ r.loadObstacleModel ('hpp-rbprm-corba', "polaris", "contact")
#~ configs = fullBody.interpolate(0.09)
#~ 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)
#~ fullBody.draw(q_0,r)
#~ fullBody.client.rbprm.rbprm.getOctreeTransform(larmId, q_0)
#~ problem = ps.client.problem
#~ length = problem.pathLength (0)
#~ t = 0
#~ i = 0
#~ configs = []
q = q_init[::]
q[0] += 0.3
q = fullBody.generateContacts(q,dir_init,acc_init,robTreshold)
mid_sid = fullBody.addState(q,[lLegId,rLegId])
"""
from hpp.gepetto import PathPlayer
pp = PathPlayer(fullBody.client.basic, r)
import fullBodyPlayerHrp2
tStart = time.time()
configsFull = fullBody.interpolate(0.01,
pathId=pId,
robustnessTreshold=0,
filterStates=True,
testReachability=False,
quasiStatic=False)
tInterpolateConfigs = time.time() - tStart
print("number of configs :", len(configsFull))
r(configsFull[-1])
"""
import check_qp
planValid,curves_initGuess,timings_initGuess = check_qp.check_contact_plan(ps,r,pp,fullBody,0,len(configsFull))
print "Contact plan valid : "+str(planValid)
"""
from configs.straight_walk_dynamic_planning_config import *
from generate_contact_sequence import *
beginState = 0
fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True) fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True) fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True) fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True) 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.08,1,3) #hole import time try: time.sleep(2) fullBody.dumpProfile() except Exception as e: pass
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
pp = PathPlayer (fullBody.client.basic, r)
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj, profile
q_init = fullBody.generateContacts(q_init, [0, 0, -1])
r(q_init)
fullBody.setCurrentConfig(q_goal)
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0, 0, 1])
#~ r(q_goal)
#~ gui = r.client.gui
#~ fullBody.setStartState(q_init,[rLegId,lLegId,larmId])
fullBody.setStartState(q_init, [rLegId, lLegId])
fullBody.setEndState(q_goal, [rLegId, lLegId])
#~
#~ r(q_init)
configs = fullBody.interpolate(0.01, 10, 10, True)
#~ r.loadObstacleModel ('hpp-rbprm-corba', "polaris", "contact")
#~ configs = fullBody.interpolate(0.09)
#~ 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)
#~ fullBody.draw(q_0,r)
#~ fullBody.client.rbprm.rbprm.getOctreeTransform(larmId, q_0)
#~ problem = ps.client.problem
#~ length = problem.pathLength (0)
#~ t = 0
#~ i = 0
#~ configs = []
