implementing sliding window of any number of states forward

script/scenarios/ground_crouch_hyq_interp.py

@@ -7,8 +7,8 @@ from os import environ
 ins_dir = environ['DEVEL_DIR']
 db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
 
-#~ import ground_crouch_hyq_path as tp
-import ground_crouch_hyq_path_bridge as tp
+import ground_crouch_hyq_path as tp
+#~ import ground_crouch_hyq_path_bridge as tp
 
 packageName = "hyq_description"
 meshPackageName = "hyq_description"
@@ -97,7 +97,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
 
 r(q_init)
 
-configs = fullBody.interpolate(0.1,1,10, False) #hole 
+configs = fullBody.interpolate(0.1,1,10, True) #hole 
 #~ configs = fullBody.interpolate(0.08,1,5) # bridge
 
 r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
@@ -120,14 +120,16 @@ limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector'
 
 
 
-def act(i, numOptim = 0):
-	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.6, optim_effectors = True, time_scale = 20., useCOMConstraints = False)
+def act(i, numOptim = 0, use_window = False, verbose = False, draw = False):
+	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = True, time_scale = 20., useCOMConstraints = False, use_window = use_window,
+	verbose = verbose, draw = draw)
 
 def play(frame_rate = 1./24.):
 	play_traj(fullBody,pp,frame_rate)
 	
 def saveAll(name):
 	saveAllData(fullBody, r, name)
-#~ fullBody.exportAll(r, trajec, 'obstacle_hyq_t_var_04f_andrea');
+#~ fullBody.exportAll(r, trajec, 'hole_hyq_t_var_04f_andrea');
+#~ fullBody.exportAll(r, configs, 'obstacle_hyq_t_var_04f_contact_planning');
 #~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
 

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

@@ -93,21 +93,23 @@ def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, li
 	return p, N, init_com, end_com, t_end_phases, cones, COMConstraints
 
 def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, phase_dt = [0.4, 1],
-reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False, use_window = False):
-	use_window = use_window and state_id + 2 < len(states) - 1 # can't use preview if last state is reached
-	assert( len(phase_dt) == 4 or not use_window ), "phase_dt does not describe all phases"
+reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):
+	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']
 	param_constraints = []	
 	mass = fullBody.getMass()
 	
 	p, N, init_com, end_com, t_end_phases, cones, COMConstraints = compute_state_info(fullBody,states, state_id, phase_dt[:2], mu, computeCones, limbsCOMConstraints)
-	if(use_window):
+	if(use_window > 0):
+		init_waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
+	for w in range(1,use_window+1):
 		waypoint = end_com[:]
 		waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
 		print "waypoint_time", waypoint_time
-		param_constraints = [("waypoint_reached_constraint",(waypoint_time, waypoint))]
-		p1, N1, init_com1, end_com1, t_end_phases1, cones1, COMConstraints1 = compute_state_info(fullBody,states, state_id+1, phase_dt[2:], mu, computeCones, limbsCOMConstraints)
+		param_constraints += [("waypoint_reached_constraint",(waypoint_time, waypoint))]
+		p1, N1, init_com1, end_com1, t_end_phases1, cones1, COMConstraints1 = compute_state_info(fullBody,states, state_id+w, phase_dt[2*w:], mu, computeCones, limbsCOMConstraints)
 		p+=p1;
 		N+=N1;
 		end_com = end_com1;
@@ -129,19 +131,14 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
 	if (profile):
 		end = time.clock() 
 		timeelapsed = (end - start) * 1000
-	if(use_window):
-		print "wtf", var_final['c']
-		var_final['c'] = var_final['c'][:waypoint_time+1]
-		print "wtf2", var_final['c']
-		print "t_init_phases", params["t_init_phases"]
-		params["t_init_phases"] = params["t_init_phases"][:-3]
-		print "t_init_phases", params["t_init_phases"]		
+	if(use_window > 0):
+		var_final['c'] = var_final['c'][:init_waypoint_time+1]
+		params["t_init_phases"] = params["t_init_phases"][:-3*use_window]
 		global lastspeed
-		lastspeed = var_final['dc'][waypoint_time]
-		print "lastspeed", lastspeed
+		lastspeed = var_final['dc'][init_waypoint_time]
 	return var_final, params, timeelapsed
 
-def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  dt=0.2, phase_dt = [0.4, 1], reduce_ineq = True, verbose = False, limbsCOMConstraints = None, use_window = False):
+def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  dt=0.2, phase_dt = [0.4, 1], reduce_ineq = True, verbose = False, limbsCOMConstraints = None, use_window = 0):
 	var_final, params, elapsed = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, False, use_window = use_window)
 	p, N = fullBody.computeContactPoints(state_id)
 	from mpl_toolkits.mplot3d import Axes3D
@@ -176,7 +173,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  d
 	ax = fig.add_subplot(111)
 	points = var_final['dc']
 	#~ print "points", points
-	ys = [norm(el) for el in points]
+	ys = [el for el in points]
 	xs = [i * params['dt'] for i in range(0,len(points))]
 	ax.scatter(xs, ys, c='b')
 
@@ -187,7 +184,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  d
 	fig = plt.figure()
 	ax = fig.add_subplot(111)
 	points = var_final['ddc']
-	ys = [norm(el) for el in points]
+	ys = [el for el in points]
 	xs = [i * params['dt'] for i in range(0,len(points))]
 	ax.scatter(xs, ys, c='b')
 
@@ -199,7 +196,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  d
 	return var_final, params, elapsed
 	
 def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1], reduce_ineq = True,
- num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = False):
+ num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):
 	print "callgin gen ",state_id
 	if(draw):
 		res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, use_window)		
@@ -221,7 +218,7 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
 	return comPosPerPhase, res[2] #res[2] is timeelapsed
 
 def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1],
-reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = False):
+reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):
 	comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
 	reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile)
 	print "done. generating state trajectory ",state_id	
@@ -231,7 +228,7 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
 	return paths_ids[-1], paths_ids[:-1], timeElapsed
 	
 def solve_effector_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1],
-reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = False):
+reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):
 	comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
 	reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window)
 	print "done. generating state trajectory ",state_id	

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

@@ -129,16 +129,18 @@ 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 = False, verbose = False, draw = False):
+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):
 	global errorid
 	global stat_data	
 	fail = 0
-	#~ try:
-	if(True):
-		times, dt, distance = __getTimes(fullBody, configs, i, time_scale)
-		use_window = use_window and i + 2 < len(configs) - 1 # can't use preview if last state is reached	
-		if(use_window):
-			times2, dt2, dist2 = __getTimes(fullBody, configs, i+1, time_scale)
+	try:
+	#~ if(True):
+		times = [];
+		dt = 1000;
+		distance = __getTimes(fullBody, configs, i, time_scale)
+		use_window = max(0, min(use_window,  (len(configs) - 1) - (i + 2))) # can't use preview if last state is reached
+		for w in range(use_window+1):
+			times2, dt2, dist2 = __getTimes(fullBody, configs, i+w, time_scale)
 			times += times2
 			dt = min(dt, dt2)
 		print 'time per path', times
@@ -158,11 +160,16 @@ def step(fullBody, configs, i, optim, pp, limbsCOMConstraints,  friction = 0.5,
 			global res
 			res = res + [pid]
 			global trajec
-			global trajec_mil
-			new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, times)
-			new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, times, 1./1000.)
-			new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, 1./1000.)	
-			Ps, Ns = genPandNperFrame(fullBody, i, pp, trajectory, times, 1./1000.)
+			global trajec_mil			
+			frame_rate = 1./24.
+			frame_rate_andrea = 1./1000.
+			if(len(trajec) > 0):
+				frame_rate = 1./25.
+				frame_rate_andrea = 1./1001.
+			new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, times, frame_rate)
+			new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, times,frame_rate_andrea)
+			new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)	
+			Ps, Ns = genPandNperFrame(fullBody, i, pp, trajectory, times, frame_rate_andrea)
 			if(len(trajec) > 0):
 				new_traj = new_traj[1:]
 				new_traj_andrea = new_traj_andrea[1:]