cwc_trajectory_helper.py 16.4 KB
Newer Older
1
2
3

from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
4
from cwc import OptimError, cone_optimization
5
from hpp.corbaserver.rbprm.tools.path_to_trajectory import gen_trajectory_to_play
6
from numpy import append, array
7
8
9
10
11

#global variables
res = []
trajec = []
trajec_mil = []
Steve Tonneau's avatar
Steve Tonneau committed
12
contacts = []
13
14
pos = []
normals = []
15
16
pEffs = []
coms = []
17
errorid = []
Steve Tonneau's avatar
Steve Tonneau committed
18
cones_saved = []
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

def displayComPath(pp, pathId,color=[0.,0.75,0.15,0.9]) :
	pathPos=[]
	length = pp.end*pp.client.problem.pathLength (pathId)
	t = pp.start*pp.client.problem.pathLength (pathId)
	while t < length :
		q = pp.client.problem.configAtParam (pathId, t)
		pp.publisher.robot.setCurrentConfig(q)
		q = pp.publisher.robot.getCenterOfMass()
		pathPos = pathPos + [q[:3]]
		t += pp.dt
	nameCurve = "path_"+str(pathId)+"_com"
	pp.publisher.client.gui.addCurve(nameCurve,pathPos,color)
	pp.publisher.client.gui.addToGroup(nameCurve,pp.publisher.sceneName)
	pp.publisher.client.gui.refresh()


Steve Tonneau's avatar
Steve Tonneau committed
36
def genPandNandConesperFrame(fullBody, stateid, limbsCOMConstraints, cones, pp, path_ids, times, dt_framerate=1./24.):
37
	p, N= fullBody.computeContactPointsPerLimb(stateid, limbsCOMConstraints.keys(), limbsCOMConstraints)
38
	freeEffectors = [ [limbsCOMConstraints[limb]['effector'] for limb in limbsCOMConstraints.keys() if not p[i].has_key(limbsCOMConstraints[limb]['effector'])] for i in range(len(p))]
39
40
41
42
	config_size = len(fullBody.getCurrentConfig())
	interpassed = False
	pRes = []
	nRes = []
Steve Tonneau's avatar
Steve Tonneau committed
43
	cRes = []
44
	for idx, path_id in enumerate(path_ids):		
45
		length = pp.client.problem.pathLength (path_id)
46
		num_frames_required = times[idx] / dt_framerate
47
48
		#~ print "dt_framerate", dt_framerate
		#~ print "num_frames_required", times[idx], " ", num_frames_required
49
		dt = float(length) / num_frames_required
50
		dt_finals  = [dt*i for i in range(int(round(num_frames_required)))]	
51
52
		pRes+= [p[idx] for t in dt_finals]
		nRes+= [N[idx] for t in dt_finals]
Steve Tonneau's avatar
Steve Tonneau committed
53
54
		cRes+= [cones[idx] for t in dt_finals]
	return pRes, nRes, freeEffectors, cRes
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75


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
76

77

78
79
def genComPerFrame(final_state, dt, dt_framerate = 1./1000.):
	num_frames_per_dt = int(round(dt / dt_framerate))
80
	inc = 1./((float)(num_frames_per_dt))
81
82
83
84
	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 = []
85
86
87
88
	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])
89
90
	return cs

91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
stat_data = { 
"error_com_proj" : 0,
"error_optim_fail" : 0,
"error_unknown" : 0,
"num_errors" : 0,
"num_success" : 0,
"num_trials" : 0,
"time_cwc" : { "min" : 10000000., "avg" : 0., "max" : 0., "totaltime" : 0., "numiter" : 0 },
}

def __update_cwc_time(t):
	global stat_data
	stat_data["time_cwc"]["min"] = min(stat_data["time_cwc"]["min"], t) 
	stat_data["time_cwc"]["max"] = max(stat_data["time_cwc"]["max"], t) 
	stat_data["time_cwc"]["totaltime"] += t
	stat_data["time_cwc"]["numiter"] += 1
	

109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
def __getTimes(fullBody, configs, i, time_scale):
	trunk_distance =  np.linalg.norm(np.array(configs[i+1][0:3]) - np.array(configs[i][0:3]))
	distance = max(fullBody.getEffectorDistance(i,i+1), trunk_distance)
	dist = int(distance * time_scale)#heuristic
	while(dist %4 != 0):
		dist +=1
	total_time_flying_path = max(float(dist)/10., 0.3)
	total_time_support_path = float((int)(math.ceil(min(total_time_flying_path /2., 0.2)*10.))) / 10.
	times = [total_time_support_path, total_time_flying_path]
	if(total_time_flying_path>= 1.):
		dt = 0.1
	elif total_time_flying_path<= 0.3:
		dt = 0.05
	else:
		dt = 0.1
	return times, dt, distance
		

127
from hpp import Error as hpperr
128
import sys, time
129
130
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,
trackedEffectors = []):
131
	global errorid
132
	global stat_data	
133
	fail = 0
134
135
	#~ try:
	if(True):
136
137
138
139
140
141
		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)
142
143
			times += times2
			dt = min(dt, dt2)
144
145
		time_per_path = [times[0]] + [times[1]] + [times [0]]
		print 'time per path', times, time_per_path
146
		print 'dt', dt
147
148
		if(distance > 0.0001):		
			stat_data["num_trials"] += 1
149
150
151
152
153
			if(useCOMConstraints):
				comC = limbsCOMConstraints
			else:
				comC = None
			if(optim_effectors):
Steve Tonneau's avatar
Steve Tonneau committed
154
				pid, trajectory, timeelapsed, final_state, cones  =  solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
155
			else :
Steve Tonneau's avatar
Steve Tonneau committed
156
				pid, trajectory, timeelapsed, final_state, cones  =       solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
157
158
159
160
161
			displayComPath(pp, pid)
			#~ pp(pid)
			global res
			res = res + [pid]
			global trajec
162
163
			global trajec_mil			
			frame_rate = 1./24.
Steve Tonneau's avatar
Steve Tonneau committed
164
			#~ frame_rate_andrea = 1./1000.
Steve Tonneau's avatar
Steve Tonneau committed
165
			frame_rate_andrea = 1./1000.
166
167
168
			#~ if(len(trajec) > 0):
				#~ frame_rate = 1./25.
				#~ frame_rate_andrea = 1./1001.
169
			new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path, frame_rate)
Steve Tonneau's avatar
Steve Tonneau committed
170
			new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea)
171
			#~ new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)	
Steve Tonneau's avatar
Steve Tonneau committed
172
173
174
			Ps, Ns, freeEffectorsPerPhase, Ks = genPandNandConesperFrame(fullBody, i, limbsCOMConstraints, cones, 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)
175
176
177
178
179
180
181
			#~ 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:]
182
			trajec = trajec + new_traj
Steve Tonneau's avatar
Steve Tonneau committed
183
			trajec_mil += new_traj_andrea
184
185
			#~ global contacts
			#~ contacts += new_contacts	
Steve Tonneau's avatar
Steve Tonneau committed
186
187
188
189
190
191
192
193
194
195
196
197
			global cones_saved
			cones_saved += Ks
			global pos
			pos += Ps
			global normals
			normals+= Ns
			global pEffs
			pEffs+= NPeffs
			global coms
			coms+= com
			print len(trajec_mil), " ",  len(pos), " ", len(normals), " ", len(coms), " ", len(pEffs), " ", len(cones_saved)
			assert(len(trajec_mil) == len(pos) and len(normals) == len(pos) and len(normals) == len(coms) and len(cones_saved) == len(coms) and len(coms) == len(pEffs))
198
			stat_data["num_success"] += 1
199
200
		else:
			print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
201
	#~ except hpperr as e:		
202
203
204
205
206
207
208
209
210
211
		#~ print "hpperr failed at id " + str(i) , e.strerror
		#~ if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
			#~ print "could not project com, trying to increase velocity "
			#~ try:
				#~ return step(fullBody, configs, i, optim, pp, limbsCOMConstraints,  friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors)
			#~ except: 
				#~ if ((len(configs) - 1) - (i + 3) > 0):
					#~ print "could not project com, trying to increase velocity more "
					#~ step(fullBody, configs, i, optim, pp, limbsCOMConstraints,  friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw,  trackedEffectors = trackedEffectors)		
		#~ else:
212
213
214
215
216
217
218
219
220
221
222
223
		#~ print "In hpperr and window != 0"
		#~ print "hpperr failed at id " + str(i) , e.strerror
		#~ stat_data["error_com_proj"] += 1
		#~ stat_data["num_errors"] += 1
		#~ errorid += [i]
		#~ fail+=1
	#~ except OptimError as e:
		#~ print "OptimError failed at id " + str(i) , e
		#~ stat_data["error_optim_fail"] += 1
		#~ stat_data["num_errors"] += 1
		#~ errorid += [i]
		#~ fail+=1
224
225
226
227
228
229
230
231
232
233
234
235
	#~ except ValueError as e:
		#~ print "ValueError failed at id " + str(i) , e
		#~ stat_data["error_unknown"] += 1
		#~ stat_data["num_errors"] += 1
		#~ errorid += [i]
		#~ fail+=1
	#~ except IndexError as e:
		#~ print "IndexError failed at id " + str(i) , e
		#~ stat_data["error_unknown"] += 1
		#~ stat_data["num_errors"] += 1
		#~ errorid += [i]
		#~ fail+=1
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
	#~ except Exception as e:
		#~ print e
		#~ if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
			#~ print "could not project com, trying to increase velocity "
			#~ try:
				#~ return step(fullBody, configs, i, optim, pp, limbsCOMConstraints,  friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw,  trackedEffectors = trackedEffectors)
			#~ except: 
				#~ print "faile twice"
				#~ if ((len(configs) - 1) - (i + 3) > 0):
					#~ print "could not project com, trying to increase velocity more "
					#~ step(fullBody, configs, i, optim, pp, limbsCOMConstraints,  friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw,  trackedEffectors = trackedEffectors)		
		#~ else:
			#~ print "In Exception and window != 0"
			#~ stat_data["error_unknown"] += 1
			#~ stat_data["num_errors"] += 1
			#~ print e
			#~ errorid += [i]
			#~ fail+=1
254
255
	#~ except:
		#~ print "unknown"
256
257
258
259
260
261
262
263
264
		#~ if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
			#~ print "could not project com, trying to increase velocity "
			#~ try:
				#~ return step(fullBody, configs, i, optim, pp, limbsCOMConstraints,  friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw,  trackedEffectors = trackedEffectors)
			#~ except: 
				#~ if ((len(configs) - 1) - (i + 3) > 0):
					#~ print "could not project com, trying to increase velocity more "
					#~ step(fullBody, configs, i, optim, pp, limbsCOMConstraints,  friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw,  trackedEffectors = trackedEffectors)		
		#~ else:
265
266
267
268
269
		#~ print "In unknown and window != 0"
		#~ stat_data["error_unknown"] += 1
		#~ stat_data["num_errors"] += 1
		#~ errorid += [i]
		#~ fail+=1
Steve Tonneau's avatar
Steve Tonneau committed
270
271
	#~ return fail
	return final_state, cones
272
	
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
def step_profile(fullBody, configs, i, optim, limbsCOMConstraints,  friction = 0.5, optim_effectors = True, time_scale = 20., useCOMConstraints = False):
	global errorid		
	global stat_data	
	fail = 0
	try:
		trunk_distance =  np.linalg.norm(np.array(configs[i+1][0:3]) - np.array(configs[i][0:3]))
		distance = max(fullBody.getEffectorDistance(i,i+1), trunk_distance)
		dist = int(distance * time_scale)#heuristic
		while(dist %4 != 0):
			dist +=1
		total_time_flying_path = max(float(dist)/10., 0.3)
		total_time_support_path = float((int)(math.ceil(min(total_time_flying_path /2., 0.2)*10.))) / 10.
		times = [total_time_support_path, total_time_flying_path]
		if(total_time_flying_path>= 1.):
			dt = 0.1
		elif total_time_flying_path<= 0.3:
			dt = 0.05
		else:
			dt = 0.1
292
		if(distance > 0.0001):				
293
294
295
296
297
298
			stat_data["num_trials"] += 1
			if(useCOMConstraints):
				comC = limbsCOMConstraints
			else:
				comC = None
			if(optim_effectors):
299
				pid, trajectory, timeelapsed, final_state =  solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True)
300
			else :
301
				pid, trajectory, timeelapsed, final_state =       solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True)			
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
			__update_cwc_time(timeelapsed)	
			stat_data["num_success"] += 1
		else:
			print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
	except hpperr as e:
		print "hpperr failed at id " + str(i) , e.strerror
		stat_data["error_com_proj"] += 1
		stat_data["num_errors"] += 1
		errorid += [i]
		fail+=1
	except OptimError as e:
		print "OptimError failed at id " + str(i) , e.strerror
		stat_data["error_optim_fail"] += 1
		stat_data["num_errors"] += 1
		errorid += [i]
		fail+=1
	except ValueError as e:
		print "ValueError failed at id " + str(i) , e
		stat_data["error_unknown"] += 1
		stat_data["num_errors"] += 1
		errorid += [i]
		fail+=1
	except IndexError as e:
		print "IndexError failed at id " + str(i) , e
		stat_data["error_unknown"] += 1
		stat_data["num_errors"] += 1
		errorid += [i]
		fail+=1
	except Exception as e:
		stat_data["error_unknown"] += 1
		stat_data["num_errors"] += 1
		print e
		errorid += [i]
		fail+=1
	except:
		stat_data["error_unknown"] += 1
		stat_data["num_errors"] += 1
		errorid += [i]
		fail+=1
	return fail
	
	
344
345
346
347
348
349
350
351
352
353
354
355
def displayInSave(pp, pathId, configs):
	length = pp.end*pp.client.problem.pathLength (pathId)
	t = pp.start*pp.client.problem.pathLength (pathId)
	while t < length :
		q = pp.client.problem.configAtParam (pathId, t)
		configs.append(q)
		t += (pp.dt * pp.speed)

	

import time

356
357
358
def __isDiff(P0, P1):
	return len(set(P0.keys()) - set(P1.keys())) != 0 or len(set(P1.keys()) - set(P0.keys()))

359
from pickle import dump
360
361
362
def compressData(data_array, filename):
	qs = [data['q'][:] for data in data_array]
	C =  [data['C'][:] for data in data_array]
Steve Tonneau's avatar
Steve Tonneau committed
363
	cones =  [data['cone'][:] for data in data_array]
364
365
366
367
368
369
370
371
372
373
374
	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]
Steve Tonneau's avatar
Steve Tonneau committed
375
	res['cones'] = [data['cone'][:] for data in data_array]
376
377
378
379
380
381
382
	res['fly'] = pEffs
	res['frameswitches'] = frameswitches
	f1=open(filename+"_compressed", 'w+')
	dump(res, f1)
	f1.close()
	return res

383
384
385
386
387
388
389
390
def saveToPinocchio(filename):
	res = []
	for i, q_gep in enumerate(trajec_mil):
		#invert to pinocchio config:
		q = q_gep[:]
		quat_end = q[4:7]
		q[6] = q[3]
		q[3:6] = quat_end
Steve Tonneau's avatar
Steve Tonneau committed
391
		data = {'q':q, 'P' : pos[i], 'N' : normals[i], 'C' : coms [i], 'pEffs' : pEffs[i], 'cone' : cones_saved[i]}
392
393
394
395
		res += [data]
	f1=open(filename, 'w+')
	dump(res, f1)
	f1.close()
396
	return compressData(res,filename)
397
398
399
400
401
402
403
404
405
		
def clean():
	global res
	global trajec
	global trajec_mil
	global contacts
	global errorid
	global pos
	global normals
406
407
	global pEffs
	global coms
Steve Tonneau's avatar
Steve Tonneau committed
408
409
	global cones_saved
	cones_saved = []
410
411
412
	res = []
	trajec = []
	trajec_mil = []
Steve Tonneau's avatar
Steve Tonneau committed
413
	#~ contacts = []
414
415
416
	errorid = []
	pos = []
	normals = []
417
418
	pEffs = []
	coms = []
419
420
421
422
423

import copy

def stats():	
	global stat_data	
424
	stat_data["error_id"] = errorid
425
426
	stat_data_copy = copy.deepcopy(stat_data)
	return stat_data_copy
427
	
428
429
430
431
432
433
434
435
436
437
def write_stats(filename):
	global stat_data	
	sd = copy.deepcopy(stat_data)
	f = open(filename, 'a')
	f.write("optim_error_com_proj " + str(sd["error_com_proj"]) + "\n")
	f.write("optim_error_optim_fail " + str(sd["error_optim_fail"]) + "\n")
	f.write("optim_error_unknown " + str(sd["error_unknown"]) + "\n")
	f.write("optim_num_success " + str(sd["num_success"]) + "\n")
	f.write("optim_num_trials " + str(sd["num_trials"]) + "\n")
	f.write("num_errors " + str(sd["num_errors"]) + "\n")
438
	f.write("error_id " + str(errorid) + "\n")
439
440
441
442
443
	f.write("time_cwc " + str(sd["time_cwc"]["min"]) + " " + str(sd["time_cwc"]["avg"]) + " " + str(sd["time_cwc"]["max"]) + " " + str(sd["time_cwc"]["totaltime"])  + " " + str(sd["time_cwc"]["numiter"]) + " " + "\n")
	f.close()
	return sd

def profile(fullBody, configs, i_start, i_end, limbsCOMConstraints,  friction = 0.5, optim_effectors = False, time_scale = 20., useCOMConstraints = False, filename ="log.txt"):	
444
445
446
	global stat_data		
	if(i_end > len(configs)-1):
		print "ERROR: i_end < len_configs ", i_end, len(configs)-1
447
		return # no point in trying optim, path was not fully computed
448
449
	for i in range(i_start, i_end):		
		step_profile(fullBody, configs, i, 0, limbsCOMConstraints,  friction, optim_effectors, time_scale, useCOMConstraints)
450
451
	stat_data["time_cwc"]["avg"] = stat_data["time_cwc"]["totaltime"] / float(stat_data["time_cwc"]["numiter"])
	write_stats(filename)
452
453

def saveAllData(fullBody, r, name):
Steve Tonneau's avatar
Steve Tonneau committed
454
	global trajec
455
	fullBody.exportAll(r, trajec, name)
456
	return saveToPinocchio(name)
457
458

def play_traj(fullBody,pp,frame_rate):
459
460
	global trajec
	return play_trajectory(fullBody,pp,trajec,frame_rate)
461
462
463

#~ fullBody.exportAll(r, trajec, 'darpa_hyq_t_var_04f_andrea');
#~ saveToPinocchio('darpa_hyq_t_var_04f_andrea')