new api for pinocchio trajectory export

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -228,6 +228,11 @@ module hpp
     /// \param contactLimbs ids of the limb in contact for the state
     void setStartState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
 
+    /// Compute effector contact points and normals for a given configuration
+    /// in local coordinates
+    /// \param dofArray configuration of the robot
+    /// \param limbName ids of the limb in contact
+    floatSeq computeContactForConfig(in floatSeq dofArray, in string limbName) raises (Error);
 
     /// Set the end state of a contact generation problem
     /// environment, ordered by their efficiency
@@ -241,11 +246,11 @@ module hpp
     /// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
     floatSeqSeq computeContactPoints(in unsigned short stateId) raises (Error);
 
-		/// Provided a discrete contact sequence has already been computed, computes
-		/// all the contact positions and normals for a given state, the next one, and the intermediate between them.
-		/// \param stateId normalized step for generation along the path (ie the path has a length of 1).
-		/// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
-		floatSeqSeq computeContactPointsForLimb(in unsigned short stateId, in string limbname) raises (Error);
+    /// Provided a discrete contact sequence has already been computed, computes
+    /// all the contact positions and normals for a given state, the next one, and the intermediate between them.
+    /// \param stateId normalized step for generation along the path (ie the path has a length of 1).
+    /// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
+    floatSeqSeq computeContactPointsForLimb(in unsigned short stateId, in string limbname) raises (Error);
 
     /// Provided a path has already been computed, interpolates it and generates the statically stable
     /// constact configurations along it. setStartState and setEndState must have been called prior

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -307,6 +307,16 @@ class FullBody (object):
     def computeContactPointsForLimb(self, stateId, limb):
 		rawdata = self.client.rbprm.rbprm.computeContactPointsForLimb(stateId, limb)
 		return [[b[i:i+3] for i in range(0, len(b), 6)] for b in rawdata], [[b[i+3:i+6] for i in range(0, len(b), 6)] for b in rawdata]
+	
+	## Compute effector contact points and normals for a given configuration
+    # in local coordinates
+    #
+    # \param q configuration of the robot
+    # \param limbName ids of the limb in contact
+    # \return list 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
+    def computeContactForConfig(q, limbName):
+		rawdata = self.computeContactPointsForLimb(stateId, limb)
+		return [[rawdata[i:i+3]] for i in range(0, len(rawdata), 6)], [[rawdata[i+3:i+6]] for i in range(0, len(rawdata), 6)]
 
 	## Provided a discrete contact sequence has already been computed, computes
     # all the contact positions and normals for a given state, the next one, and the intermediate between them.

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

@@ -129,13 +129,16 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
 		timeelapsed = (end - start) * 1000
 		#~ print "solving time", timeelapsed
 	if(use_window > 0):
+		var_final['c_old'] = var_final['c'][:]
+		var_final['dc_old'] = var_final['dc'][:]
+		var_final['ddc_old'] = var_final['ddc'][:]
 		var_final['c'] = var_final['c'][:init_waypoint_time+1]
 		var_final['dc'] = var_final['dc'][:init_waypoint_time+1]
 		var_final['ddc'] = var_final['ddc'][:init_waypoint_time+1]
 		params["t_init_phases"] = params["t_init_phases"][:-3*use_window]
 		print "trying to project on com (from, to) ", init_end_com, var_final['c'][-1]
 		if (fullBody.projectStateToCOM(state_id+1, (var_final['c'][-1]).tolist())):
-			print "PROJECTED", init_end_com, var_final['c'][-1]
+			#~ print "PROJECTED", init_end_com, var_final['c'][-1]
 			states[state_id+1] = fullBody.getConfigAtState(state_id+1) #updating config from python side)
 			lastspeed = var_final['dc'][init_waypoint_time]		
 			print "init speed", lastspeed
@@ -182,9 +185,13 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  d
 	print "end target ",  params['x_end']
 	fig = plt.figure()
 	ax = fig.add_subplot(111)
-	points = var_final['dc']
+	if(use_window > 0):
+		points = var_final['dc_old']
+	else:
+		points = var_final['dc']
+		
 	#~ print "points", points
-	ys = [norm(el) * el[0] / abs(el[0]) for el in points]
+	ys = [norm(el) * el[0] / abs(el[0]+ _EPS) for el in points]
 	xs = [i * params['dt'] for i in range(0,len(points))]
 	ax.scatter(xs, ys, c='b')
 
@@ -194,14 +201,25 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  d
 	print "plotting acceleration "
 	fig = plt.figure()
 	ax = fig.add_subplot(111)
-	points = var_final['ddc']
-	ys = [norm(el) * el[0] / abs(el[0]) for el in points]
+	if(use_window > 0):
+		points = var_final['ddc_old']
+	else:
+		points = var_final['ddc']
+	ys = [norm(el) * el[0] / abs(el[0]+ _EPS) for el in points]
 	xs = [i * params['dt'] for i in range(0,len(points))]
 	ax.scatter(xs, ys, c='b')
 
 
 	plt.show()
 	
+	print "plotting Dl "
+	fig = plt.figure()
+	ax = fig.add_subplot(111)
+	points = var_final['dL']
+	ys = [norm(el) * el[0] / abs(el[0]+ _EPS) for el in points]
+	xs = [i * params['dt'] for i in range(0,len(points))]
+	ax.scatter(xs, ys, c='b')
+
 
 	plt.show()
 	return var_final, params, elapsed
@@ -241,20 +259,20 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
 		
 	#now compute com trajectorirs
 	comTrajIds = [fullBody.generateComTraj(comPosPerPhase[i], comVelPerPhase[i], comAccPerPhase[i], dt) for i in range(0,3)]
-	return comTrajIds, res[2] #res[2] is timeelapsed
+	return comTrajIds, res[2], final_state #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 = 0, trackedEffectors = []):
-	comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
+	comPosPerPhase, timeElapsed, final_state = __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	
 	paths_ids = [int(el) for el in fullBody.comRRTFromPos(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
 	print "done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1]
-	return paths_ids[-1], paths_ids[:-1], timeElapsed
+	return paths_ids[-1], paths_ids[:-1], timeElapsed, final_state
 	
 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 = 0, trackedEffectors = []):
-	comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
+	comPosPerPhase, timeElapsed, final_state = __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		
 	if(len(trackedEffectors) == 0):
@@ -264,5 +282,5 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
 		refPathId = trackedEffectors[0]; path_start = trackedEffectors[1]; path_to  = trackedEffectors[2]; effectorstracked = trackedEffectors[3]
 		paths_ids = [int(el) for el in fullBody.effectorRRTFromPath(state_id, refPathId, path_start, path_to, comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims, effectorstracked)]
 	print "done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1]
-	return paths_ids[-1], paths_ids[:-1], timeElapsed
+	return paths_ids[-1], paths_ids[:-1], timeElapsed, final_state
 	

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

@@ -3,7 +3,7 @@ from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
 from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
 from cwc import OptimError, cone_optimization
 from hpp.corbaserver.rbprm.tools.path_to_trajectory import gen_trajectory_to_play
-from numpy import append
+from numpy import append, array
 
 #global variables
 res = []
@@ -12,6 +12,8 @@ trajec_mil = []
 #~ contacts = []
 pos = []
 normals = []
+pEffs = []
+coms = []
 errorid = []
 
 def displayComPath(pp, pathId,color=[0.,0.75,0.15,0.9]) :
@@ -29,43 +31,10 @@ def displayComPath(pp, pathId,color=[0.,0.75,0.15,0.9]) :
 	pp.publisher.client.gui.addToGroup(nameCurve,pp.publisher.sceneName)
 	pp.publisher.client.gui.refresh()
 
-#~ 
-#~ def getContactPerPhase(fullBody, stateid, limbsCOMConstraints):
-	#~ contacts = [[],[],[]]
-	#~ for k, v in limbsCOMConstraints.iteritems():
-		#~ if(fullBody.isLimbInContact(k, stateid)):
-			#~ contacts[0]+=[v['effector']]
-		#~ if(fullBody.isLimbInContactIntermediary(k, stateid)):
-			#~ contacts[1]+=[v['effector']]
-		#~ if(fullBody.isLimbInContact(k, stateid+1)):
-			#~ contacts[2]+=[v['effector']]
-	#~ return contacts
-#~ 
-#~ def gencontactsPerFrame(fullBody, stateid, limbsCOMConstraints, pp, path_ids, times, dt_framerate=1./24.):
-	#~ contactsPerPhase = getContactPerPhase(fullBody, stateid, limbsCOMConstraints)
-	#~ config_size = len(fullBody.getCurrentConfig())
-	#~ interpassed = False
-	#~ res = []
-	#~ for path_id in path_ids:		
-		#~ length = pp.client.problem.pathLength (path_id)
-		#~ num_frames_required_fly = times[1] / dt_framerate
-		#~ num_frames_required_support = times[0] / dt_framerate
-		#~ dt_fly = float(length) / num_frames_required_fly
-		#~ dt_support = float(length) / num_frames_required_support
-		#~ dt_finals_fly  = [dt_fly*i for i in range(int(num_frames_required_fly))] + [1]		
-		#~ dt_finals_support  = [dt_support*i for i in range(int(num_frames_required_support))] + [1]	
-		#~ config_size_path = len(pp.client.problem.configAtParam (path_id, 0))
-		#~ if(config_size_path > config_size):
-			#~ interpassed = True
-			#~ res+= [contactsPerPhase[1] for t in dt_finals_fly]
-		#~ elif interpassed:			
-			#~ res+= [contactsPerPhase[2] for t in dt_finals_support]
-		#~ else:
-			#~ res+= [contactsPerPhase[0] for t in dt_finals_support]
-	#~ return res
 
 def genPandNperFrame(fullBody, stateid, limbsCOMConstraints, pp, path_ids, times, dt_framerate=1./24.):
 	p, N= fullBody.computeContactPointsPerLimb(stateid, limbsCOMConstraints.keys(), limbsCOMConstraints)
+	freeEffectors = [ [limbsCOMConstraints[limb]['effector'] for limb in limbsCOMConstraints.keys() if not p[i].has_key(limbsCOMConstraints[limb]['effector'])] for i in range(len(p))]
 	config_size = len(fullBody.getCurrentConfig())
 	interpassed = False
 	pRes = []
@@ -73,24 +42,45 @@ def genPandNperFrame(fullBody, stateid, limbsCOMConstraints, pp, path_ids, times
 	for idx, path_id in enumerate(path_ids):		
 		length = pp.client.problem.pathLength (path_id)
 		num_frames_required = times[idx] / dt_framerate
-		print "dt_framerate", dt_framerate
-		print "num_frames_required", times[idx], " ", num_frames_required
+		#~ print "dt_framerate", dt_framerate
+		#~ print "num_frames_required", times[idx], " ", num_frames_required
 		dt = float(length) / num_frames_required
-		dt_finals  = [dt*i for i in range(int(num_frames_required))] + [1]		
-		#~ config_size_path = len(pp.client.problem.configAtParam (path_id, 0))
-		#~ if(config_size_path > config_size):
-			#~ interpassed = True
+		dt_finals  = [dt*i for i in range(int(round(num_frames_required)))]	
 		pRes+= [p[idx] for t in dt_finals]
 		nRes+= [N[idx] for t in dt_finals]
-		#~ elif interpassed:			
-			#~ pRes+= [p[2] for t in dt_finals_support]
-			#~ nRes+= [N[2] for t in dt_finals_support]
-		#~ else:
-			#~ pRes+= [p[0] for t in dt_finals_support]
-			#~ nRes+= [N[0] for t in dt_finals_support]
-	return pRes, nRes
+	return pRes, nRes, freeEffectors
+
+
+def __getPos(effector, fullBody, config):
+	fullBody.setCurrentConfig (config)
+	q = fullBody.getJointPosition(effector)
+	quat_end = q[4:7]
+	q[6] = q[3]
+	q[3:6] = quat_end
+	return q
+
+def genPEffperFrame(fullBody, freeEffectorsPerPhase, qs, pp, times, dt_framerate):
+	res = []
+	for idx, phase in enumerate(freeEffectorsPerPhase):
+		num_frames_required = int(times[idx] / dt_framerate)
+		qid = len(res)
+		for q in qs[qid:num_frames_required+qid]:			
+			p = {}
+			for effector in phase:
+				p[effector] = __getPos(effector, fullBody, q)
+			res.append(p)
+	return res
 
 
+def genComPerFrame(final_state, dt, dt_framerate = 1./1000.):
+	num_frames_per_dt = int(round(dt / dt_framerate))
+	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))]
+	return cs
+
 stat_data = { 
 "error_com_proj" : 0,
 "error_optim_fail" : 0,
@@ -154,9 +144,9 @@ trackedEffectors = []):
 			else:
 				comC = None
 			if(optim_effectors):
-				pid, trajectory, timeelapsed  =  solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
+				pid, trajectory, timeelapsed, final_state  =  solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
 			else :
-				pid, trajectory, timeelapsed  =       solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
+				pid, trajectory, timeelapsed, final_state  =       solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
 			displayComPath(pp, pid)
 			#~ pp(pid)
 			global res
@@ -171,24 +161,31 @@ trackedEffectors = []):
 			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)
 			#~ new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)	
-			Ps, Ns = genPandNperFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, time_per_path, frame_rate_andrea)
+			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:]
 			trajec = trajec + new_traj
 			trajec_mil += new_traj_andrea
 			#~ global contacts
 			#~ contacts += new_contacts	
 			global pos
-			print "pos", len(pos), " ps, ", len(Ps)
 			pos += Ps
 			global normals
 			normals+= Ns
-			print len(trajec_mil), " ",  len(pos), " ", len(normals)
-			assert(len(trajec_mil) == len(pos) and len(normals) == len(pos))			
+			global pEffs
+			pEffs+= NPeffs
+			global coms
+			coms+= com
+			print len(trajec_mil), " ",  len(pos), " ", len(normals), " ", len(coms), " ", len(pEffs)
+			assert(len(trajec_mil) == len(pos) and len(normals) == len(pos) and len(normals) == len(coms) and len(coms) == len(pEffs))
 			stat_data["num_success"] += 1
 		else:
 			print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
@@ -289,9 +286,9 @@ def step_profile(fullBody, configs, i, optim, limbsCOMConstraints,  friction = 0
 			else:
 				comC = None
 			if(optim_effectors):
-				pid, trajectory, timeelapsed =  solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True)
+				pid, trajectory, timeelapsed, final_state =  solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True)
 			else :
-				pid, trajectory, timeelapsed =       solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True)			
+				pid, trajectory, timeelapsed, final_state =       solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True)			
 			__update_cwc_time(timeelapsed)	
 			stat_data["num_success"] += 1
 		else:
@@ -346,7 +343,31 @@ def displayInSave(pp, pathId, configs):
 
 import time
 
+def __isDiff(P0, P1):
+	return len(set(P0.keys()) - set(P1.keys())) != 0 or len(set(P1.keys()) - set(P0.keys()))
+
 from pickle import dump
+def compressData(data_array, filename):
+	qs = [data['q'][:] for data in data_array]
+	C =  [data['C'][:] for data in data_array]
+	a = {}
+	frameswitches = []
+	for i in range(0,len(pos)):
+		if i == 0 or __isDiff(pos[i], pos[i-1]):
+			a = {}
+			for effector in pos[i].keys():
+				a[effector] = {'P' : pos[i][effector], 'N' : normals[i][effector]}
+			frameswitches.append([i,a])
+	res = {}
+	res['Q'] = [data['q'][:] for data in data_array]
+	res['C'] = [data['C'][:] for data in data_array]
+	res['fly'] = pEffs
+	res['frameswitches'] = frameswitches
+	f1=open(filename+"_compressed", 'w+')
+	dump(res, f1)
+	f1.close()
+	return res
+
 def saveToPinocchio(filename):
 	res = []
 	for i, q_gep in enumerate(trajec_mil):
@@ -355,12 +376,12 @@ def saveToPinocchio(filename):
 		quat_end = q[4:7]
 		q[6] = q[3]
 		q[3:6] = quat_end
-		data = {'q':q, 'P' : pos[i], 'N' : normals[i]}
+		data = {'q':q, 'P' : pos[i], 'N' : normals[i], 'C' : coms [i], 'pEffs' : pEffs[i]}
 		res += [data]
 	f1=open(filename, 'w+')
 	dump(res, f1)
 	f1.close()
-	return res
+	return compressData(res,filename)
 		
 def clean():
 	global res
@@ -370,6 +391,8 @@ def clean():
 	global errorid
 	global pos
 	global normals
+	global pEffs
+	global coms
 	res = []
 	trajec = []
 	trajec_mil = []
@@ -377,6 +400,8 @@ def clean():
 	errorid = []
 	pos = []
 	normals = []
+	pEffs = []
+	coms = []
 
 import copy
 
@@ -413,7 +438,7 @@ def profile(fullBody, configs, i_start, i_end, limbsCOMConstraints,  friction =
 
 def saveAllData(fullBody, r, name):
 	fullBody.exportAll(r, trajec, name)
-	saveToPinocchio(name)
+	return saveToPinocchio(name)
 
 def play_traj(fullBody,pp,frame_rate):
 	global trajec

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

 __24fps = 1. / 24.
-__EPS = 0.0000001
+__EPS = 0.00001
 from numpy.linalg import norm
 
 def __linear_interpolation(p0,p1,dist_p0_p1, val):
@@ -9,7 +9,7 @@ def __gen_frame_positions(com_waypoints, dt, dt_framerate=__24fps):
 	total_time = (len(com_waypoints) - 1) * dt
 	dt_final = total_time * dt_framerate
 	num_frames_required = total_time / dt_framerate + 1
-	dt_finals = [dt_final*i for i in range(int(num_frames_required))]
+	dt_finals = [dt_final*i for i in range(int(round(num_frames_required)))]
 	ids_val = []
 	for i in range(len(com_waypoints) - 1):
 		ids_val += [(i,val-dt*i) for val in dt_finals if (val < (i+1) *dt) and (val > i*dt)]
@@ -44,7 +44,7 @@ def linear_interpolate_path(robot, path_player, path_id, total_time, dt_framerat
 	length = pp.client.problem.pathLength (path_id)
 	num_frames_required = total_time / dt_framerate
 	dt = float(length) / num_frames_required
-	dt_finals = [dt*i for i in range(int(num_frames_required))] + [length]
+	dt_finals = [dt*i for i in range(int(round(num_frames_required))-1)] + [length]
 	return[pp.client.problem.configAtParam (path_id, t) for t in dt_finals]
 	
 def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate=__24fps):
@@ -53,8 +53,8 @@ def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate
 	num_frames_required = total_time / dt_framerate
 	dt = float(length) / num_frames_required
 	#matches the extradof normalized
-	print "length ", length, "total tome ", total_time, "frame rate ", dt_framerate, "num_frames_required ", num_frames_required, "dt ", dt
-	dt_finals = [dt*i / length for i in range(int(num_frames_required))] + [1]
+	#~ print "length ", length, "total tome ", total_time, "frame rate ", dt_framerate, "num_frames_required ", num_frames_required, "dt ", dt
+	dt_finals = [dt*i / length for i in range(int(round(num_frames_required)))]
 	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):

src/rbprmbuilder.impl.cc

@@ -845,13 +845,12 @@ namespace hpp {
 
     void SetPositionAndNormal(rbprm::State& state,
 			hpp::rbprm::RbPrmFullBodyPtr_t fullBody, const hpp::floatSeq& configuration,
-			const hpp::Names_t& contactLimbs)
+            std::vector<std::string>& names)
     {
         core::Configuration_t old = fullBody->device_->currentConfiguration();
         model::Configuration_t config = dofArrayToConfig (fullBody->device_, configuration);
         fullBody->device_->currentConfiguration(config);
         fullBody->device_->computeForwardKinematics();
-        std::vector<std::string> names = stringConversion(contactLimbs);
         for(std::vector<std::string>::const_iterator cit = names.begin(); cit != names.end();++cit)
         {
             rbprm::T_Limb::const_iterator lit = fullBody->GetLimbs().find(*cit);
@@ -865,7 +864,8 @@ namespace hpp {
             const fcl::Matrix3f& rot = transform.getRotation();
             state.contactPositions_[*cit] = transform.getTranslation();
             state.contactRotation_[*cit] = rot;
-            state.contactNormals_[*cit] = fcl::Vec3f(rot(0,2),rot(1,2), rot(2,2));
+            const fcl::Vec3f z = transform.getRotation() * lit->second->normal_;
+            state.contactNormals_[*cit] = z;
             state.contacts_[*cit] = true;
             state.contactOrder_.push(*cit);
         }        
@@ -880,7 +880,8 @@ namespace hpp {
     {
         try
         {
-            SetPositionAndNormal(startState_,fullBody_, configuration, contactLimbs);
+            std::vector<std::string> names = stringConversion(contactLimbs);
+            SetPositionAndNormal(startState_,fullBody_, configuration, names);
         }
         catch(std::runtime_error& e)
         {
@@ -888,11 +889,43 @@ namespace hpp {
         }
     }
 
+
+    hpp::floatSeq* RbprmBuilder::computeContactForConfig(const hpp::floatSeq& configuration, const char *limbNam) throw (hpp::Error)
+    {
+        State state;
+        std::string limb(limbNam);
+        try
+        {
+            std::vector<std::string> limbs; limbs.push_back(limbNam);
+            SetPositionAndNormal(state,fullBody_, configuration, limbs);
+
+            const std::vector<fcl::Vec3f>& positions = computeRectangleContact(fullBody_,state,limb);
+            _CORBA_ULong size = (_CORBA_ULong) positions.size () * 3;
+            hpp::floatSeq* dofArray = new hpp::floatSeq();
+            dofArray->length(size);
+            for(std::size_t h = 0; h<positions.size(); ++h)
+            {
+                for(std::size_t k =0; k<3; ++k)
+                {
+                    model::size_type j (h*3 + k);
+                    dofArray[j] = positions[h][k];
+                }
+            }
+            return dofArray;
+        }
+        catch(std::runtime_error& e)
+        {
+            throw Error(e.what());
+        }
+
+    }
+
     void RbprmBuilder::setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error)
     {
         try
         {
-            SetPositionAndNormal(endState_,fullBody_, configuration, contactLimbs);
+            std::vector<std::string> names = stringConversion(contactLimbs);
+            SetPositionAndNormal(endState_,fullBody_, configuration, names);
         }
         catch(std::runtime_error& e)
         {

src/rbprmbuilder.impl.hh

@@ -147,6 +147,7 @@ namespace hpp {
 
         virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
         virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
+        virtual hpp::floatSeq*  computeContactForConfig(const hpp::floatSeq& configuration, const char* limbNam) throw (hpp::Error);
         virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
         virtual hpp::floatSeqSeq* computeContactPointsForLimb(unsigned short cId, const char* limbName) throw (hpp::Error);
         virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);