updated demos

script/scenarios/darpa_hyq_interp.py

@@ -58,27 +58,27 @@ lLegId = 'lhleg'
 rarmId = 'rhleg'
 larmId = 'lfleg'
 
-addLimbDb(rLegId, "static")
-addLimbDb(lLegId, "static")
-addLimbDb(rarmId, "static")
-addLimbDb(larmId, "static")
+#~ addLimbDb(rLegId, "static")
+#~ addLimbDb(lLegId, "static")
+#~ addLimbDb(rarmId, "static")
+#~ addLimbDb(larmId, "static")
 
-#~ fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.1, cType)
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.1, cType)
 
 lLegId = 'lhleg'
 lLeg = 'lh_haa_joint'
 lfoot = 'lh_foot_joint'
-#~ fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
 #~ 
 rarmId = 'rhleg'
 rarm = 'rh_haa_joint'
 rHand = 'rh_foot_joint'
-#~ fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
-
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
+#~ 
 larmId = 'lfleg'
 larm = 'lf_haa_joint'
 lHand = 'lf_foot_joint'
-#~ fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
 
 q_0 = fullBody.getCurrentConfig(); 
 q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
@@ -198,17 +198,23 @@ def go(dt_framerate=1./24.):
 #~ for i in range(24,26):
 	#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
 	#~ saveAll("test"+str(i));
-for i in range(16,28):
-	act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=False, draw=False);go()
+for i in range(14,19):
+	act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=True, draw=False);go()
+	saveAll("test"+str(i));
+for i in range(19,22):
+	act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=True, draw=False);go()
+	saveAll("test"+str(i));
+for i in range(22,28):
+	act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=True, draw=False);go()
 	saveAll("test"+str(i));
 for i in range(28,30):
-	act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=False, draw=False);go()
+	act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=True, draw=False);go()
 	saveAll("test"+str(i));
 for i in range(30,42):
-	act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
+	act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=True, draw=False);go()
 	saveAll("test"+str(i));
 for i in range(32,83):
-	act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=False, draw=False);go()
+	act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=True, draw=False);go()
 	saveAll("test"+str(i));
 	
 	

script/scenarios/demos/ground_crouch_hyq_interp.py

@@ -172,19 +172,7 @@ def contactPlan():
 	tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
 	for i in range(1,len(configs)):
 		r(configs[i]);
-		time.sleep(0.5)		
-		
-def interpolate():
-	tp.cl.problem.selectProblem("default")
-	r.client.gui.setVisibility("hyq", "ON")
-	tp.r.client.gui.setVisibility("toto", "OFF")
-	tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
-	for i in range(7,20):
-		act(i,1,optim_effectors=True)
-		
-def play(frame_rate = 1./24.):
-	play_traj(fullBody,pp,frame_rate)
-	
+		time.sleep(0.5)			
 
 		
 def a():
@@ -207,13 +195,5 @@ def e():
 	print "displaying contact plan"
 	contactPlan()
 	
-def f():
-	print "computing feasible com trajectory"
-	interpolate()
-
-def g():
-	print "playing feasible trajectory"
-	play()
-
 print "Root path generated in " + str(tp.t) + " ms."
 	

script/scenarios/polaris/poralis_hrp2_interp.py

@@ -43,7 +43,7 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "man
 rarmId = 'hrp2_rarm_rom'
 rarm = 'RARM_JOINT0'
 rHand = 'RARM_JOINT5'
-rArmOffset = [-0.05,-0.050,-0.050]
+rArmOffset = [-0.040,-0.01,-0.085]
 rArmNormal = [1,0,0]
 rArmx = 0.024; rArmy = 0.024
 fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
@@ -51,7 +51,7 @@ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "
 larmId = 'hrp2_larm_rom'
 larm = 'LARM_JOINT0'
 lHand = 'LARM_JOINT5'
-lArmOffset = [-0.05,-0.050,-0.050]
+lArmOffset = [-0.040,0.01,-0.085]
 lArmNormal = [1,0,0]
 lArmx = 0.024; lArmy = 0.024
 fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
@@ -158,5 +158,5 @@ def saveAll(name):
 	saveAllData(fullBody, r, name)
 
 
-for i in range(10,25):  act(i,numOptim = 60, use_window = 0, friction = 1, optim_effectors = True, verbose = False, draw = False);
-saveAll('polarisTestAndrea')
+#~ for i in range(10,25):  act(i,numOptim = 60, use_window = 0, friction = 1, optim_effectors = True, verbose = False, draw = False);
+#~ saveAll('polarisTestAndrea')

script/scenarios/polaris/poralis_hrp2_interp_2.py

@@ -43,7 +43,7 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "man
 rarmId = 'hrp2_rarm_rom'
 rarm = 'RARM_JOINT0'
 rHand = 'RARM_JOINT5'
-rArmOffset = [-0.05,-0.050,-0.050]
+rArmOffset = [-0.045,-0.01,-0.085]
 rArmNormal = [1,0,0]
 rArmx = 0.024; rArmy = 0.024
 #~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
@@ -51,7 +51,7 @@ rArmx = 0.024; rArmy = 0.024
 larmId = 'hrp2_larm_rom'
 larm = 'LARM_JOINT0'
 lHand = 'LARM_JOINT5'
-lArmOffset = [-0.05,-0.050,-0.050]
+lArmOffset = [-0.045,0.01,-0.085]
 lArmNormal = [1,0,0]
 lArmx = 0.024; lArmy = 0.024
 fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", True)

script/scenarios/polaris/poralis_hrp2_interp_3.py

@@ -43,7 +43,7 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "sta
 rarmId = 'hrp2_rarm_rom'
 rarm = 'RARM_JOINT0'
 rHand = 'RARM_JOINT5'
-rArmOffset = [-0.05,-0.050,-0.050]
+rArmOffset = [-0.045,-0.01,-0.085]
 rArmNormal = [1,0,0]
 rArmx = 0.024; rArmy = 0.024
 #~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
@@ -51,10 +51,10 @@ rArmx = 0.024; rArmy = 0.024
 larmId = 'hrp2_larm_rom'
 larm = 'LARM_JOINT0'
 lHand = 'LARM_JOINT5'
-lArmOffset = [-0.05,-0.050,-0.050]
+lArmOffset = [-0.045,0.01,-0.085]
 lArmNormal = [1,0,0]
 lArmx = 0.024; lArmy = 0.024
-fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "static", 0.05, "_6_DOF", True)
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
 
  #~ 
 fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)

script/scenarios/polaris/poralis_hrp2_interp_4.py

@@ -43,7 +43,7 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "sta
 rarmId = 'hrp2_rarm_rom'
 rarm = 'RARM_JOINT0'
 rHand = 'RARM_JOINT5'
-rArmOffset = [-0.05,-0.050,-0.050]
+rArmOffset = [-0.045,-0.01,-0.085]
 rArmNormal = [1,0,0]
 rArmx = 0.024; rArmy = 0.024
 #~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
@@ -51,10 +51,10 @@ rArmx = 0.024; rArmy = 0.024
 larmId = 'hrp2_larm_rom'
 larm = 'LARM_JOINT0'
 lHand = 'LARM_JOINT5'
-lArmOffset = [-0.05,-0.050,-0.050]
+lArmOffset = [-0.045,0.01,-0.085]
 lArmNormal = [1,0,0]
 lArmx = 0.024; lArmy = 0.024
-#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "static", 0.05, "_6_DOF", True)
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
 
  #~ 
 fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)

src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py

@@ -126,8 +126,8 @@ def __getTimes(fullBody, configs, i, time_scale):
 
 from hpp import Error as hpperr
 import sys, time
-def step(fullBody, configs, i, optim, pp, limbsCOMConstraints,  friction = 0.5, optim_effectors = True, time_scale = 20., useCOMConstraints = False, use_window = 0, verbose = False, draw = False,
-trackedEffectors = []):
+def step(fullBody, configs, i, optim, pp, limbsCOMConstraints,  friction = 0.5, optim_effectors = True, time_scale = 20., 
+useCOMConstraints = False, use_window = 0, verbose = False, draw = False,trackedEffectors = [], saveForSim=False,):
 	global errorid
 	global stat_data	
 	fail = 0
@@ -161,29 +161,29 @@ trackedEffectors = []):
 			global trajec
 			global trajec_mil			
 			frame_rate = 1./24.
-			frame_rate_andrea = 1./1000.
-			#~ frame_rate_andrea = 1./10.
 			new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path, frame_rate)
-			new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea)
-			Ps, Ns, freeEffectorsPerPhase, Ks = genPandNandConesperFrame(fullBody, i, limbsCOMConstraints, cones, pp, trajectory, time_per_path, frame_rate_andrea)
-			NPeffs = genPEffperFrame(fullBody, freeEffectorsPerPhase, new_traj_andrea, pp, time_per_path, frame_rate_andrea)
-			com = genComPerFrame(final_state, dt, frame_rate_andrea)
 			trajec = trajec + new_traj
-			trajec_mil += new_traj_andrea
-			#~ global contacts
-			#~ contacts += new_contacts	
-			global cones_saved
-			cones_saved += Ks
-			global pos
-			pos += Ps
-			global normals
-			normals+= Ns
-			global pEffs
-			pEffs+= NPeffs
-			global coms
-			coms+= com
-			print len(trajec_mil), " ",  len(pos), " ", len(normals), " ", len(coms), " ", len(pEffs), " ", len(cones_saved)
-			assert(len(trajec_mil) == len(pos) and len(normals) == len(pos) and len(normals) == len(coms) and len(cones_saved) == len(coms) and len(coms) == len(pEffs))
+			if(saveForSim):
+				frame_rate_sim = 1./1000.
+				new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_sim)
+				Ps, Ns, freeEffectorsPerPhase, Ks = genPandNandConesperFrame(fullBody, i, limbsCOMConstraints, cones, pp, trajectory, time_per_path, frame_rate_andrea)
+				NPeffs = genPEffperFrame(fullBody, freeEffectorsPerPhase, new_traj_andrea, pp, time_per_path, frame_rate_andrea)
+				com = genComPerFrame(final_state, dt, frame_rate_andrea)
+				trajec_mil += new_traj_andrea
+				#~ global contacts
+				#~ contacts += new_contacts	
+				global cones_saved
+				cones_saved += Ks
+				global pos
+				pos += Ps
+				global normals
+				normals+= Ns
+				global pEffs
+				pEffs+= NPeffs
+				global coms
+				coms+= com
+				print len(trajec_mil), " ",  len(pos), " ", len(normals), " ", len(coms), " ", len(pEffs), " ", len(cones_saved)
+				assert(len(trajec_mil) == len(pos) and len(normals) == len(pos) and len(normals) == len(coms) and len(cones_saved) == len(coms) and len(coms) == len(pEffs))
 			stat_data["num_success"] += 1
 		else:
 			print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"