Hide keyboard shortcuts

Hot-keys on this page

r m x p   toggle line displays

j k   next/prev highlighted chunk

0   (zero) top of page

1   (one) first highlighted chunk

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66

67

68

69

70

71

72

73

74

75

76

77

78

79

80

81

82

83

84

85

86

87

88

89

90

91

92

93

94

95

96

97

98

99

100

101

102

103

104

105

106

107

108

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

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

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

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

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

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

""" 

""" 

import numpy as np 

from navipy.trajectories.triangle import Triangle 

from navipy.maths import homogeneous_transformations as ht 

from scipy.optimize import minimize 

 

_modes = [] 

for axel in ['x-axis', 'y-axis', 'z-axis']: 

for marker in range(3): 

_modes.append(axel + '=median-from-{}'.format(marker)) 

 

for axel in ['x-axis', 'y-axis', 'z-axis']: 

for marki in range(3): 

for markj in range(3): 

if marki == markj: 

continue 

_modes.append(axel + '={}-{}'.format(marki, markj)) 

 

 

def determine_mode(mode): 

cmode = mode.split('=') 

if len(cmode) != 2: 

raise KeyError('mode should contain one and only one sign =') 

axes = ['x-axis', 'y-axis', 'z-axis'] 

if cmode[0] not in axes: 

raise KeyError( 

'left side of sign = should be {}, {}, or {}'.format( 

axes[0], axes[1], axes[2])) 

axel = cmode[0] 

cmode = cmode[1].split('-') 

if len(cmode) == 2: 

apexes = (int(cmode[0]), int(cmode[1])) 

method = 'aligned-with' 

elif len(cmode) == 3: 

apexes = int(cmode[-1]) 

method = cmode[0] + '-' + cmode[1] 

return axel, apexes, method 

 

 

def normalise_vec(vec): 

"""Normalise vector when its norm is >0""" 

# assert np.linalg.norm(vec) > 0, 'vec axis has a null norm' 

if np.linalg.norm(vec) > 0: 

vec = vec / np.linalg.norm(vec) 

else: 

vec = vec * np.nan 

return vec 

 

 

def median_from(triangle, axel, marker): 

"""One axel of the body is assumed to be aligned with the median \ 

from an apex of the triangle. 

""" 

# find next marker 

next_marker = marker + 1 

if next_marker > 2: 

next_marker = 0 

medians = triangle.medians() 

vectors = triangle.apexes2vectors() 

x_axis = medians.loc[:, marker] - triangle.apexes.apex0 

x_axis = x_axis.loc[['x', 'y', 'z']].values 

if ((marker, next_marker) == (0, 1)) or \ 

((marker, next_marker) == (1, 2)) or \ 

((marker, next_marker) == (2, 0)): 

vector = vectors.loc[['x', 'y', 'z'], (marker, next_marker)] 

elif ((next_marker, marker) == (0, 1)) or \ 

((next_marker, marker) == (1, 2)) or \ 

((next_marker, marker) == (2, 0)): 

vector = -vectors.loc[['x', 'y', 'z'], (next_marker, marker)] 

else: 

raise KeyError( 

'Can not compute median from with apexes {}'.format(marker)) 

z_axis = np.cross(vector.values, 

x_axis) 

y_axis = np.cross(z_axis, x_axis) 

x_axis = normalise_vec(x_axis) 

y_axis = normalise_vec(y_axis) 

z_axis = normalise_vec(z_axis) 

if axel == 'x-axis': 

return x_axis, y_axis, z_axis 

elif axel == 'y-axis': 

return y_axis, z_axis, x_axis 

elif axel == 'z-axis': 

return z_axis, x_axis, y_axis 

else: 

raise KeyError('{} axis is not supported'.format(axel)) 

 

return x_axis, y_axis, z_axis 

 

 

def aligned_with(triangle, axel, markers): 

"""One axel of the body is assumed to be aligned with one edge of the triangle. 

""" 

# find third marker 

third_marker = None 

for mark in [0, 1, 2]: 

if mark not in markers: 

third_marker = mark 

break 

 

vectors = triangle.apexes2vectors() 

if (markers == (0, 1)) or \ 

(markers == (1, 2)) or \ 

(markers == (2, 0)): 

y_axis = vectors.loc[:, (markers[0], markers[1])] 

elif (markers[::-1] == (0, 1)) or \ 

(markers[::-1] == (1, 2)) or \ 

(markers[::-1] == (2, 0)): 

y_axis = - vectors.loc[['x', 'y', 'z'], 

(markers[1], markers[0])] 

else: 

raise KeyError( 

'Can not compute aligned-with with apexes {}, {}'.format( 

markers[0], markers[1])) 

 

y_axis = y_axis.loc[['x', 'y', 'z']].values 

if ((markers[0], third_marker) == (0, 1)) or \ 

((markers[0], third_marker) == (1, 2)) or \ 

((markers[0], third_marker) == (2, 0)): 

vector = - vectors.loc[['x', 'y', 'z'], 

(markers[0], third_marker)] 

elif ((third_marker, markers[0]) == (0, 1)) or \ 

((third_marker, markers[0]) == (1, 2)) or \ 

((third_marker, markers[0]) == (2, 0)): 

vector = vectors.loc[['x', 'y', 'z'], 

(third_marker, markers[0])] 

else: 

raise KeyError( 

'Can not compute aligned-with with apexes {}, {}'.format( 

markers[0], markers[1])) 

 

z_axis = np.cross(vector, y_axis) 

x_axis = np.cross(y_axis, z_axis) 

 

x_axis = normalise_vec(x_axis) 

y_axis = normalise_vec(y_axis) 

z_axis = normalise_vec(z_axis) 

 

if axel == 'y-axis': 

return x_axis, y_axis, z_axis 

elif axel == 'x-axis': 

return y_axis, z_axis, x_axis 

elif axel == 'z-axis': 

return z_axis, x_axis, y_axis 

else: 

raise KeyError('{} axis is not supported'.format(axel)) 

 

 

def triangle2bodyaxis(triangle, mode): 

""" 

The center of mass of the triangle is the center of all axis of the body. 

The triangle may not be always placed in the same relative to \ 

the body axis. Therefore several methods can be used to compute \ 

the body axis from a three points placed on a body. Those methods \ 

are accesible via the mode. 

 

modes: 

- 'x-axis=median-from-0' 

- 'y-axis=1-2' 

""" 

axel, markers, method = determine_mode(mode) 

origin = triangle.center_of_mass() 

if method == 'median-from': 

x_axis, y_axis, z_axis = median_from(triangle, 

axel, markers) 

elif method == 'aligned-with': 

x_axis, y_axis, z_axis = aligned_with(triangle, 

axel, markers) 

else: 

print('supported modes') 

print('---------------') 

for m in _modes: 

print(m) 

raise ValueError('mode {} is not supported.'.format(mode)) 

return origin, x_axis, y_axis, z_axis 

 

 

def bodyaxistransformations(x_axis, y_axis, z_axis): 

frame = np.zeros((4, 4)) 

frame[:3, 0] = x_axis 

frame[:3, 1] = y_axis 

frame[:3, 2] = z_axis 

frame[3, 3] = 1 

return frame 

 

 

def triangle2homogeous_transform(triangle, triangle_mode): 

origin, x_axis, y_axis, z_axis = triangle2bodyaxis(triangle, triangle_mode) 

transform = bodyaxistransformations(x_axis, y_axis, z_axis) 

transform[:3, 3] = origin 

return transform 

 

 

def markers2decompose(mark0, mark1, mark2, triangle_mode, euler_axes): 

triangle = Triangle(mark0, mark1, mark2) 

transform = triangle2homogeous_transform(triangle, triangle_mode) 

return ht.decompose_matrix(transform, axes=euler_axes) 

 

 

def markers2euler(mark0, mark1, mark2, triangle_mode, euler_axes): 

_, angles = markers2posorient(mark0, mark1, mark2, 

triangle_mode, euler_axes) 

return angles 

 

 

def markers2translate(mark0, mark1, mark2, triangle_mode, euler_axes): 

translate, _ = markers2posorient(mark0, mark1, mark2, 

triangle_mode, euler_axes) 

return translate 

 

 

def markers2posorient(mark0, mark1, mark2, triangle_mode, euler_axes): 

_, _, angle, translate, _ = markers2decompose(mark0, mark1, mark2, 

triangle_mode, euler_axes) 

return translate, angle 

 

 

def twomarkers2euler_easy_euleraxes(mark0, mark1, axis_alignement, 

known_angle): 

""" Determine euler angles from two markers and a known angle 

 

The functions require a known angle express in the temporary \ 

rotation convention. 

 

The known angle is: 

 

* rotation around x, for axis alignment x-axis 

* rotation around y, for axis alignment y-axis 

* rotation around z, for axis alignment z-axis 

 

The temporary rotation convention used two determine the euler angles are: 

 

* zyx, for axis alignment x-axis 

* zxy, for axis alignment y-axis 

* yxz, for axis alignment z-axis 

 

Note: If you know the angle in your rotation convention, then you \ 

can run this function through a minimisation procedure with free parameter \ 

known angle until you get the desired orientation (see twomarkers2euler) 

 

""" 

vector = mark1 - mark0 

vector = normalise_vec(vector) 

if axis_alignement == 'x-axis': 

axes_convention = 'zyx' 

beta = np.arcsin(vector.z) 

alpha = np.arctan2(-vector.y / np.cos(beta), 

vector.x / np.cos(beta)) 

gamma = known_angle 

angles = [alpha, beta, gamma] 

elif axis_alignement == 'y-axis': 

axes_convention = 'zxy' 

gamma = np.arcsin(-vector.z) 

alpha = np.arctan2(vector.x / np.cos(gamma), 

vector.y / np.cos(gamma)) 

beta = known_angle 

angles = [alpha, gamma, beta] 

elif axis_alignement == 'z-axis': 

axes_convention = 'yxz' 

gamma = np.arcsin(vector.y) 

beta = np.arctan2(-vector.x / np.cos(gamma), 

vector.z / np.cos(gamma)) 

alpha = known_angle 

angles = [beta, gamma, alpha] 

else: 

raise KeyError( 

'Axis aligment {} is not supported'.format( 

axis_alignement)) 

return angles, axes_convention 

 

 

def twomarkers2euler_score(x, mark0, mark1, 

axis_alignement, known_angles, euler_axes): 

""" A root mean square score between wished angles and obtained \ 

angles from two markers 

 

This function is used by twomarkers2euler within the minimisation of scipy 

""" 

angles, axes_convention = twomarkers2euler_easy_euleraxes( 

mark0, mark1, axis_alignement, x[0]) 

matrix = ht.compose_matrix( 

angles=angles, axes=axes_convention) 

_, _, angles, _, _ = ht.decompose_matrix(matrix, axes=euler_axes) 

return np.nanmean((angles - known_angles)**2) 

 

 

def twomarkers2euler(mark0, mark1, axis_alignement, 

known_angle, euler_axes, 

method='SLSQP', tol=1e-8): 

""" Determine euler angles from two markers and a known angle. 

 

The known angle is assumed to be the one around the axis aligment. 

If euler_axes contains twice the same axis the first rotation axis is used. 

 

""" 

if axis_alignement not in ['x-axis', 'y-axis', 'z-axis']: 

raise KeyError( 

'Axis aligment {} is not supported'.format( 

axis_alignement)) 

known_angles = np.nan * np.zeros(3) 

# Find the first rotation axis 

index = euler_axes.find(axis_alignement[0]) 

if index < 0: 

raise KeyError( 

'Axis aligment {} can not work with euler_axes {}'.format( 

axis_alignement, euler_axes)) 

if (axis_alignement == 'x-axis') and (euler_axes != 'zyx'): 

msg = 'When x-axis is known, euler_axis zyx should be used\n' 

msg += 'Other convention have not been implemented, sorry' 

raise NameError(msg) 

elif (axis_alignement == 'z-axis') and (euler_axes != 'yxz'): 

msg = 'When z-axis is known, euler_axis yxz should be used\n' 

msg += 'Other convention have not been implemented, sorry' 

raise NameError(msg) 

elif (axis_alignement == 'y-axis') and (euler_axes != 'zxy'): 

msg = 'When y-axis is known, euler_axis zxy should be used\n' 

msg += 'Other convention have not been implemented, sorry' 

raise NameError(msg) 

 

known_angles[index] = known_angle 

args = (mark0, mark1, axis_alignement, known_angles, euler_axes) 

res = minimize(twomarkers2euler_score, x0=known_angle, args=args, 

method=method, tol=tol) 

angles, axes_convention = twomarkers2euler_easy_euleraxes( 

mark0, mark1, axis_alignement, res.x[0]) 

# Build rotation matrix and decompse 

matrix = ht.compose_matrix( 

angles=angles, axes=axes_convention) 

_, _, angles, _, _ = ht.decompose_matrix(matrix, axes=euler_axes) 

return angles