working on effector trajhectories

script/scenarios/ground_crouch_hyq_interp.py

+import matplotlib
+#~ matplotlib.use('Agg')
+import matplotlib.pyplot as plt
 from hpp.corbaserver.rbprm.rbprmbuilder import Builder
 from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
 from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
@@ -147,32 +150,11 @@ from numpy import matlib
 #~ for i in range(6,25):
 	#~ act(i, 60, optim_effectors = True)
 
-#~ pid = act(8,0,optim_effectors = False)
-from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import res
-pid = res[0]-2 
-dt = 0.001
-
-#~ qs = [ pp.client.problem.configAtParam (pid, i*dt) for i in range(600,700)]
-#~ cs = []
-#~ res = zeros((3, 100))
-#~ for i in range(len(qs)):
-	#~ q = qs[i]
-	#~ fullBody.setCurrentConfig(q[:-1]) 
-	#~ c = fullBody.getCenterOfMass()
-	#~ cs += [c[0]]
-	#~ res[:,i] = c
-
-#~ for i in range(8,9): act(i,0,optim_effectors=False, draw=False)
-#~ ddc = [2 *(cs[i-1] - c[i])  for i in range(1, len(c))]
-#~ ddcx = [ddci[0] for ddci in ddc]
-
 
-#~ res = zeros((3, 100))
+for i in range(7,8):
+    act(i,0, use_window=0, verbose=True, optim_effectors=False, draw=False)
 
-#~ for i,ci in enumerate(c):
-	#~ res[:,i] = ci
-
-#~ m = matrix(res)
+#~ pid = act(8,0,optim_effectors = False)
 
 
 from derivative_filters import *
@@ -188,6 +170,7 @@ import plot_utils
 
 from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import trajec_mil
 from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import trajec
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import coms
 
 #~ plt.plot(x[0,:].A.squeeze())
 #~ plt.plot(dx[0,:].A.squeeze())

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

+import matplotlib
+#~ matplotlib.use('Agg')
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
 from cwc import cone_optimization
 from obj_to_constraints import ineq_from_file, rotate_inequalities
 import numpy as np
@@ -155,9 +159,6 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = 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
-	import matplotlib.pyplot as plt
-	
 	fig = plt.figure()
 	ax = fig.add_subplot(111, projection='3d')
 	n = 100
@@ -179,14 +180,16 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  d
 	ax.set_ylabel('Y Label')
 	ax.set_zlabel('Z Label')
 
-	plt.show()
+	#~ plt.show()
+	plt.savefig('/tmp/c'+ str(state_id)+ '.png')
 	
 	print "plotting speed "
 	print "end target ",  params['x_end']
 	fig = plt.figure()
 	ax = fig.add_subplot(111)
 	if(use_window > 0):
-		points = var_final['dc_old']
+		#~ points = var_final['dc_old']
+		points = var_final['dc']
 	else:
 		points = var_final['dc']
 		
@@ -196,13 +199,15 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  d
 	ax.scatter(xs, ys, c='b')
 
 
-	plt.show()
+	#~ plt.show()
+	plt.savefig('/tmp/dc'+ str(state_id)+ '.png')
 	
 	print "plotting acceleration "
 	fig = plt.figure()
 	ax = fig.add_subplot(111)
 	if(use_window > 0):
-		points = var_final['ddc_old']
+		#~ points = var_final['ddc_old']
+		points = var_final['ddc']
 	else:
 		points = var_final['ddc']
 	ys = [norm(el) * el[0] / abs(el[0]+ _EPS) for el in points]
@@ -210,7 +215,8 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  d
 	ax.scatter(xs, ys, c='b')
 
 
-	plt.show()
+		#~ plt.show()
+	plt.savefig('/tmp/ddc'+ str(state_id)+ '.png')
 	
 	print "plotting Dl "
 	fig = plt.figure()
@@ -221,7 +227,8 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  d
 	ax.scatter(xs, ys, c='b')
 
 
-	plt.show()
+	#~ plt.show()
+	plt.savefig('/tmp/dL'+ str(state_id)+ '.png')
 	return var_final, params, elapsed
 	
 def __cVarPerPhase(var, dt, t, final_state, addValue):

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

@@ -74,11 +74,15 @@ def genPEffperFrame(fullBody, freeEffectorsPerPhase, qs, pp, times, dt_framerate
 
 def genComPerFrame(final_state, dt, dt_framerate = 1./1000.):
 	num_frames_per_dt = int(round(dt / dt_framerate))
+	inc = 1./((float)(num_frames_per_dt))
 	c =   [array(final_state['x_init'][:3])] + final_state['c']
 	dc =   [array(final_state['x_init'][3:])] + final_state['dc']
 	ddc = final_state['ddc']
 	cs = []
-	[cs.append(c[i] + ddt*dt *dc[i] + ddt*dt *ddt* dt * 0.5 * ddc[i]) for ddt in range(num_frames_per_dt) for i in range(0,len(ddc))]
+	for i in range(0,len(c)-1):
+		for j in range(num_frames_per_dt):
+			ddt = j * inc * dt
+			cs.append(c[i] + ddt *dc[i] + ddt *ddt * 0.5 * ddc[i])
 	return cs
 
 stat_data = { 
@@ -164,14 +168,13 @@ trackedEffectors = []):
 			Ps, Ns, freeEffectorsPerPhase = genPandNperFrame(fullBody, i, limbsCOMConstraints, 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)
-			if(len(trajec) > 0):
-				new_traj = new_traj[1:]
-				new_traj_andrea = new_traj_andrea[1:]
-				#~ new_contacts = new_contacts[1:]
-				Ps = Ps[1:]
-				Ns = Ns[1:]
-				com = com[1:]
-				pEffs = pEffs[1:]
+			#~ if(len(trajec) > 0):
+				#~ new_traj = new_traj[1:]
+				#~ new_traj_andrea = new_traj_andrea[1:]
+				#~ Ps = Ps[1:]
+				#~ Ns = Ns[1:]
+				#~ com = com[1:]
+				#~ NPeffs = NPeffs[1:]
 			trajec = trajec + new_traj
 			trajec_mil += new_traj_andrea
 			#~ global contacts

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

@@ -66,10 +66,8 @@ def gen_trajectory_to_play(robot, path_player, path_ids, total_time_per_paths, d
 		config_size_path = len(path_player.client.problem.configAtParam (path_id, 0))
 		if(config_size_path > config_size):
 		#~ if(i == 1 ):
-			print "a traj"
 			res+= follow_trajectory_path(robot, path_player, path_id, total_time_per_paths[i], dt_framerate)
 		else:
-			print "a path"
 			res+= linear_interpolate_path(robot, path_player, path_id, total_time_per_paths[i], dt_framerate)
 		activeid +=1
 	return res