add debug messages in cwc_trajectory

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

@@ -185,13 +185,15 @@ trackedEffectors = [],use_velocity=False,pathId = 0):
 			res = res + [pid]
 			global trajec
 			global trajec_mil			
-			frame_rate = 1./24.
+			frame_rate = 0.01
 			frame_rate_andrea = 1./100.
 #			frame_rate_andrea = 1./1000.
 			#~ if(len(trajec) > 0):
 				#~ frame_rate = 1./25.
 				#~ frame_rate_andrea = 1./1001.
+			print "first traj :"
 			new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path, frame_rate)
+			print "traj Andrea : "
 			new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea)
 			#~ new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)	
 			Ps, Ns, freeEffectorsPerPhase = genPandNperFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, time_per_path, frame_rate_andrea)

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

@@ -31,6 +31,7 @@ def __find_q_t(robot, path_player, path_id, t):
 		if(a >= b):
 			print "ERROR, a > b, t does not exist"
 		if abs(current_t - t) < __EPS:
+			print "last config q = ",q[-1]
 			return q[:-1]
 		elif(current_t - t) < __EPS:
 			a = u
@@ -49,6 +50,7 @@ def linear_interpolate_path(robot, path_player, path_id, total_time, dt_framerat
 	
 def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate=__24fps):
 	pp = path_player
+	print "total_times in follow path : ",total_time
 	length = pp.client.problem.pathLength (path_id)
 	num_frames_required = total_time / dt_framerate
 	dt = float(length) / num_frames_required
@@ -58,10 +60,12 @@ def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate
 	return[__find_q_t(robot, path_player, path_id, t) for t in dt_finals]
 	
 def gen_trajectory_to_play(robot, path_player, path_ids, total_time_per_paths, dt_framerate=__24fps):
+	print "gen_trajectory : dt = ",dt_framerate
 	config_size = len(robot.getCurrentConfig())
 	res = []
 	pp = path_player
 	activeid = 0
+	print "path_ids = ", path_ids
 	for i, path_id in enumerate(path_ids):
 		config_size_path = len(path_player.client.problem.configAtParam (path_id, 0))
 		if(config_size_path > config_size):