Commit b28053d8 authored by stevet's avatar stevet
Browse files

cleaning darpa

parent 4437e7c0
...@@ -9,7 +9,7 @@ from hyq_ref_pose import hyq_ref ...@@ -9,7 +9,7 @@ from hyq_ref_pose import hyq_ref
import darpa_hyq_path as tp import darpa_hyq_path as tp
from os import environ from os import environ
ins_dir = environ['DEVEL_DIR'] ins_dir = environ['DEVEL_HPP_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_" db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
...@@ -42,7 +42,6 @@ cType = "_3_DOF" ...@@ -42,7 +42,6 @@ cType = "_3_DOF"
rLegId = 'rfleg' rLegId = 'rfleg'
rLeg = 'rf_haa_joint' rLeg = 'rf_haa_joint'
rfoot = 'rf_foot_joint' rfoot = 'rf_foot_joint'
#~ offset = [0.,-0.021,0.]
offset = [0.,-0.021,0.] offset = [0.,-0.021,0.]
normal = [0,1,0] normal = [0,1,0]
legx = 0.02; legy = 0.02 legx = 0.02; legy = 0.02
...@@ -72,11 +71,6 @@ fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "random ...@@ -72,11 +71,6 @@ fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "random
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True) #~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True) #~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
#~ q_init = hyq_ref[:]; q_init[0:7] = tp.q_init[0:7];
#~ q_goal = hyq_ref[:]; q_goal[0:7] = tp.q_goal[0:7];
q_init = hyq_ref[:]; q_init[0:7] = tp.q_init[0:7]; q_init[2]=hyq_ref[2]
q_goal = hyq_ref[:]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=hyq_ref[2]+0.02
q_init = [-2.0, q_init = [-2.0,
0.0, 0.0,
0.6838277139631803, 0.6838277139631803,
...@@ -96,15 +90,10 @@ q_init = [-2.0, ...@@ -96,15 +90,10 @@ q_init = [-2.0,
0.03995660287873871, 0.03995660287873871,
-0.9577096766517215, -0.9577096766517215,
0.9384602821326071] 0.9384602821326071]
q_goal = hyq_ref[:]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=hyq_ref[2]+0.02
# Randomly generating a contact configuration at q_init
#~ fullBody.setCurrentConfig (q_init)
#~ q_init = fullBody.generateContacts(q_init, [0,0,1])
# 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 # specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId]) fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
...@@ -169,90 +158,7 @@ def contactPlan(step = 0.5): ...@@ -169,90 +158,7 @@ def contactPlan(step = 0.5):
r(configs[i]); r(configs[i]);
time.sleep(step) time.sleep(step)
def contactPlanDontMove(step = 0.5):
r.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
for i in range(0,len(configs)):
a = configs[i]
a[:6] = [0 for _ in range(6)]
a[6] = 1
#~ r(configs[i]);
r(a);
time.sleep(step)
def a():
print "initial configuration"
initConfig()
def b():
print "end configuration"
endConfig()
def c():
print "displaying root path"
rootPath()
def d(step=0.06):
print "computing contact plan"
genPlan(step)
def e(step = 0.5):
print "displaying contact plan"
contactPlan(step)
def f(step = 0.5):
print "displaying static contact plan"
contactPlanDontMove(step)
print "Root path generated in " + str(tp.t) + " ms." print "Root path generated in " + str(tp.t) + " ms."
#~ d();e() genPlan(0.01);contactPlan(0.01)
d(0.01);e(0.01)
#~ d(0.004);e(0.01)
from hpp.corbaserver.rbprm.rbprmstate import *
com = fullBody.getCenterOfMass()
s = None
def d1():
global s
s = State(fullBody,q = q_init, limbsIncontact = [larmId])
a = s.q()
a[2]=a[2]+0.01
s.setQ(a)
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
def d2():
global s
s = State(fullBody,q = q_init, limbsIncontact = [larmId, rarmId])
a = s.q()
a[2]=a[2]+0.05
a[0]=a[0]+0.05
s.setQ(a)
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
def d3():
global s
s = State(fullBody,q = q_init, limbsIncontact = [rarmId])
a = s.q()
a[2]=a[2]+0.01
s.setQ(a)
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
def d4():
global s
s = State(fullBody,q = q_init, limbsIncontact = [rarmId])
a = s.q()
a[2]=a[2]-0.01
s.setQ(a)
print s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
s = State(fullBody,q = s.q(), limbsIncontact = [larmId])
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
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