cwc_trajectory.py 13.1 KB
Newer Older
1
2
3
4
import matplotlib
#~ matplotlib.use('Agg')
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
5
from cwc import cone_optimization
6
from obj_to_constraints import ineq_from_file, rotate_inequalities
7
import numpy as np
8
import math
9
from numpy.linalg import norm
10
import time
11
12

import hpp.corbaserver.rbprm.data.com_inequalities as ine
13

14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
ineqPath = ine.__path__[0] +"/"

# epsilon for testing whether a number is close to zero
_EPS = np.finfo(float).eps * 4.0

def quaternion_matrix(quaternion):
    q = np.array(quaternion, dtype=np.float64, copy=True)
    n = np.dot(q, q)
    if n < _EPS:
        return np.identity(4)
    q *= math.sqrt(2.0 / n)
    q = np.outer(q, q)
    return np.array([
        [1.0-q[2, 2]-q[3, 3],     q[1, 2]-q[3, 0],     q[1, 3]+q[2, 0], 0.0],
        [    q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3],     q[2, 3]-q[1, 0], 0.0],
        [    q[1, 3]-q[2, 0],     q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0],
        [                0.0,                 0.0,                 0.0, 1.0]])
31
32
33
34

def __get_com(robot, config):
	save = robot.getCurrentConfig()
	robot.setCurrentConfig(config)
35
	com = robot.getCenterOfMass()
36
37
38
	robot.setCurrentConfig(save)
	return com

39
40
constraintsComLoaded = {}

41
42
lastspeed = np.array([0,0,0])

43
44
45
46
def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm = False):
	global constraintsLoaded
	As = [];	bs = []
	fullBody.setCurrentConfig(config)
Steve Tonneau's avatar
Steve Tonneau committed
47
	contacts = []
48
49
50
51
52
53
54
	for i, v in limbsCOMConstraints.iteritems():
		if not constraintsComLoaded.has_key(i):
			constraintsComLoaded[i] = ineq_from_file(ineqPath+v['file'])
		contact = (interm and fullBody.isLimbInContactIntermediary(i, state)) or (not interm and fullBody.isLimbInContact(i, state))
		if contact:
			ineq = constraintsComLoaded[i]
			qEffector = fullBody.getJointPosition(v['effector'])
Steve Tonneau's avatar
Steve Tonneau committed
55
56
			tr = quaternion_matrix(qEffector[3:7])			
			tr[:3,3] = np.array(qEffector[0:3])
57
58
			ineq_r = rotate_inequalities(ineq, tr)
			As.append(ineq_r.A); bs.append(ineq_r.b);
Steve Tonneau's avatar
Steve Tonneau committed
59
			contacts.append(v['effector'])
60
61
	return [np.vstack(As), np.hstack(bs)]
		
62
def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, limbsCOMConstraints):
63
64
65
	init_com = __get_com(fullBody, states[state_id])
	end_com = __get_com(fullBody, states[state_id+1])
	p, N = fullBody.computeContactPoints(state_id)
66
67
	fly_time = phase_dt [1]
	support_time = phase_dt [0]
68
	t_end_phases = [0]
69
70
71
72
73
74
	[t_end_phases.append(t_end_phases[-1]+fly_time) for _ in range(len(p))]
	if(len(t_end_phases) == 4):
		t_end_phases[1] = support_time
		t_end_phases[2] = t_end_phases[1] + fly_time
		t_end_phases[3] = t_end_phases[2] + support_time
		t_end_phases = [float((int)(math.ceil(el*10.))) / 10. for el in t_end_phases]
75
76
	cones = None
	if(computeCones):
77
		cones = [fullBody.getContactCone(state_id, mu)[0]]
Steve Tonneau's avatar
Steve Tonneau committed
78
		if(len(p) > 2):
79
			cones.append(fullBody.getContactIntermediateCone(state_id, mu)[0])
80
		if(len(p) > len(cones)):
81
			cones.append(fullBody.getContactCone(state_id+1, mu)[0])		
82
83
84
85
86
87
88
	COMConstraints = None
	if(not (limbsCOMConstraints == None)):
		COMConstraints = [__get_com_constraint(fullBody, state_id, states[state_id], limbsCOMConstraints)]
		if(len(p) > 2):
			COMConstraints.append(__get_com_constraint(fullBody, state_id, states[state_id], limbsCOMConstraints, True))
		if(len(p) > len(COMConstraints)):
			COMConstraints.append(__get_com_constraint(fullBody, state_id + 1, states[state_id + 1], limbsCOMConstraints))
89
90
	return p, N, init_com, end_com, t_end_phases, cones, COMConstraints

91

92
def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, phase_dt = [0.4, 1],
93
94
reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):	
	global lastspeed
95
	use_window = max(0, min(use_window,  (len(states) - 1) - (state_id + 2))) # can't use preview if last state is reached	
96
	assert( len(phase_dt) >= 2 +  use_window * 2 ), "phase_dt does not describe all phases"
97
	
98
99
	constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
	#~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint', 'com_kinematic_constraint']
100
101
102
103
	param_constraints = []	
	mass = fullBody.getMass()
	
	p, N, init_com, end_com, t_end_phases, cones, COMConstraints = compute_state_info(fullBody,states, state_id, phase_dt[:2], mu, computeCones, limbsCOMConstraints)
104
105
	if(use_window > 0):
		init_waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
106
		init_end_com = end_com[:]
107
	for w in range(1,use_window+1):
108
109
		waypoint = end_com[:]
		waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
110
111
		# trying not to apply constraint
		#~ param_constraints += [("waypoint_reached_constraint",(waypoint_time, waypoint))]
112
		p1, N1, init_com1, end_com1, t_end_phases1, cones1, COMConstraints1 = compute_state_info(fullBody,states, state_id+w, phase_dt[2*w:], mu, computeCones, limbsCOMConstraints)
113
114
115
116
117
118
119
120
121
122
123
124
125
		p+=p1;
		N+=N1;
		end_com = end_com1;
		cones += cones1;
		if(COMConstraints != None and COMConstraints1 != None):
			COMConstraints += COMConstraints1;
		t_end_phases += [t_end_phases[-1] + t for t in t_end_phases1[1:]]
	
	if (not profile):
			print "num cones ", len(cones)
			print "end_phases", t_end_phases
	
	timeelapsed = 0		
Steve Tonneau's avatar
Steve Tonneau committed
126
	if (profile):
127
		start = time.clock() 
128
129
	var_final, params = cone_optimization(p, N, [init_com + lastspeed.tolist(), end_com + [0,0,0]], t_end_phases[1:], dt, cones, COMConstraints, mu, mass, 9.81, reduce_ineq, verbose,
	constraints, param_constraints)	
130
	#~ print "end_com ", end_com , "computed end come", var_final['c'][-1], var_final['c_end']
Steve Tonneau's avatar
Steve Tonneau committed
131
	if (profile):
132
133
		end = time.clock() 
		timeelapsed = (end - start) * 1000
Steve Tonneau's avatar
Steve Tonneau committed
134
		#~ print "solving time", timeelapsed
135
	if(use_window > 0):
136
137
138
		var_final['c_old'] = var_final['c'][:]
		var_final['dc_old'] = var_final['dc'][:]
		var_final['ddc_old'] = var_final['ddc'][:]
139
		var_final['c'] = var_final['c'][:init_waypoint_time+1]
140
141
		var_final['dc'] = var_final['dc'][:init_waypoint_time+1]
		var_final['ddc'] = var_final['ddc'][:init_waypoint_time+1]
142
		params["t_init_phases"] = params["t_init_phases"][:-3*use_window]
143
		print "trying to project on com (from, to) ", init_end_com, var_final['c'][-1]
144
		if (fullBody.projectStateToCOM(state_id+1, (var_final['c'][-1]).tolist())):
145
			#~ print "PROJECTED", init_end_com, var_final['c'][-1]
146
			states[state_id+1] = fullBody.getConfigAtState(state_id+1) #updating config from python side)
147
			lastspeed = var_final['dc'][init_waypoint_time]		
148
			print "init speed", lastspeed
149
150
151
		else:
			print "reached com is not good, restarting problem with 0 window"
			return gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt[:2], reduce_ineq, verbose, limbsCOMConstraints, profile, use_window = 0)			
152
153
154
	else:		
		if norm(np.array(var_final['c'][-1]) - np.array(__get_com(fullBody, states[state_id+1]))) > 0.00001:
			print "error in com desired: obtained ", __get_com(fullBody, states[state_id+1]), var_final['c'][-1]
155
		lastspeed = np.array([0,0,0])
156
		
157
	return var_final, params, timeelapsed
158

159
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):
160
	var_final, params, elapsed = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, False, use_window = use_window)
161
	p, N = fullBody.computeContactPoints(state_id)
Steve Tonneau's avatar
Steve Tonneau committed
162
	print "p", p
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
	fig = plt.figure()
	ax = fig.add_subplot(111, projection='3d')
	n = 100
	points = var_final['x']
	xs = [points[i] for i in range(0,len(points),6)]
	ys = [points[i] for i in range(1,len(points),6)]
	zs = [points[i] for i in range(2,len(points),6)]
	ax.scatter(xs, ys, zs, c='b')

	colors = ["r", "b", "g"]
	#print contact points of first phase
	for id_c, points in enumerate(p):
		xs = [point[0] for point in points]
		ys = [point[1] for point in points]
		zs = [point[2] for point in points]
		ax.scatter(xs, ys, zs, c=colors[id_c])
		
	ax.set_xlabel('X Label')
	ax.set_ylabel('Y Label')
	ax.set_zlabel('Z Label')

Steve Tonneau's avatar
Steve Tonneau committed
184
185
	plt.show()
	#~ plt.savefig('/tmp/c'+ str(state_id)+ '.png')
186
187
188
189
190
	
	print "plotting speed "
	print "end target ",  params['x_end']
	fig = plt.figure()
	ax = fig.add_subplot(111)
191
	if(use_window > 0):
192
193
		#~ points = var_final['dc_old']
		points = var_final['dc']
194
195
196
	else:
		points = var_final['dc']
		
197
	#~ print "points", points
198
	ys = [norm(el) * el[0] / abs(el[0]+ _EPS) for el in points]
199
200
201
202
	xs = [i * params['dt'] for i in range(0,len(points))]
	ax.scatter(xs, ys, c='b')


Steve Tonneau's avatar
Steve Tonneau committed
203
204
	plt.show()
	#~ plt.savefig('/tmp/dc'+ str(state_id)+ '.png')
205
206
207
208
	
	print "plotting acceleration "
	fig = plt.figure()
	ax = fig.add_subplot(111)
209
	if(use_window > 0):
210
211
		#~ points = var_final['ddc_old']
		points = var_final['ddc']
212
213
214
	else:
		points = var_final['ddc']
	ys = [norm(el) * el[0] / abs(el[0]+ _EPS) for el in points]
215
216
217
218
	xs = [i * params['dt'] for i in range(0,len(points))]
	ax.scatter(xs, ys, c='b')


Steve Tonneau's avatar
Steve Tonneau committed
219
	plt.show()
220
	plt.savefig('/tmp/ddc'+ str(state_id)+ '.png')
221
	
222
223
224
225
226
227
228
229
	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')

230

Steve Tonneau's avatar
Steve Tonneau committed
231
	plt.show()
232
	plt.savefig('/tmp/dL'+ str(state_id)+ '.png')
233
	return var_final, params, elapsed
234
	
235
def __cVarPerPhase(var, dt, t, final_state, addValue):
236
	varVals = addValue + [v.tolist() for v in final_state[var]]
237
	varPerPhase = [[varVals[(int)(round(t_id/dt)) ] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1])  ]
238
	varPerPhase[2].append(varVals[-1])
239
240
	if(not var == "ddc"):
		assert len(varVals) == len(varPerPhase[0]) + len(varPerPhase[1]) + len(varPerPhase[2]), mess
241
		
242
243
244
245
246
247
	if var == "dc":
		varPerPhase[2] = varPerPhase[2][:-1] # not relevant for computation
	else:
		varPerPhase[0].append(varPerPhase[1][0]) # end pos of state is the same as the previous one
		varPerPhase[1].append(varPerPhase[2][0])
	if var == "ddc": #acceleration: remove first
248
		varPerPhase = [v[1:] for v in varPerPhase]
249
		assert len(final_state[var]) == len(varPerPhase[0]) + len(varPerPhase[1]) + len(varPerPhase[2]), "incorrect num of ddc"		
250
251
	return varPerPhase
	
252
def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1], reduce_ineq = True,
253
 num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):
254
255
	print "callgin gen ",state_id
	if(draw):
256
		res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, use_window)		
257
	else:
258
		res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, profile, use_window)
259
260
261
262
	t = res[1]["t_init_phases"];
	dt = res[1]["dt"];
	final_state = res[0]
	c0 =  res[1]["x_init"][0:3]
263
264
265
	dc0 = res[1]["x_init"][3:7]
	comPosPerPhase = __cVarPerPhase('c'  , dt, t, final_state, [c0])
	comVelPerPhase = __cVarPerPhase('dc' , dt, t, final_state, [dc0])
266
267
	comAccPerPhase = __cVarPerPhase('ddc', dt, t, final_state, [[0,0,0]])
		
268
269
	#now compute com trajectorirs
	comTrajIds = [fullBody.generateComTraj(comPosPerPhase[i], comVelPerPhase[i], comAccPerPhase[i], dt) for i in range(0,3)]
270
	return comTrajIds, res[2], final_state #res[2] is timeelapsed
271

272
def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1],
273
reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = []):
274
	comPosPerPhase, timeElapsed, final_state = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
275
	reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window)
276
277
	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)]
278
	print "done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1]
279
	return paths_ids[-1], paths_ids[:-1], timeElapsed, final_state
280
	
281
def solve_effector_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1],
282
reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = []):
283
	comPosPerPhase, timeElapsed, final_state = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
284
	reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window)
285
	print "done. generating state trajectory ",state_id		
286
287
288
289
290
291
	if(len(trackedEffectors) == 0):
		paths_ids = [int(el) for el in fullBody.effectorRRT(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
	else:
		print "handling extra effector constraints"
		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)]
292
	print "done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1]
293
	return paths_ids[-1], paths_ids[:-1], timeElapsed, final_state
294