# Commits

committed 6520e00 Merge

Merge with dev

• Participants
• Parent commits 0c16258, effd538
• Branches dev-utils

# File ars/utils/geometry.py

 import generic as gut


+rtb_license = """RTB (The Robotics Toolbox for Matlab) is free software: you
+can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+"""
+
+
+def _rot_matrix_to_rpy_angles(rot_matrix, zyx=False):
+	"""The roll-pitch-yaw angles corresponding to a rotation matrix.
+	The 3 angles RPY correspond to sequential rotations about the X, Y and Z
+	axes respectively.
+
+	WARNING: for the convention where Y axis points upwards, swap the returned
+	pitch and yaw. The input remains the same.
+
+
+	Translated to Python by German Larrain.
+
+	Original version in Matlab, part of	'The Robotics Toolbox for Matlab (RTB)'
+	as '/robot/tr2rpy.m'
+	Copyright (C) 1993-2011, by Peter I. Corke. See rtb_license.
+
+	"""
+	m = mut.matrix_as_3x3_tuples(rot_matrix)
+
+	# "eps: distance from 1.0 to the next largest double-precision number"
+	eps = 2e-52 # http://www.mathworks.com/help/techdoc/ref/eps.html
+
+	rpy_1 = 0.0
+	rpy_2 = 0.0
+	rpy_3 = 0.0
+
+	if not zyx:
+		# XYZ order
+		if abs(m[2][2]) < eps and abs(m[1][2]) < eps:  # if abs(m(3,3)) < eps && abs(m(2,3)) < eps
+			# singularity
+			rpy_1 = 0.0
+			rpy_2 = mut.atan2(m[0][2], m[2][2])  # atan2(m(1,3), m(3,3))
+			rpy_3 = mut.atan2(m[1][0], m[1][1])  # atan2(m(2,1), m(2,2))
+		else:
+			rpy_1 = mut.atan2(-m[1][2], m[2][2])  # atan2(m(2,1), m(2,2))
+			# compute sin/cos of roll angle
+			sr = mut.sin(rpy_1)  # sr = sin(rpy(1))
+			cr = mut.cos(rpy_1)  # cr = cos(rpy(1))
+			rpy_2 = mut.atan2(m[0][2], cr * m[2][2] - sr * m[1][2])  # atan2(m(1,3), cr * m(3,3) - sr * m(2,3))
+			rpy_3 = mut.atan2(-m[0][1], m[0][0])  # atan2(-m(1,2), m(1,1))
+	else:
+		# old ZYX order (as per Paul book)
+		if abs(m[0][0]) < eps and abs(m[1][0]) < eps:  # if abs(m(1,1)) < eps && abs(m(2,1)) < eps
+			# singularity
+			rpy_1 = 0.0
+			rpy_2 = mut.atan2(-m[2][0], m[0][0])  # atan2(-m(3,1), m(1,1))
+			rpy_3 = mut.atan2(-m[1][2], m[1][1])  # atan2(-m(2,3), m(2,2))
+		else:
+			rpy_1 = mut.atan2(m[1][0], m[0][0])  	# atan2(m(2,1), m(1,1))
+			sp = mut.sin(rpy_1)  					# sp = sin(rpy(1))
+			cp = mut.cos(rpy_1)  					# cp = cos(rpy(1))
+			rpy_2 = mut.atan2(-m[2][0],  			# atan2(-m(3,1),
+				cp * m[0][0] + sp * m[1][0])  		# cp * m(1,1) + sp * m(2,1))
+			rpy_3 = mut.atan2(sp * m[0][2] - cp * m[1][2],  # atan2(sp * m(1,3) - cp * m(2,3),
+				cp * m[1][1] - sp * m[0][1])  				# cp*m(2,2) - sp*m(1,2))
+
+	return rpy_1, rpy_2, rpy_3
+
+
 class Transform:
 	def __init__(self, position, rot_matrix):
 		"""
 	\cos \theta + u_z^2\left(1-\cos \theta\right)
 	\end{bmatrix}
 	[/itex]
+
+	The returned matrix format is length-9 tuple.
+
 	"""

 	cos_theta = mut.cos(angle)
 	sin_theta = mut.sin(angle)
 	t = 1.0 - cos_theta
-	return (
-	t * axis[0]**2 + cos_theta,
-	t * axis[0] * axis[1] - sin_theta * axis[2],
-	t * axis[0] * axis[2] + sin_theta * axis[1],
-	t * axis[0] * axis[1] + sin_theta * axis[2],
-	t * axis[1]**2 + cos_theta,
-	t * axis[1] * axis[2] - sin_theta * axis[0],
-	t * axis[0] * axis[2] - sin_theta * axis[1],
-	t * axis[1] * axis[2] + sin_theta * axis[0],
-	t * axis[2]**2 + cos_theta)
+	return (t * axis[0]**2 + cos_theta,
+		t * axis[0] * axis[1] - sin_theta * axis[2],
+		t * axis[0] * axis[2] + sin_theta * axis[1],
+		t * axis[0] * axis[1] + sin_theta * axis[2],
+		t * axis[1]**2 + cos_theta,
+		t * axis[1] * axis[2] - sin_theta * axis[0],
+		t * axis[0] * axis[2] - sin_theta * axis[1],
+		t * axis[1] * axis[2] + sin_theta * axis[0],
+		t * axis[2]**2 + cos_theta)


 def make_OpenGL_matrix(rotation, position):
 	"""Returns an OpenGL compatible (column-major, 4x4 homogeneous)
 	transformation matrix from ODE compatible (row-major, 3x3) rotation matrix
-	rotation and position vector position"""
-	return (
-	rotation[0], rotation[3], rotation[6], 0.0,
-	rotation[1], rotation[4], rotation[7], 0.0,
-	rotation[2], rotation[5], rotation[8], 0.0,
-	position[0], position[1], position[2], 1.0)
+	rotation and position vector position.
+
+	The returned matrix format is length-9 tuple.
+
+	"""
+	return (rotation[0], rotation[3], rotation[6], 0.0,
+		rotation[1], rotation[4], rotation[7], 0.0,
+		rotation[2], rotation[5], rotation[8], 0.0,
+		position[0], position[1], position[2], 1.0)


 def get_body_relative_vector(body, vector):


 def rot_matrix_to_euler_angles(rot_matrix):
-	"""Returns the 3-1-3 Euler angles 'phi', 'theta' and 'psi' (using the
+	"""Returns the 3-1-3 Euler angles phi, theta and psi (using the
 	x-convention) corresponding to the rotation matrix rot_matrix, which
 	is a tuple of three 3-element tuples, where each one is a row (what is
 	called row-major order).

-	Using the x-convention, the 3-1-3 Euler angles 'phi', 'theta' and 'psi'
+	Using the x-convention, the 3-1-3 Euler angles phi, theta and psi
 	(around	the Z, X and again the Z-axis) can be obtained as follows
 		phi = arctan2(A_{31}, A_{32})
 		theta = arccos(A_{33})
 		psi = -arctan2(A_{13}, A_{23})
- 	http://en.wikipedia.org/wiki/Rotation_representation_(mathematics)#Rotation_matrix_.E2.86.94_Euler_angles
+
+ 	http://en.wikipedia.org/wiki/Rotation_representation_(mathematics)#
+ 		Rotation_matrix_.E2.86.94_Euler_angles
+
 	"""
 	A = rot_matrix
 	phi = mut.atan2(A[2][0], A[2][1])		# arctan2(A_{31}, A_{32})
 	return angles


-def calc_inclination(rot_matrix, axis=mut.upAxis):
-	"""Returns the inclination of an orientation -defined by the rotation matrix
-	rot_matrix- with respect to a plane orthogonal to the given axis, which
-	defaults to the unit vector pointing upwards as defined in this module.
+def calc_inclination(rot_matrix):
+	"""Returns the inclination (as pitch and roll) inherent of a rotation
+	matrix rot_matrix, with respect to the plane (XZ, since the	vertical
+	axis is Y). pitch is the rotation around Z axis and roll
+	around Y.

-	Example:
-	If rot_matrix = calc_rotation_matrix(rightAxis, pi/4)
+	Examples:
+		rot_matrix = calc_rotation_matrix((1.0, 0.0, 0.0), pi/6)
+		pitch, roll = gemut.calc_inclination(rot_matrix)
+		pitch: 0.0, roll: pi/6
+
+		rot_matrix = calc_rotation_matrix((0.0, 1.0, 0.0), whatever)
+		pitch, roll = gemut.calc_inclination(rot_matrix)
+		pitch: 0.0, roll: 0.0
+
+		rot_matrix = calc_rotation_matrix((0.0, 0.0, 1.0), pi/6)
+		pitch, roll = gemut.calc_inclination(rot_matrix)
+		pitch: pi/6, roll: 0.0
+
 	"""
-	# TODO: complete docstrings
-	# TODO: check tests
-	rotated_axis = mut.rotate3(rot_matrix, axis)
-	angle = mut.acos_dot3(axis, rotated_axis)
-	return angle
+	# THE FOLLOWING worked only in some cases, damn
+	#y_up = mut.upAxis
+	#z_front = mut.bkwdAxis
+	#x_right = mut.rightAxis
+	#
+	#up_rotated = mut.rotate3(rot_matrix, y_up)
+	#pitch_proj = mut.dot_product(mut.cross_product(y_up, up_rotated), x_right)
+	#pitch =  mut.sign(pitch_proj) * mut.acos_dot3(y_up, up_rotated)
+	#
+	#front_rotated = mut.rotate3(rot_matrix, z_front)
+	#roll_proj = mut.dot_product(mut.cross_product(z_front, front_rotated), y_up)
+	#roll = mut.sign(roll_proj) * mut.acos_dot3(z_front, front_rotated)
+	#
+	#return pitch, roll
+
+	roll_x, pitch_y, yaw_z = _rot_matrix_to_rpy_angles(rot_matrix)
+	roll = roll_x
+	pitch = yaw_z
+	#yaw = pitch_y  # we don't need it
+	return pitch, roll

# File ars/utils/mathematical.py

 deal with homogeneous transforms; convert angles and other structures.
 """

-#import itertools
+import itertools
 from math import sqrt, pi, cos, sin, acos, atan, atan2, degrees, radians
-#import operator
+import operator

 import numpy as np

 downRot = (1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.0, 0.0)
 bkwdRot = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)

+X_AXIS = (1.0, 0.0, 0.0)
+X_AXIS_NEG = (-1.0, 0.0, 0.0)
+Y_AXIS = (0.0, 1.0, 0.0)
+Y_AXIS_NEG = (0.0, -1.0, 0.0)
+Z_AXIS = (0.0, 0.0, 1.0)
+Z_AXIS_NEG = (0.0, 0.0, -1.0)
+
 # axes used to determine constrained joint rotations
-rightAxis = (1.0, 0.0, 0.0)
-leftAxis = (-1.0, 0.0, 0.0)
-upAxis = (0.0, 1.0, 0.0)
-downAxis = (0.0, -1.0, 0.0)
-bkwdAxis = (0.0, 0.0, 1.0)
-fwdAxis = (0.0, 0.0, -1.0)
+rightAxis = X_AXIS
+leftAxis = X_AXIS_NEG
+upAxis = Y_AXIS
+downAxis = Y_AXIS_NEG
+bkwdAxis = Z_AXIS  # direction: out of the screen
+fwdAxis = Z_AXIS_NEG  # direction: into the screen

 #===============================================================================
 # added to the original refactored code

 def norm3(vector):
 	"""Returns the unit length 3-vector parallel to 3-vector vector"""
-	#TODO: convert it so it can handle vector of any dimension
-	l = length3(vector)
-	if l > 0.0:
-		return (vector[0] / l, vector[1] / l, vector[2] / l)
+	#l = length3(vector)
+	#if l > 0.0:
+	#	return (vector[0] / l, vector[1] / l, vector[2] / l)
+	#else:
+	#	return (0.0, 0.0, 0.0)
+	return unitize(vector)
+
+def unitize(vector_):
+	"""Unitize a vector, i.e. return a unit-length vector parallel to vector
+
+	"""
+	len_ = sqrt(sum(itertools.imap(operator.mul, vector_, vector_)))
+	size_ = len(vector_)
+
+	if len_ > 0.0:
+		div_vector = (len_,) * size_  # (len_, len_, len_, ...)
+		return tuple(itertools.imap(operator.div, vector_, div_vector))
 	else:
 		return (0.0, 0.0, 0.0)

 	return sum(itertools.imap(operator.mul, vec1, vec2))

 def cross_product(vector1, vector2):
-	"""Returns the cross_product product of 3-vectors vector1 and vector2"""
+	"""Returns the cross_product product of length-3 vectors vector1 and
+	vector2
+
+	"""
 	return (vector1[1] * vector2[2] - vector1[2] * vector2[1],
 		vector1[2] * vector2[0] - vector1[0] * vector2[2],
 		vector1[0] * vector2[1] - vector1[1] * vector2[0])

# File tests/utilities_containers.py

-
-# Created on 2012.05.03
-#
-# @author: german
-
-import ars.utils.containers as cont
-
-def test_Queue():
-	q = cont.Queue()
-	print(q.is_empty())
-	print(q.count())
-
-	q.put('first element')
-	q.put('second')
-	q.put(('third', {'key 1': 1, 'another key': []}))
-	print(q.count())
-
-	print(q.pull())
-	print(q.pull())
-
-	print(q.count())
-	q.put('after some pulls')
-	print(q.count())
-
-	print(q.pull())
-	print(q.pull())
-	print(q.count())
-
-if __name__ == "__main__":
-	test_Queue()

# File tests/utilities_generic.py

-
-# Created on 2012.05.03
-#
-# @author: german
-
-import ars.utils.generic as gen
-
-def test_get_current_epoch_time():
-	print(repr(gen.get_current_epoch_time()))
-	print(repr(gen.get_current_epoch_time()))
-
-if __name__ == "__main__":
-	test_get_current_epoch_time()

# File tests/utilities_geometry.py

-
-# TODO: create a test for calc_rotation_matrix
-
-import numpy as np
-
-import ars.utils.geometry as gemut
-import ars.utils.mathematical as mut
-
-
-def _test_Transform():
-
-	pos = (10,20,30)
-	rot = (0,0,1,1,0,0,0,1,0)
-	t = gemut.Transform(pos, rot)
-
-	print(pos)
-	print(rot)
-	print(t)
-	print(t.get_long_tuple())
-
-def _test_rot_matrix_to_hom_transform():
-
-	rot1 = np.mat([[1,2,3],[4,5,6],[7,8,9]])
-	rot2 = ((1,2,3),(4,5,6),(7,8,9))
-	rot3 = (1,2,3,4,5,6,7,8,9)
-	ht1 = gemut.rot_matrix_to_hom_transform(rot1)
-	ht2 = gemut.rot_matrix_to_hom_transform(rot2)
-	ht3 = gemut.rot_matrix_to_hom_transform(rot3)
-
-	#print(rot1)
-	#print(rot2)
-	print(ht1)
-	print(ht2)
-	print(ht3)
-
-def test_get_inclination(axis, angle, correct):
-	#axis = mut.upAxis
-	#angle = mut.pi/4
-	rot_matrix = gemut.calc_rotation_matrix(axis, angle)
-	res = gemut.calc_inclination(rot_matrix, mut.upAxis)
-	print('angle: %f' % angle)
-	print('tilt: %f' % res)
-	print('error: %f' % (correct - res))
-	print('')
-
-if __name__ == "__main__":
-
-	_test_rot_matrix_to_hom_transform()
-
-	_test_Transform()
-
-	# rotation around axii perpendicular to the ground does not affect
-	# the inclination
-	test_get_inclination(mut.upAxis, mut.pi/4, 0)
-	test_get_inclination(mut.upAxis, 3*mut.pi/9, 0)
-	test_get_inclination(mut.upAxis, -mut.pi/8, 0)
-	test_get_inclination(mut.downAxis, mut.pi/4, 0)
-	test_get_inclination(mut.downAxis, 3*mut.pi/9, 0)
-	test_get_inclination(mut.downAxis, -mut.pi/8, 0)
-
-	test_get_inclination(mut.rightAxis, mut.pi/6, mut.pi/6)
-	test_get_inclination(mut.rightAxis, 3*mut.pi/9, 3*mut.pi/9)
-	test_get_inclination(mut.rightAxis, -mut.pi/8, -mut.pi/8)

# File tests/utilities_mathematical.py

-
-# Created on 2011.12.01
-#
-# @author: german
-
-import ars.utils.mathematical as mut
-
-def test_matrix_multiply():
-	print (mut.matrix3_multiply(((2,0,0),(0,1,0),(0,0,1)), ((1,0,0),(0,2,0),(0,0,1))))
-
-if __name__ == "__main__":
-	#import sys;sys.argv = ['', 'Test.testName']
-	test_matrix_multiply()

# File tests/utils_containers.py

+
+# Created on 2012.05.03
+#
+# @author: german
+
+import ars.utils.containers as cont
+
+def test_Queue():
+	q = cont.Queue()
+	print(q.is_empty())
+	print(q.count())
+
+	q.put('first element')
+	q.put('second')
+	q.put(('third', {'key 1': 1, 'another key': []}))
+	print(q.count())
+
+	print(q.pull())
+	print(q.pull())
+
+	print(q.count())
+	q.put('after some pulls')
+	print(q.count())
+
+	print(q.pull())
+	print(q.pull())
+	print(q.count())
+
+if __name__ == "__main__":
+	test_Queue()

# File tests/utils_generic.py

+
+# Created on 2012.05.03
+#
+# @author: german
+
+import ars.utils.generic as gen
+
+def test_get_current_epoch_time():
+	print(repr(gen.get_current_epoch_time()))
+	print(repr(gen.get_current_epoch_time()))
+
+if __name__ == "__main__":
+	test_get_current_epoch_time()

# File tests/utils_geometry.py

+
+# TODO: create a test for calc_rotation_matrix
+
+import numpy as np
+
+import ars.utils.geometry as gemut
+import ars.utils.mathematical as mut
+
+
+def _test_Transform():
+
+	pos = (10,20,30)
+	rot = (0,0,1,1,0,0,0,1,0)
+	t = gemut.Transform(pos, rot)
+
+	print(pos)
+	print(rot)
+	print(t)
+	print(t.get_long_tuple())
+
+def _test_rot_matrix_to_hom_transform():
+
+	rot1 = np.mat([[1,2,3],[4,5,6],[7,8,9]])
+	rot2 = ((1,2,3),(4,5,6),(7,8,9))
+	rot3 = (1,2,3,4,5,6,7,8,9)
+	ht1 = gemut.rot_matrix_to_hom_transform(rot1)
+	ht2 = gemut.rot_matrix_to_hom_transform(rot2)
+	ht3 = gemut.rot_matrix_to_hom_transform(rot3)
+
+	#print(rot1)
+	#print(rot2)
+	print(ht1)
+	print(ht2)
+	print(ht3)
+
+def test_calc_inclination(axis, angle):
+	#axis = mut.upAxis
+	#angle = mut.pi/4
+	rot_matrix = gemut.calc_rotation_matrix(axis, angle)
+	pitch, roll = gemut.calc_inclination(rot_matrix)
+	print('axis: %s, angle: %f' % (axis, angle))
+	print('pitch: %f, roll: %f' % (pitch, roll))
+	print('')
+
+if __name__ == "__main__":
+
+	_test_rot_matrix_to_hom_transform()
+
+	_test_Transform()
+
+	# rotation around axii perpendicular to the ground does not affect
+	# the inclination
+	test_calc_inclination(mut.upAxis, mut.pi/4)
+	test_calc_inclination(mut.upAxis, 3*mut.pi/9)
+	test_calc_inclination(mut.upAxis, -mut.pi/8)
+	test_calc_inclination(mut.downAxis, mut.pi/4)
+	test_calc_inclination(mut.downAxis, 3*mut.pi/9)
+	test_calc_inclination(mut.downAxis, -mut.pi/8)
+
+	test_calc_inclination(mut.rightAxis, mut.pi/6)
+	test_calc_inclination(mut.rightAxis, 3*mut.pi/9)
+	test_calc_inclination(mut.rightAxis, -mut.pi/8)
+	test_calc_inclination(mut.leftAxis, mut.pi/8)
+	test_calc_inclination(mut.bkwdAxis, mut.pi/8)
+

# File tests/utils_mathematical.py

+
+# Created on 2011.12.01
+#
+# @author: german
+
+import ars.utils.mathematical as mut
+
+
+def _vector_diff_length(v1, v2):
+	return mut.length3(mut.sub3(v1, v2))
+
+
+def test_matrix_multiply():
+	print (mut.matrix3_multiply(((2,0,0),(0,1,0),(0,0,1)),
+		((1,0,0),(0,2,0),(0,0,1))))
+
+
+def test_XYZ_axes_for_orthogonality():
+	X = mut.X_AXIS
+	Y = mut.Y_AXIS
+	Z = mut.Z_AXIS
+	print(_vector_diff_length(mut.cross_product(X, Y), Z))
+	print(_vector_diff_length(mut.cross_product(Z, X), Y))
+	print(_vector_diff_length(mut.cross_product(Y, Z), X))
+
+
+if __name__ == "__main__":
+
+	test_matrix_multiply()
+	test_XYZ_axes_for_orthogonality()