cwc_trajectory.py 9.47 KB
Newer Older
1
from cwc import cone_optimization
2
from obj_to_constraints import ineq_from_file, rotate_inequalities
3
import numpy as np
4
import math
5
from numpy.linalg import norm
6
import time
7
8

import hpp.corbaserver.rbprm.data.com_inequalities as ine
9
10
from hpp.corbaserver.rbprm.tools.path_to_trajectory import gen_trajectory_to_play

11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
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]])
28
29
30
31

def __get_com(robot, config):
	save = robot.getCurrentConfig()
	robot.setCurrentConfig(config)
32
	com = robot.getCenterOfMass()
33
34
35
	robot.setCurrentConfig(save)
	return com

36
37
38
39
40
41
constraintsComLoaded = {}

def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm = False):
	global constraintsLoaded
	As = [];	bs = []
	fullBody.setCurrentConfig(config)
Steve Tonneau's avatar
Steve Tonneau committed
42
	contacts = []
43
44
45
	for i, v in limbsCOMConstraints.iteritems():
		if not constraintsComLoaded.has_key(i):
			constraintsComLoaded[i] = ineq_from_file(ineqPath+v['file'])
46
47
48
		#~ print "inter", interm
		#~ print "intermed", fullBody.isLimbInContactIntermediary(i, state)
		#~ print "inter", fullBody.isLimbInContact(i, state)
49
50
51
52
		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
53
54
			tr = quaternion_matrix(qEffector[3:7])			
			tr[:3,3] = np.array(qEffector[0:3])
55
56
			ineq_r = rotate_inequalities(ineq, tr)
			As.append(ineq_r.A); bs.append(ineq_r.b);
57
			#~ print 'contact', v['effector']
Steve Tonneau's avatar
Steve Tonneau committed
58
			contacts.append(v['effector'])
59
	#~ print 'contacts', contacts
60
61
62
	return [np.vstack(As), np.hstack(bs)]
		

63
def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, phase_dt = [0.4, 1], reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False):
64
65
66
67
	init_com = __get_com(fullBody, states[state_id])
	end_com = __get_com(fullBody, states[state_id+1])
	p, N = fullBody.computeContactPoints(state_id)
	mass = fullBody.getMass()
68
69
	fly_time = phase_dt [1]
	support_time = phase_dt [0]
70
	t_end_phases = [0]
71
72
73
74
75
76
	[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]
77
78
		if (not profile):
			print "end_phases", t_end_phases
79
80
	cones = None
	if(computeCones):
81
		cones = [fullBody.getContactCone(state_id, mu)[0]]
Steve Tonneau's avatar
Steve Tonneau committed
82
		if(len(p) > 2):
83
			cones.append(fullBody.getContactIntermediateCone(state_id, mu)[0])
84
		if(len(p) > len(cones)):
85
			cones.append(fullBody.getContactCone(state_id+1, mu)[0])
86
87
	if (not profile):
			print "num cones ", len(cones)
88
89
90
	
	COMConstraints = None
	if(not (limbsCOMConstraints == None)):
91
		#~ print "retrieving COM constraints"
92
93
94
95
96
		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))
97
		#~ print "num com constraints", len(COMConstraints)
98
99
100
101
102
103
104
105
	timeelapsed = 0
	if (profile):
		start = time.clock() 
	var_final, params = cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, cones, COMConstraints, mu, mass, 9.81, reduce_ineq, verbose)	
	if (profile):
		end = time.clock() 
		timeelapsed = (end - start) * 1000
	return var_final, params, timeelapsed
106

107
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):
108
	var_final, params, elapsed = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, False)
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
	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
	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')

134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
	plt.show()
	
	print "plotting speed "
	print "end target ",  params['x_end']
	fig = plt.figure()
	ax = fig.add_subplot(111)
	points = var_final['dc']
	print "points", points
	ys = [norm(el) 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 acceleration "
	fig = plt.figure()
	ax = fig.add_subplot(111)
	points = var_final['ddc']
	ys = [norm(el) for el in points]
	xs = [i * params['dt'] for i in range(0,len(points))]
	ax.scatter(xs, ys, c='b')


	plt.show()
	

161
	plt.show()
162
	return var_final, params, elapsed
163
	
164
def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1], reduce_ineq = True,
165
 num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False):
166
167
	print "callgin gen ",state_id
	if(draw):
168
		res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints)		
169
	else:
170
		res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, profile)
171
172
173
174
175
	t = res[1]["t_init_phases"];
	dt = res[1]["dt"];
	final_state = res[0]
	c0 =  res[1]["x_init"][0:3]
	comPos = [c0] + [c.tolist() for c in final_state['c']]
176
	comPosPerPhase = [[comPos[(int)(t_id/dt)] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1]) ]
177
178
	comPosPerPhase[-1].append(comPos[-1])
	assert(len(comPos) == len(comPosPerPhase[0]) + len(comPosPerPhase[1]) + len(comPosPerPhase[2]))
179
	return comPosPerPhase, res[2] #res[2] is timeelapsed
180

181
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):
182
	comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile)
183
	print "done. generating state trajectory ",state_id	
184
	print "comePhaseLength", len(comPosPerPhase[0]),  len(comPosPerPhase[1]),  len(comPosPerPhase[2])
185
186
	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
187
	return paths_ids[-1], paths_ids[:-1], timeElapsed
188
	
189
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):
190
	comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile)
191
192
193
194
	print "done. generating state trajectory ",state_id	
	print "comePhaseLength", len(comPosPerPhase[0]),  len(comPosPerPhase[1]),  len(comPosPerPhase[2])
	paths_ids = [int(el) for el in fullBody.effectorRRT(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
	print "done. computing final trajectory to display ",state_id
195
	return paths_ids[-1], paths_ids[:-1], timeElapsed
196
	
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
#~ from multiprocessing import Process	

#~ def solve_com_RRTs(fullBody, states, state_ids, computeCones = False, mu = 1, dt =0.1, phase_dt = 1, reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None):
	#~ results = {}
	#~ processes = {}
	#~ allpathsids =[[],[]]
	#~ errorid = []
	#~ for sid in state_ids:
		#~ pid = str(sid)
		#~ p = Process(target=__optim__threading_ok, args=(fullBody, states, sid, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, results))
		#~ processes[str(sid)] = p
		#~ p.start()
	#~ for i,p in processes.iteritems():
		#~ p.join()
	#~ print results
	#~ print "done. generating state trajectory "
	#~ for sid in state_ids:
		#~ comPosPerPhase = results[str(sid)]
		#~ try:
			#~ 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
			#~ allpathsids[0].append(paths_ids[-1])
			#~ allpathsids[1].append(paths_ids[:-1])
		#~ except:
			#~ errorid += [i]
	#~ print "errors at states: "; errorid
	#~ return allpathsids[0], allpathsids[1]