1. German Larrain
  2. ars

Commits

German Larrain  committed eca7783 Merge

Merge with dev

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

Comments (0)

Files changed (4)

File ars/model/simulator/__init__.py

View file
  • 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

View file
  • 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

View file
  • 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

View file
  • 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()