trying to make darpa scenario work

script/scenarios/darpa_hyq_interp.py

@@ -99,9 +99,11 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
 
 r(q_init)
 # computing the contact sequence
-configs = fullBody.interpolate(0.12, 10, 10, True)
+#~ configs = fullBody.interpolate(0.12, 10, 5, True)
+#~ configs = fullBody.interpolate(0.08, 10, 10, True)
 #~ configs = fullBody.interpolate(0.11, 7, 10, True)
-#~ configs = fullBody.interpolate(0.1, 1, 5, True)
+#~ configs = fullBody.interpolate(0.1, 1, 10, True)
+configs = fullBody.interpolate(0.04, 1, 10, True)
 
 #~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
 
@@ -139,3 +141,74 @@ def saveAll(name):
 
 #~ fullBody.exportAll(r, trajec, 'darpa_hyq_t_var_04f_andrea');
 #~ saveToPinocchio('darpa_hyq_t_var_04f_andrea')
+
+
+gui = r.client.gui
+scene = "oddct"
+r.client.gui.createScene(scene)
+resolution = 0.03
+i = 0
+boxname = scene+"/b"+str(i)
+gui.addBox(boxname,resolution,resolution,resolution, [1,1,1,1])
+gui.applyConfiguration(boxname,[0,0,0,1,0,0,0])
+gui.addSceneToWindow(scene,0)
+gui.refresh()
+
+import hpp.corbaserver.rbprm.tools.cwc_trajectory_helper as cwc_trajectory_helper
+import time
+
+def applycom():
+	global gui
+	global com
+	c = fullBody.getCenterOfMass()
+	gui.applyConfiguration(boxname,[c[0],c[1],0,1,0,0,0])
+	gui.refresh()
+
+def go(dt_framerate=1./24.):
+	path_player = pp
+	configs = cwc_trajectory_helper.trajec
+	for q in configs:
+		start = time.time()
+		pp.publisher.robotConfig = q
+		pp.publisher.publishRobots ()
+		elapsed = time.time() - start  
+		applycom()
+		if elapsed < dt_framerate :
+			time.sleep(dt_framerate-elapsed)
+
+
+#~ for i in range(9,14):
+	#~ act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=False, draw=False);go()
+	#~ saveAll("test"+str(i));
+#~ for i in range(14,16):
+	#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
+	#~ saveAll("test"+str(i));
+#~ for i in range(16,17):
+	#~ act(i,verbose=True, use_window=2, numOptim=5, optim_effectors=False, draw=False);go()
+	#~ saveAll("test"+str(i));
+#~ for i in range(17,19):
+	#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
+	#~ saveAll("test"+str(i));
+#~ for i in range(19,22):
+	#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
+	#~ saveAll("test"+str(i));
+#~ for i in range(23,24):
+	#~ act(i,verbose=True, use_window=2, numOptim=5, optim_effectors=False, draw=False);go()
+	#~ saveAll("test"+str(i));
+#~ 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()
+	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()
+	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()
+	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()
+	saveAll("test"+str(i));
+	
+	

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

@@ -96,7 +96,9 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
 	use_window = max(0, min(use_window,  (len(states) - 1) - (state_id + 2))) # can't use preview if last state is reached	
 	assert( len(phase_dt) >= 2 +  use_window * 2 ), "phase_dt does not describe all phases"
 	
-	constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
+	#~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
+	#~ constraints = ['cones_constraint', 'end_reached_constraint', 'com_kinematic_constraint']
+	constraints = ['cones_constraint', 'end_reached_constraint']
 	#~ constraints = ['end_reached_constraint']
 	#~ constraints = ['cones_constraint', 'end_reached_constraint']
 	#~ constraints = ['cones_constraint']
@@ -108,6 +110,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
 	if(use_window > 0):
 		init_waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
 		init_end_com = end_com[:]
+		constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
 	for w in range(1,use_window+1):
 		waypoint = end_com[:]
 		waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
@@ -164,7 +167,9 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
 				print "init speed", lastspeed
 			else:
 				raise ValueError("projection failed, this is bad")
-		lastspeed = np.array([0,0,0])
+		#~ lastspeed = np.array([0,0,0])
+		lastspeed = var_final['dc'][-1]	
+		print "end speed", lastspeed
 		
 	return var_final, params, timeelapsed, cones
 

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

@@ -162,6 +162,7 @@ trackedEffectors = []):
 			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)