Commit 6d7525c7 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

export helpers for all scenarios

parent 095ccb59
No preview for this file type
No preview for this file type
No preview for this file type
...@@ -84,14 +84,15 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId]) ...@@ -84,14 +84,15 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init) r(q_init)
#~ configs = fullBody.interpolate(0.1) configs = fullBody.interpolate(0.1)
#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact") r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,) #~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,) #~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
i = 0; i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1 fullBody.draw(configs[i],r); i=i+1; i-1
#~ fullBody.exportAll(r, configs, 'darpa_hyq_robust_1');
#~ r (configs[i]); i=i+1; i-1 #~ r (configs[i]); i=i+1; i-1
#~ q0 = configs[2] #~ q0 = configs[2]
......
...@@ -43,16 +43,18 @@ ps.addGoalConfig (q_goal) ...@@ -43,16 +43,18 @@ ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05) ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "darpa", "planning") r.loadObstacleModel (packageName, "darpa", "planning")
#~ ps.solve () ps.solve ()
from hpp.gepetto import PathPlayer from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r) pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "darpa_hyq_path.txt")
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") #~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~ #~
#~ pp (2) #~ pp (2)
#~ pp (0) #~ pp (0)
#~ pp (1) pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") #~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
r (q_init) r (q_init)
...@@ -76,10 +76,10 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId]) ...@@ -76,10 +76,10 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init) r(q_init)
configs = fullBody.interpolate(0.1,0) configs = fullBody.interpolate(0.1)
r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact") r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
fullBody.exportAll(r, configs, 'obstacle_hyq_robust_1');
i = 0; i = 0;
r (configs[i]); i=i+1; i-1 r (configs[i]); i=i+1; i-1
......
...@@ -40,14 +40,15 @@ ps.setInitialConfig (q_init) ...@@ -40,14 +40,15 @@ ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal) ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.001) ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, "groundcrouch", "planning") r.loadObstacleModel (packageName, "groundcrouch", "planning")
ps.solve () ps.solve ()
rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "obstacle_hyq_path.txt")
from hpp.gepetto import PathPlayer from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r) pp = PathPlayer (rbprmBuilder.client.basic, r)
pp (0) #~ pp (0)
#~ pp (1) #~ pp (1)
r (q_init) r (q_init)
...@@ -108,6 +108,7 @@ i = 0; ...@@ -108,6 +108,7 @@ i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1 fullBody.draw(configs[i],r); i=i+1; i-1
r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact") 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]) #~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT0',[-1])
#~ q_0 = fullBody.getCurrentConfig(); #~ q_0 = fullBody.getCurrentConfig();
#~ fullBody.draw(q_0,r); #~ fullBody.draw(q_0,r);
...@@ -121,7 +122,7 @@ r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact") ...@@ -121,7 +122,7 @@ r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init) #~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
#~ f1 = open("secondchoice","w+") #~ f1 = open("secondchoice","w+")
#~ f1 = open("hrp2_stand_nocone_30_10_15","w+") #~ f1 = open("hrp2_stair_not_robust_configs","w+")
#~ f1.write(str(configs)) #~ f1.write(str(configs))
#~ f1.close() #~ f1.close()
...@@ -59,4 +59,5 @@ pp = PathPlayer (rbprmBuilder.client.basic, r) ...@@ -59,4 +59,5 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (1) #~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") #~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "stair_bauzil_hrp2_path.txt")
r (q_init) r (q_init)
...@@ -102,6 +102,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId]) ...@@ -102,6 +102,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
#~ r(q_init) #~ r(q_init)
configs = fullBody.interpolate(0.1) configs = fullBody.interpolate(0.1)
r.loadObstacleModel ('hpp-rbprm-corba', "scene_wall", "contact") r.loadObstacleModel ('hpp-rbprm-corba', "scene_wall", "contact")
#~ fullBody.exportAll(r, configs, 'standing_hrp2_robust_2');
#~ configs = fullBody.interpolate(0.09) #~ configs = fullBody.interpolate(0.09)
#~ configs = fullBody.interpolate(0.08) #~ configs = fullBody.interpolate(0.08)
i = 0; i = 0;
......
...@@ -63,12 +63,13 @@ pp = PathPlayer (rbprmBuilder.client.basic, r) ...@@ -63,12 +63,13 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (2) #~ pp (2)
#~ pp (0) #~ pp (0)
pp (1) #~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") #~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~ r (q_init) #~ r (q_init)
rob = rbprmBuilder.client.basic.robot rob = rbprmBuilder.client.basic.robot
r(q_init) r(q_init)
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "standing_hrp2__path.txt")
#~ configs = [] #~ configs = []
......
...@@ -45,7 +45,7 @@ rHand = 'RARM_JOINT5' ...@@ -45,7 +45,7 @@ rHand = 'RARM_JOINT5'
rArmOffset = [-0.05,-0.050,-0.050] rArmOffset = [-0.05,-0.050,-0.050]
rArmNormal = [1,0,0] rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024 rArmx = 0.024; rArmy = 0.024
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "random", 0.05) fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "forward", 0.05)
larmId = '4Larm' larmId = '4Larm'
larm = 'LARM_JOINT0' larm = 'LARM_JOINT0'
...@@ -53,30 +53,10 @@ lHand = 'LARM_JOINT5' ...@@ -53,30 +53,10 @@ lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050] lArmOffset = [-0.05,-0.050,-0.050]
lArmNormal = [1,0,0] lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024 lArmx = 0.024; lArmy = 0.024
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "random", 0.05) fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "forward", 0.05)
rKneeId = '0RKnee'
rLeg = 'RLEG_JOINT0'
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)
#~
q_0 = fullBody.getCurrentConfig(); q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
confsize = len(tp.q_init) confsize = len(tp.q_init)
q_init = fullBody.getCurrentConfig(); q_init[0:confsize] = tp.q_init[0:confsize] q_init = fullBody.getCurrentConfig(); q_init[0:confsize] = tp.q_init[0:confsize]
q_goal = fullBody.getCurrentConfig(); q_goal[0:confsize] = tp.q_goal[0:confsize] q_goal = fullBody.getCurrentConfig(); q_goal[0:confsize] = tp.q_goal[0:confsize]
...@@ -96,29 +76,18 @@ q_goal = fullBody.generateContacts(q_goal, [0,0,1]) ...@@ -96,29 +76,18 @@ q_goal = fullBody.generateContacts(q_goal, [0,0,1])
fullBody.setStartState(q_init,[rLegId,lLegId])#,rarmId,larmId]) fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId]) #~ fullBody.setStartState(q_init,[rLegId,lLegId])
#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId])
#~ fullBody.setStartState(q_init,[rLegId,lLegId,larmId])
#~ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
#~ fullBody.setEndState(q_goal,[rLegId,lLegIdlarmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])
#~ #~
#~ r(q_init) #~ r(q_init)
configs = fullBody.interpolate(0.1) configs = fullBody.interpolate(0.1)
r.loadObstacleModel ('hpp-rbprm-corba', "truck", "contact") r.loadObstacleModel ('hpp-rbprm-corba', "truck", "contact")
#~ #~ fullBody.exportAll(r, configs, 'truck_hrp2_not_robust');
#~ 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 (1)
#~ t = 0
#~ i = 0
#~ configs = []
#~ dt = 0.1 / length
#~ while t < length :
#~ q = fullBody.getCurrentConfig()
#~ q[0:confsize] = problem.configAtParam (1, t)[0:confsize]
#~ configs.append(q)
#~ t += dt
#~ i = i+1
#~
i = 0; i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1 fullBody.draw(configs[i],r); i=i+1; i-1
#~ #~
......
...@@ -20,7 +20,7 @@ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom']) ...@@ -20,7 +20,7 @@ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.4) #~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.4)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.6) #~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.6)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.6) #~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.6)
rbprmBuilder.boundSO3([-0.1,0.1,-3,3,-0.0,0.0]) rbprmBuilder.boundSO3([-0.1,0.1,-3,3,-1.0,1.0])
#~ from hpp.corbaserver.rbprm. import ProblemSolver #~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
...@@ -37,7 +37,7 @@ q_goal [0:3] = [0.19, 0.05, 0.9]; r (q_goal) ...@@ -37,7 +37,7 @@ q_goal [0:3] = [0.19, 0.05, 0.9]; r (q_goal)
rbprmBuilder.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, 0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init) rbprmBuilder.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, 0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_init = rbprmBuilder.getCurrentConfig (); q_init = rbprmBuilder.getCurrentConfig ();
q_init[0:9] = [0.27, -0.15, 1.2, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0472] q_init[0:9] = [0.27, -0.05, 1.2, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0472]
rbprmBuilder.setCurrentConfig (q_init); r (q_init) rbprmBuilder.setCurrentConfig (q_init); r (q_init)
...@@ -66,13 +66,13 @@ pp = PathPlayer (rbprmBuilder.client.basic, r) ...@@ -66,13 +66,13 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (2) #~ pp (2)
#~ pp (0) #~ pp (0)
pp (0) #~ pp (0)
pp (1) #~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") #~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~ r (q_init) #~ r (q_init)
rob = rbprmBuilder.client.basic.robot rob = rbprmBuilder.client.basic.robot
rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "truck_hrp2_path_not_robust.txt")
#~ configs = [] #~ configs = []
#~ problem = ps.client.problem #~ problem = ps.client.problem
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment