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

working darpa

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