From f6e5659cfe0a0d32ab1d9543fefb3cde13537fad Mon Sep 17 00:00:00 2001 From: Pierre Fernbach <pierre.fernbach@laas.fr> Date: Tue, 21 Feb 2017 16:18:46 +0100 Subject: [PATCH] add debug messages in cwc_trajectory --- src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py | 4 +++- src/hpp/corbaserver/rbprm/tools/path_to_trajectory.py | 4 ++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py index e23c72ae..3983bd59 100644 --- a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py +++ b/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) diff --git a/src/hpp/corbaserver/rbprm/tools/path_to_trajectory.py b/src/hpp/corbaserver/rbprm/tools/path_to_trajectory.py index 36d4ce0f..eaa738a8 100644 --- a/src/hpp/corbaserver/rbprm/tools/path_to_trajectory.py +++ b/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): -- GitLab