Commit bab0ab86 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[script] adapt hyq script to changes in robot-data

parent 2f707d70
......@@ -18,6 +18,8 @@ from hpp.corbaserver import Client
# This time we load the full body model of HyQ
fullBody = Robot ()
fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.client.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
# Setting a number of sample configurations used
nbSamples = 10000
......@@ -31,38 +33,20 @@ cType = "_6_DOF"
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.1, cType)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.1, cType)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
"""
q_init = [-2.0,
0.0,
0.6838277139631803,
0.0,
0.0,
0.0,
1.0,
0.14279812395541294,
0.934392553166556,
-0.9968239786882757,
-0.06521258938340457,
-0.8831796268418511,
1.150049183494211,
-0.06927610020154493,
0.9507443168724581,
-0.8739975339028809,
0.03995660287873871,
-0.9577096766517215,
0.9384602821326071]
"""
q_init=fullBody.referenceConfig[::]; q_init[0:7] = tp.q_init[0:7]; q_init[2]=fullBody.referenceConfig[2]
q_goal = fullBody.referenceConfig[::]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=fullBody.referenceConfig[2]
q_init=fullBody.referenceConfig[::] + [0]*6; q_init[0:7] = tp.q_init[0:7];
#q_init[2]=fullBody.referenceConfig[2]
q_goal = fullBody.referenceConfig[::]+ [0]*6; q_goal[0:7] = tp.q_goal[0:7];
#q_goal[2]=fullBody.referenceConfig[2]
q_init = fullBody.generateContacts(q_init, [0,0,1])
......@@ -73,6 +57,7 @@ fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,f
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId])
v(q_init)
configs = []
......@@ -117,9 +102,9 @@ def genPlan(stepsize=0.06):
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
global configs
start = time.clock()
configs = fullBody.interpolate(stepsize, 8, 0, filterStates = False, testReachability=False, quasiStatic=True)
end = time.clock()
start = time.time()
configs = fullBody.interpolate(stepsize, 5, 0., filterStates = True,testReachability = False)
end = time.time()
print "Contact plan generated in " + str(end-start) + "seconds"
def contactPlan(step = 0.5):
......
......@@ -36,9 +36,9 @@ v = vf.createViewer()
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); v (q_init)
q_init [0:3] = [-2, 0, 0.64]; rbprmBuilder.setCurrentConfig (q_init); v (q_init)
q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.63]; v (q_goal)
q_goal [0:3] = [3, 0, 0.64]; v (q_goal)
#~ q_goal [0:3] = [-1.5, 0, 0.75]; r (q_goal)
# Choosing a path optimizer
......@@ -64,7 +64,7 @@ from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
q_far = q_init [::]
q_far [0:3] = [-2, -3, 0.63];
q_far [0:3] = [-2, -3, 0.6];
v(q_far)
for i in range(1,10):
......
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