From b28053d850bca5e7a2e7c354e83aa7f17bf9e0de Mon Sep 17 00:00:00 2001
From: stevet <stevetonneau@gotmail.fr>
Date: Mon, 14 Jan 2019 11:52:46 +0000
Subject: [PATCH] cleaning darpa
---
script/scenarios/demos/darpa_hyq.py | 102 ++--------------------------
1 file changed, 4 insertions(+), 98 deletions(-)
diff --git a/script/scenarios/demos/darpa_hyq.py b/script/scenarios/demos/darpa_hyq.py
index cd96ad6..2b4b312 100644
--- a/script/scenarios/demos/darpa_hyq.py
+++ b/script/scenarios/demos/darpa_hyq.py
@@ -9,7 +9,7 @@ from hyq_ref_pose import hyq_ref
import darpa_hyq_path as tp
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_"
@@ -42,7 +42,6 @@ cType = "_3_DOF"
rLegId = 'rfleg'
rLeg = 'rf_haa_joint'
rfoot = 'rf_foot_joint'
-#~ offset = [0.,-0.021,0.]
offset = [0.,-0.021,0.]
normal = [0,1,0]
legx = 0.02; legy = 0.02
@@ -72,11 +71,6 @@ fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "random
#~ fullBody.runLimbSampleAnalysis(rarmId, "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,
0.0,
0.6838277139631803,
@@ -96,15 +90,10 @@ q_init = [-2.0,
0.03995660287873871,
-0.9577096766517215,
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
fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
@@ -169,90 +158,7 @@ def contactPlan(step = 0.5):
r(configs[i]);
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."
-#~ d();e()
-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)
-
-
+genPlan(0.01);contactPlan(0.01)
--
GitLab