cleaning darpa

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)