# Commits

committed eca7783 Merge

Merge with dev

• Participants
• Parent commits d5971fe, ab0ca24
• Branches dev-DH

# File ars/model/simulator/__init__.py

• Ignore whitespace
 			angle))
 		# Matrix (of the rotation to apply)
 		# multiplies from the LEFT the actual one
-		rot_final = mu.matrix_as_tuple(mu.matrix3_multiply(rot_to_apply, rot_now))
+		rot_final = mu.matrix_as_tuple(mu.matrix_multiply(rot_to_apply, rot_now))
 		self.set_rotation(rot_final)

 	def offset_by_position(self, offset_pos):

# File ars/utils/geometry.py

• Ignore whitespace

 	Translated to Python by German Larrain.

-	Original version in Matlab, part of	'The Robotics Toolbox for Matlab (RTB)'
+	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.


 class Transform:

-	"""An homogeneous transform."""
+	r"""An homogeneous transform.
+
+	It is a composition of rotation and translation. Mathematically it can be
+	expressed as
+
+	.. math::
+
+		\left[
+		\begin{array}{ccc|c}
+			 &  &  &  \\
+			 & R &  & T \\
+			 & &  &  \\
+			\hline
+			0 & 0 & 0 & 1
+		  \end{array}
+		\right]
+
+	where R is the 3x3 submatrix describing rotation and T is the
+	3x1 submatrix describing translation.
+
+	source:
+	http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters#Denavit-Hartenberg_matrix
+
+	"""

 	def __init__(self, position, rot_matrix):
 		"""Constructor.
 def rot_matrix_to_hom_transform(rot_matrix):
 	"""Convert a rotation matrix to a homogeneous transform.

-	From transform.r2t in Corke's Robotic Toolbox (python) rot_matrix 3x3
-	matrix.
+	source: transform.r2t in Corke's Robotic Toolbox (python)

-	:param rot_matrix: a tuple, a tuple of tuples or the result of
-		numpy.mat()
+	:param rot_matrix: 3x3 matrix
+	:type rot_matrix: a tuple, a tuple of tuples or :class:numpy.mat

 	"""
-
 	if isinstance(rot_matrix, tuple):
 		if len(rot_matrix) == 9:
 			rot_matrix = (rot_matrix[0:3], rot_matrix[3:6], rot_matrix[6:9])
 	matrix rot_matrix, with respect to the plane (XZ, since the vertical
 	axis is Y). pitch is the rotation around Z and roll around Y.

-	Examples::
+	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((1.0, 0.0, 0.0), pi/6)
+	>>> pitch, roll = gemut.calc_inclination(rot_matrix)
+	0.0, 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, 1.0, 0.0), whatever)
+	>>> pitch, roll = gemut.calc_inclination(rot_matrix)
+	0.0, 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
+	>>> rot_matrix = calc_rotation_matrix((0.0, 0.0, 1.0), pi/6)
+	>>> pitch, roll = gemut.calc_inclination(rot_matrix)
+	pi/6, 0.0

 	"""
 	# THE FOLLOWING worked only in some cases, damn

# File ars/utils/mathematical.py

• Ignore whitespace
 	return tuple(result)


-def matrix3_multiply(matrix1, matrix2):
-	"""Return the matrix multiplication of matrix1 and matrix2."""
+def np_matrix_to_tuple(array_):
+	"""Convert Numpy 2D array (i.e. matrix) to a tuple of tuples.
+
+	source: http://stackoverflow.com/a/10016379/556413
+
+	Example:
+
+	>>> arr = numpy.array(((2, 2), (2, -2)))
+	>>> np_matrix_to_tuple(arr)
+	((2, 2), (2, -2))
+
+	:param array_: Numpy 2D array (i.e. matrix)
+	:return: matrix as tuple of tuples
+
+	"""
+	return tuple(tuple(x) for x in array_)
+
+
+def matrix_multiply(matrix1, matrix2):
+	"""Return the matrix multiplication of matrix1 and matrix2.
+
+	:param matrix1: LxM matrix
+	:param matrix2: MxN matrix
+	:return: LxN matrix, product of matrix1 and matrix2
+	:rtype: tuple of tuples
+
+	"""
 	# TODO: check objects are valid, or use exceptions to catch errors raised
 	# by numpy

 	a2 = np.array(matrix2)
 	result = np.dot(a1, a2)

-	return matrix_as_3x3_tuples(tuple(result.flatten()))
+	return np_matrix_to_tuple(result)


 def matrix_as_tuple(matrix_):


 def matrix_as_3x3_tuples(tuple_9):
-	#TODO: improve a lot
+	"""Return tuple_9 as a 3-tuple of 3-tuples.
+
+	:param tuple_9: tuple of 9 elements
+	:return: tuple_9 formatted as tuple of tuples
+
+	"""
+	#TODO: convert it to handle square matrices of any size

 	matrix = None
 	if isinstance(tuple_9, tuple):

# File tests/utils_mathematical.py

• Ignore whitespace
+import numpy
+
 import ars.utils.mathematical as mut


 def _vector_diff_length(v1, v2):
 	return mut.length3(mut.sub3(v1, v2))

+def test_np_matrix_to_tuple():
+	arr = numpy.array(((2, 2), (2, -2)))
+	print(mut.np_matrix_to_tuple(arr))
+

 def test_matrix_multiply():
-	print (mut.matrix3_multiply(((2,0,0),(0,1,0),(0,0,1)),
+	print (mut.matrix_multiply(((2,),(1,)), ((1,0,0,0,2,0,0,0,1), )))
+	print (mut.matrix_multiply(((2,0,0),(0,1,0),(0,0,1)),
 		((1,0,0),(0,2,0),(0,0,1))))
+	print (mut.matrix_multiply(((2,0,0,0),(0,1,0,0),(0,0,1,0),(0,0,0,1)),
+		((1,0,0,0),(0,2,0,0),(0,0,1,0),(0,0,0,1))))


 def test_calc_acceleration():

 if __name__ == "__main__":

+	test_np_matrix_to_tuple()
 	test_matrix_multiply()
 	test_XYZ_axes_for_orthogonality()
 	test_calc_acceleration()