cwc_trajectory_helper.py 15.9 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 = []
12
#~ contacts = []
13
14
pos = []
normals = []
15
16
pEffs = []
coms = []
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
errorid = []

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()


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

74

75
76
77
78
79
80
81
82
83
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

84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
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
	

102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
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
		

120
from hpp import Error as hpperr
121
import sys, time
122
123
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 = []):
124
	global errorid
125
	global stat_data	
126
	fail = 0
127
128
	#~ try:
	if(True):
129
130
131
132
133
134
		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)
135
136
			times += times2
			dt = min(dt, dt2)
137
138
		time_per_path = [times[0]] + [times[1]] + [times [0]]
		print 'time per path', times, time_per_path
139
		print 'dt', dt
140
141
		if(distance > 0.0001):		
			stat_data["num_trials"] += 1
142
143
144
145
146
			if(useCOMConstraints):
				comC = limbsCOMConstraints
			else:
				comC = None
			if(optim_effectors):
147
				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)
148
			else :
149
				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)
150
151
152
153
154
			displayComPath(pp, pid)
			#~ pp(pid)
			global res
			res = res + [pid]
			global trajec
155
156
157
			global trajec_mil			
			frame_rate = 1./24.
			frame_rate_andrea = 1./1000.
158
159
160
			#~ if(len(trajec) > 0):
				#~ frame_rate = 1./25.
				#~ frame_rate_andrea = 1./1001.
161
162
			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)
163
			#~ new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)	
164
165
166
			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)
167
168
169
			if(len(trajec) > 0):
				new_traj = new_traj[1:]
				new_traj_andrea = new_traj_andrea[1:]
170
				#~ new_contacts = new_contacts[1:]
171
172
				Ps = Ps[1:]
				Ns = Ns[1:]
173
174
				com = com[1:]
				pEffs = pEffs[1:]
175
			trajec = trajec + new_traj
Steve Tonneau's avatar
Steve Tonneau committed
176
			trajec_mil += new_traj_andrea
177
178
			#~ global contacts
			#~ contacts += new_contacts	
179
180
181
182
			global pos
			pos += Ps
			global normals
			normals+= Ns
183
184
185
186
187
188
			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))
189
			stat_data["num_success"] += 1
190
191
		else:
			print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
192
	#~ except hpperr as e:		
193
194
195
196
197
198
199
200
201
202
		#~ 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:
203
204
205
206
207
208
209
210
211
212
213
214
		#~ 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
215
216
217
218
219
220
221
222
223
224
225
226
	#~ 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
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
	#~ 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
245
246
	#~ except:
		#~ print "unknown"
247
248
249
250
251
252
253
254
255
		#~ 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:
256
257
258
259
260
		#~ print "In unknown and window != 0"
		#~ stat_data["error_unknown"] += 1
		#~ stat_data["num_errors"] += 1
		#~ errorid += [i]
		#~ fail+=1
261
262
	return fail
	
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
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
282
		if(distance > 0.0001):				
283
284
285
286
287
288
			stat_data["num_trials"] += 1
			if(useCOMConstraints):
				comC = limbsCOMConstraints
			else:
				comC = None
			if(optim_effectors):
289
				pid, trajectory, timeelapsed, final_state =  solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True)
290
			else :
291
				pid, trajectory, timeelapsed, final_state =       solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True)			
292
293
294
295
296
297
298
299
300
301
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
			__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
	
	
334
335
336
337
338
339
340
341
342
343
344
345
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

346
347
348
def __isDiff(P0, P1):
	return len(set(P0.keys()) - set(P1.keys())) != 0 or len(set(P1.keys()) - set(P0.keys()))

349
from pickle import dump
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
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

371
372
373
374
375
376
377
378
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
379
		data = {'q':q, 'P' : pos[i], 'N' : normals[i], 'C' : coms [i], 'pEffs' : pEffs[i]}
380
381
382
383
		res += [data]
	f1=open(filename, 'w+')
	dump(res, f1)
	f1.close()
384
	return compressData(res,filename)
385
386
387
388
389
390
391
392
393
		
def clean():
	global res
	global trajec
	global trajec_mil
	global contacts
	global errorid
	global pos
	global normals
394
395
	global pEffs
	global coms
396
397
398
399
400
401
402
	res = []
	trajec = []
	trajec_mil = []
	contacts = []
	errorid = []
	pos = []
	normals = []
403
404
	pEffs = []
	coms = []
405
406
407
408
409

import copy

def stats():	
	global stat_data	
410
	stat_data["error_id"] = errorid
411
412
	stat_data_copy = copy.deepcopy(stat_data)
	return stat_data_copy
413
	
414
415
416
417
418
419
420
421
422
423
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")
424
	f.write("error_id " + str(errorid) + "\n")
425
426
427
428
429
	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"):	
430
431
432
	global stat_data		
	if(i_end > len(configs)-1):
		print "ERROR: i_end < len_configs ", i_end, len(configs)-1
433
		return # no point in trying optim, path was not fully computed
434
435
	for i in range(i_start, i_end):		
		step_profile(fullBody, configs, i, 0, limbsCOMConstraints,  friction, optim_effectors, time_scale, useCOMConstraints)
436
437
	stat_data["time_cwc"]["avg"] = stat_data["time_cwc"]["totaltime"] / float(stat_data["time_cwc"]["numiter"])
	write_stats(filename)
438
439
440

def saveAllData(fullBody, r, name):
	fullBody.exportAll(r, trajec, name)
441
	return saveToPinocchio(name)
442
443

def play_traj(fullBody,pp,frame_rate):
444
445
	global trajec
	return play_trajectory(fullBody,pp,trajec,frame_rate)
446
447
448

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