geometry.poses

1 from . import (angle_from_rot2d, rotz, axis_angle_from_rotation, hat_map_2d, 2 contract, assert_allclose, np, rot2d, new_contract, 3 check_SO, check_skew_symmetric, expm, logm) 4 from geometry.constants import GeometryConstants

5 6 7 -def check_SE(M):

8 ''' Checks that the argument is in the special euclidean group. ''' 9 R, t, zero, one = extract_pieces(M) #@UnusedVariable 10 check_SO(R) 11 assert_allclose(one, 1, err_msg='I expect the lower-right to be 1') 12 assert_allclose(zero, 0, err_msg='I expect the bottom component to be 0.')

13

14 15 -def check_se(M):

16 ''' Checks that the input is in the special euclidean Lie algebra. ''' 17 omega, v, Z, zero = extract_pieces(M) #@UnusedVariable 18 check_skew_symmetric(omega) 19 assert_allclose(Z, 0, err_msg='I expect the lower-right to be 0.') 20 assert_allclose(zero, 0, err_msg='I expect the bottom component to be 0.')

21 22 23 new_contract('se', check_se) 24 new_contract('SE', check_SE) 25 new_contract('SE2', 'array[3x3], SE') 26 new_contract('se2', 'array[3x3], se') 27 new_contract('SE3', 'array[4x4], SE') 28 new_contract('se3', 'array[4x4], se') 29 30 31 @contract(x='array[NxN]', 32 returns='tuple(array[MxM],array[M],array[M],number),M=N-1')

33 -def extract_pieces(x):

34 M = x.shape[0] - 1 35 a = x[0:M, 0:M] 36 b = x[0:M, M] 37 c = x[M, 0:M] 38 d = x[M, M] 39 return a, b, c, d

40 41 42 @contract(a='array[MxM]', b='array[M]', c='array[M]', d='number', 43 returns='array[NxN],N=M+1')

44 -def combine_pieces(a, b, c, d):

45 M = a.shape[0] 46 x = np.zeros((M + 1, M + 1)) 47 x[0:M, 0:M] = a 48 x[0:M, M] = b 49 x[M, 0:M] = c 50 x[M, M] = d 51 return x

52

53 # TODO: specialize for SE2, SE3 54 55 56 @contract(returns='SE2') 57 -def SE2_identity():

58 return np.eye(3)

59

60 61 @contract(returns='SE3') 62 -def SE3_identity():

63 return np.eye(4)

64 65 66 @contract(R='array[NxN],SO', t='array[N]', returns='array[MxM],M=N+1,SE')

67 -def pose_from_rotation_translation(R, t):

68 return combine_pieces(R, t, t * 0, 1)

69 70 # TODO: make specialized 71 SE2_from_rotation_translation = pose_from_rotation_translation 72 SE3_from_rotation_translation = pose_from_rotation_translation

73 74 75 @contract(pose='array[NxN],SE', returns='tuple(array[MxM], array[M]),M=N-1') 76 -def rotation_translation_from_pose(pose):

77 R, t, zero, one = extract_pieces(pose) #@UnusedVariable 78 return R.copy(), t.copy()

79 80 # TODO: make more efficient 81 rotation_translation_from_SE2 = rotation_translation_from_pose 82 rotation_translation_from_SE3 = rotation_translation_from_pose

83 84 85 @contract(pose='SE2', returns='array[2]') 86 -def translation_from_SE2(pose):

87 # TODO: make it more efficient 88 R, t, zero, one = extract_pieces(pose) #@UnusedVariable 89 return t.copy()

90

91 92 @contract(pose='SE3', returns='array[3]') 93 -def translation_from_SE3(pose):

94 # TODO: make it more efficient 95 _, t, _, _ = extract_pieces(pose) 96 return t.copy()

97

98 99 @contract(t='array[2]|seq[2](number)', theta='number', returns='SE2') 100 -def SE2_from_translation_angle(t, theta):

101 ''' Returns an element of SE2 from translation and rotation. ''' 102 t = np.array(t) 103 return combine_pieces(rot2d(theta), t, t * 0, 1)

104

105 106 @contract(pose='SE2', returns='tuple(array[2],float)') 107 -def translation_angle_from_SE2(pose):

108 R, t, _, _ = extract_pieces(pose) 109 return t, angle_from_rot2d(R)

110

111 112 @contract(pose='SE2', returns='float') 113 -def angle_from_SE2(pose):

114 # XXX: untested 115 R, _, _, _ = extract_pieces(pose) 116 return angle_from_rot2d(R)

117

118 119 # TODO: write tests for this, and other function 120 @contract(xytheta='array[3]|seq[3](number)', returns='SE2') 121 -def SE2_from_xytheta(xytheta):

122 ''' Returns an element of SE2 from translation and rotation. ''' 123 return SE2_from_translation_angle([xytheta[0], xytheta[1]], xytheta[2])

124

125 126 @contract(linear='array[2]|seq[2](number)', angular='number', returns='se2') 127 -def se2_from_linear_angular(linear, angular):

128 ''' Returns an element of se2 from linear and angular velocity. ''' 129 linear = np.array(linear) 130 M = hat_map_2d(angular) 131 return combine_pieces(M, linear, linear * 0, 0)

132

133 134 @contract(vel='se2', returns='tuple(array[2],float)') 135 -def linear_angular_from_se2(vel):

136 M, v, Z, zero = extract_pieces(vel) #@UnusedVariable 137 omega = M[1, 0] 138 return v, omega

139

140 141 # TODO: add to docs 142 @contract(pose='SE2', returns='se2') 143 -def se2_from_SE2_slow(pose):

144 ''' Converts a pose to its Lie algebra representation. ''' 145 R, t, zero, one = extract_pieces(pose) #@UnusedVariable 146 # FIXME: this still doesn't work well for singularity 147 W = np.array(logm(pose).real) 148 M, v, Z, zero = extract_pieces(W) #@UnusedVariable 149 M = 0.5 * (M - M.T) 150 if np.abs(R[0, 0] - (-1)) < 1e-10: # XXX: threshold 151 # cannot use logarithm for log(-I), it gives imaginary solution 152 M = hat_map_2d(np.pi) 153 154 return combine_pieces(M, v, v * 0, 0)

155

156 157 @contract(pose='SE2', returns='se2') 158 -def se2_from_SE2(pose):

159 ''' 160 Converts a pose to its Lie algebra representation. 161 162 See Bullo, Murray "PD control on the euclidean group" for proofs. 163 ''' 164 R, t, zero, one = extract_pieces(pose) #@UnusedVariable 165 w = angle_from_rot2d(R) 166 167 w_abs = np.abs(w) 168 # FIXME: singularity 169 if w_abs < 1e-8: # XXX: threshold 170 a = 1 171 else: 172 a = (w_abs / 2) / np.tan(w_abs / 2) 173 A = np.array([[a, w / 2], [-w / 2, a]]) 174 175 v = np.dot(A, t) 176 w_hat = hat_map_2d(w) 177 return combine_pieces(w_hat, v, v * 0, 0)

178

179 180 @contract(returns='SE2', vel='se2') 181 -def SE2_from_se2(vel):

182 ''' Converts from Lie algebra representation to pose. 183 184 See Bullo, Murray "PD control on the euclidean group" for proofs. 185 ''' 186 w = vel[1, 0] 187 R = rot2d(w) 188 v = vel[0:2, 2] 189 if np.abs(w) < 1e-8: # XXX threshold 190 R = np.eye(2) 191 t = v 192 else: 193 A = np.array([ 194 [np.sin(w), np.cos(w) - 1], 195 [1 - np.cos(w), np.sin(w)] 196 ]) / w 197 t = np.dot(A, v) 198 return combine_pieces(R, t, t * 0, 1)

199

200 201 @contract(returns='SE2', vel='se2') 202 -def SE2_from_se2_slow(vel):

203 X = expm(vel) 204 X[2, :] = [0, 0, 1] 205 return X

206

207 208 @contract(pose='SE2', returns='SE3') 209 -def SE3_from_SE2(pose):

210 ''' Embeds a pose in SE2 to SE3, setting z=0 and upright. ''' 211 t, angle = translation_angle_from_SE2(pose) 212 return pose_from_rotation_translation(rotz(angle), 213 np.array([t[0], t[1], 0]))

214

215 216 @contract(pose='SE3', returns='SE2') 217 -def SE2_from_SE3(pose, check_exact=True, z_atol=1e-6):

218 ''' 219 Projects a pose in SE3 to SE2. 220 221 If check_exact is True, it will check that z = 0 and axis ~= [0,0,1]. 222 ''' 223 rotation, translation = rotation_translation_from_pose(pose) 224 axis, angle = axis_angle_from_rotation(rotation) 225 if check_exact: 226 sit = '\n pose %s' % pose 227 sit += '\n axis: %s' % axis 228 sit += '\n angle: %s' % angle 229 230 err_msg = ('I expect that z=0 when projecting to SE2 ' 231 '(check_exact=True).') 232 err_msg += sit 233 234 assert_allclose(translation[2], 0, atol=z_atol, err_msg=err_msg) 235 # normalize angle z 236 axis2 = axis * np.sign(axis[2]) 237 err_msg = ('I expect that the rotation is around [0,0,1] ' 238 'when projecting to SE2 (check_exact=True).') 239 err_msg += sit 240 241 assert_allclose(axis2, [0, 0, 1], 242 rtol=GeometryConstants.rtol_SE2_from_SE3, 243 atol=GeometryConstants.rtol_SE2_from_SE3, # XXX 244 err_msg=err_msg) 245 246 angle = angle * np.sign(axis[2]) 247 return SE2_from_translation_angle(translation[0:2], angle)

248

Source Code for Module geometry.poses