Commit 5a3c5f97 authored by stevet's avatar stevet
Browse files

working darpa

parent 8d227e78
...@@ -32,7 +32,7 @@ fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName ...@@ -32,7 +32,7 @@ fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName
fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4]) fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
# Setting a number of sample configurations used # Setting a number of sample configurations used
nbSamples = 20000 nbSamples = 10000
ps = tp.ProblemSolver(fullBody) ps = tp.ProblemSolver(fullBody)
r = tp.Viewer (ps, viewerClient=tp.r.client) r = tp.Viewer (ps, viewerClient=tp.r.client)
...@@ -174,7 +174,7 @@ def genPlan(stepsize=0.06): ...@@ -174,7 +174,7 @@ def genPlan(stepsize=0.06):
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF") tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
global configs global configs
start = time.clock() start = time.clock()
configs = fullBody.interpolate(stepsize, 5, 20, filterStates = True, testReachability=False, quasiStatic=True) configs = fullBody.interpolate(stepsize, 8, 0, filterStates = True, testReachability=False, quasiStatic=True)
end = time.clock() end = time.clock()
print "Contact plan generated in " + str(end-start) + "seconds" print "Contact plan generated in " + str(end-start) + "seconds"
...@@ -228,7 +228,7 @@ def f(step = 0.5): ...@@ -228,7 +228,7 @@ def f(step = 0.5):
print "Root path generated in " + str(tp.t) + " ms." print "Root path generated in " + str(tp.t) + " ms."
#~ d();e() #~ d();e()
d(0.1);e(0.01) d(0.01);e(0.01)
#~ d(0.004);e(0.01) #~ d(0.004);e(0.01)
from hpp.corbaserver.rbprm.rbprmstate import * from hpp.corbaserver.rbprm.rbprmstate import *
......
...@@ -26,7 +26,7 @@ rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',]) ...@@ -26,7 +26,7 @@ rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support']) rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',]) rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
# We also bound the rotations of the torso. # We also bound the rotations of the torso.
#~ rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3]) rbprmBuilder.boundSO3([-0.4,0.4,-0.3,0.3,-0.3,0.3])
# Creating an instance of HPP problem solver and the viewer # Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
...@@ -37,8 +37,8 @@ r = Viewer (ps) ...@@ -37,8 +37,8 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig (); q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::] q_goal = q_init [::]
#~ q_goal [0:3] = [3, 0, 0.63]; r (q_goal) q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
q_goal [0:3] = [-1, 0, 0.75]; r (q_goal) #~ q_goal [0:3] = [-1, 0, 0.75]; r (q_goal)
# Choosing a path optimizer # Choosing a path optimizer
ps.addPathOptimizer("RandomShortcut") ps.addPathOptimizer("RandomShortcut")
...@@ -66,7 +66,7 @@ print "computation time for root path ", t ...@@ -66,7 +66,7 @@ print "computation time for root path ", t
# Playing the computed path # Playing the computed path
from hpp.gepetto import PathPlayer from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r) pp = PathPlayer (r)
q_far = q_init [::] q_far = q_init [::]
q_far [0:3] = [-2, -3, 0.63]; q_far [0:3] = [-2, -3, 0.63];
...@@ -74,6 +74,8 @@ r(q_far) ...@@ -74,6 +74,8 @@ r(q_far)
for i in range(1,10): for i in range(1,10):
rbprmBuilder.client.basic.problem.optimizePath(i) rbprmBuilder.client.basic.problem.optimizePath(i)
#~ pp(9)
from hpp.corbaserver import Client from hpp.corbaserver import Client
...@@ -99,7 +101,7 @@ cl.problem.selectProblem("rbprm_path") ...@@ -99,7 +101,7 @@ cl.problem.selectProblem("rbprm_path")
rbprmBuilder2 = Robot ("toto") rbprmBuilder2 = Robot ("toto")
ps2 = ProblemSolver( rbprmBuilder2 ) ps2 = ProblemSolver( rbprmBuilder2 )
cl.problem.selectProblem("default") cl.problem.selectProblem("default")
cl.problem.movePathToProblem(3,"rbprm_path",rbprmBuilder.getAllJointNames()) cl.problem.movePathToProblem(8,"rbprm_path",rbprmBuilder.getAllJointNames())
r2 = Viewer (ps2, viewerClient=r.client) r2 = Viewer (ps2, viewerClient=r.client)
r2(q_far) r2(q_far)
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