Commits

German Larrain committed 113a2ed

utils: new 'geometry' model, to which some 'mathematical' functions and classes were moved:
-rot_matrix_to_hom_transform
-Transform
-calc_rotation_matrix
-make_OpenGL_matrix
-get_body_relative_vector

  • Participants
  • Parent commits bfeef40

Comments (0)

Files changed (6)

File ars/graphics/adapters.py

 
 import ars.exceptions as exc
 import ars.graphics as gp
-import ars.utils.mathematical as mut
+import ars.utils.geometry as gemut
+
 
 class VtkAdapter(gp.Adapter):
 
 		position: a 3-tuple
 		rotMatrix: a 9-tuple
 		"""
-		t = mut.Transform(position, rotMatrix)
+		t = gemut.Transform(position, rotMatrix)
 		vtk_matrix = vtk.vtkMatrix4x4()
 		vtk_matrix.DeepCopy(t.get_long_tuple())
 

File ars/model/contrib/ragdoll.py

 import ode
 
 import ars.model.physics.adapters as phs
+import ars.utils.geometry as gemut
 import ars.utils.mathematical as mut
 
 class RagDoll:
 				# current joint flex to the fixed body's base twist up vector
 				#===============================================================
 				baseTwistUp = mut.rotate3(j.getBody(0).getRotation(), j.baseTwistUp)
-				base2current = mut.calc_rotation_matrix(mut.norm3(mut.cross_product(baseAxis, currAxis)),
+				base2current = gemut.calc_rotation_matrix(mut.norm3(mut.cross_product(baseAxis, currAxis)),
 					mut.acos_dot3(baseAxis, currAxis))
 				projBaseTwistUp = mut.rotate3(base2current, baseTwistUp)
 

File ars/model/simulator/__init__.py

 #import ars.model.contrib.ragdoll as cb
 import ars.model.physics.adapters as phs
 import ars.model.robot.joints as jo
+import ars.utils.geometry as gemut
 import ars.utils.mathematical as mu
 
 
 		"""Rotate the object by applying a rotation matrix defined by the given
 		axis and angle"""
 		rot_now = mu.matrix_as_3x3_tuples(self.get_rotation())
-		rot_to_apply = mu.matrix_as_3x3_tuples(mu.calc_rotation_matrix(axis, angle))
+		rot_to_apply = mu.matrix_as_3x3_tuples(gemut.calc_rotation_matrix(axis, angle))
 		# the rotation matrix to be applied multiplies from the LEFT the actual one
 		rot_final = mu.matrix_as_tuple(mu.matrix3_multiply(rot_to_apply, rot_now))
 		self.set_rotation(rot_final)

File ars/utils/geometry.py

+import numpy as np
+
+import ars.utils.mathematical as mut
+
+import generic as gut
+
+
+class Transform:
+	def __init__(self, position, rot_matrix):
+		"""
+		position: a 3-tuple
+		rot_matrix: a 9-tuple
+		"""
+		if not rot_matrix:
+			rot_matrix = []
+			rot_matrix[0:3] = (1,0,0)
+			rot_matrix[3:6] = (0,1,0)
+			rot_matrix[6:9] = (0,0,1)
+			rot_matrix = tuple(rot_matrix)
+
+		row1 = rot_matrix[0:3] + (position[0],)
+		row2 = rot_matrix[3:6] + (position[1],)
+		row3 = rot_matrix[6:9] + (position[2],)
+		row4 = (0,0,0,1)
+		self.matrix = (row1,row2,row3,row4)
+
+	def __str__(self):
+		return str(self.matrix)
+
+	def get_long_tuple(self):
+		return gut.nested_iterable_to_tuple(self.matrix)
+
+	def get_position(self):
+		raise NotImplementedError()
+
+	def get_rotation_matrix(self):
+		raise NotImplementedError()
+
+
+def rot_matrix_to_hom_transform(rot_matrix):
+	"""From transform.r2t in Corke's Robotic Toolbox (python) rot_matrix 3x3
+	matrix.	It may be a tuple, tuple of tuples or the result of 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])
+
+	return np.concatenate( (np.concatenate((rot_matrix, np.zeros((3,1))),1),
+							np.mat([0,0,0,1])) )
+
+
+def calc_rotation_matrix(axis, angle):
+	"""
+	Returns the row-major 3x3 rotation matrix defining a rotation around axis by
+	angle.
+
+	Formula is the same as the one presented here (as of 2011.12.01):
+	http://goo.gl/RkW80
+
+	<math>
+	R = \begin{bmatrix}
+	\cos \theta +u_x^2 \left(1-\cos \theta\right) &
+	u_x u_y \left(1-\cos \theta\right) - u_z \sin \theta &
+	u_x u_z \left(1-\cos \theta\right) + u_y \sin \theta \\
+	u_y u_x \left(1-\cos \theta\right) + u_z \sin \theta &
+	\cos \theta + u_y^2\left(1-\cos \theta\right) &
+	u_y u_z \left(1-\cos \theta\right) - u_x \sin \theta \\
+	u_z u_x \left(1-\cos \theta\right) - u_y \sin \theta &
+	u_z u_y \left(1-\cos \theta\right) + u_x \sin \theta &
+	\cos \theta + u_z^2\left(1-\cos \theta\right)
+	\end{bmatrix}
+	</math>
+	"""
+
+	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)
+
+
+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)
+
+
+def get_body_relative_vector(body, vector):
+	"""Returns the 3-vector vector transformed into the local coordinate system
+	of ODE body 'body'"""
+	return mut.rotate3(mut.transpose3(body.get_rotation()), vector)

File ars/utils/mathematical.py

 # @author: german
 
 # TODO:	attribute the code sections that were taken from somewhere else
-# TODO: create a test for calc_rotation_matrix
 
 """
 Functions to perform operations over vectors and matrices;
 		result.append(degrees_to_radians(degrees_))
 	return tuple(result)
 
-def rot_matrix_to_hom_transform(rot_matrix):
-	"""From transform.r2t in Corke's Robotic Toolbox (python) rot_matrix 3x3
-	matrix.	It may be a tuple, tuple of tuples or the result of numpy.mat()"""
-	import numpy as np
-
-	if isinstance(rot_matrix, tuple):
-		if len(rot_matrix) == 9:
-			rot_matrix = (rot_matrix[0:3], rot_matrix[3:6], rot_matrix[6:9])
-
-	return np.concatenate( (np.concatenate((rot_matrix, np.zeros((3,1))),1),
-						np.mat([0,0,0,1])) )
-
 def matrix3_multiply(matrix1, matrix2):
 	"""returns the matrix multiplication of matrix1 and matrix2"""
 	#TODO: check objects are valid, or use exceptions to catch errors raised by numpy
 			matrix = (tuple_9[0:3], tuple_9[3:6], tuple_9[6:9])
 	return matrix
 
-class Transform:
-	def __init__(self, position, rot_matrix):
-		"""
-		position: a 3-tuple
-		rot_matrix: a 9-tuple
-		"""
-		if not rot_matrix:
-			rot_matrix = []
-			rot_matrix[0:3] = (1,0,0)
-			rot_matrix[3:6] = (0,1,0)
-			rot_matrix[6:9] = (0,0,1)
-			rot_matrix = tuple(rot_matrix)
-
-		row1 = rot_matrix[0:3] + (position[0],)
-		row2 = rot_matrix[3:6] + (position[1],)
-		row3 = rot_matrix[6:9] + (position[2],)
-		row4 = (0,0,0,1)
-		self.matrix = (row1,row2,row3,row4)
-
-	def __str__(self):
-		return str(self.matrix)
-
-	def get_long_tuple(self):
-		return gut.nested_iterable_to_tuple(self.matrix)
-
-	def get_position(self):
-		raise NotImplementedError()
-
-	def get_rotation_matrix(self):
-		raise NotImplementedError()
-
 #===============================================================================
 # Original code but formatted and some refactor
 #===============================================================================
 	#TODO: convert it so it can handle vector of any dimension, and any column
 	return (rot_matrix[2], rot_matrix[5], rot_matrix[8])
 
-def calc_rotation_matrix(axis, angle):
-	"""
-	Returns the row-major 3x3 rotation matrix defining a rotation around axis by
-	angle.
-
-	Formula is the same as the one presented here (as of 2011.12.01):
-	http://goo.gl/RkW80
-
-	<math>
-	R = \begin{bmatrix}
-	\cos \theta +u_x^2 \left(1-\cos \theta\right) &
-	u_x u_y \left(1-\cos \theta\right) - u_z \sin \theta &
-	u_x u_z \left(1-\cos \theta\right) + u_y \sin \theta \\
-	u_y u_x \left(1-\cos \theta\right) + u_z \sin \theta &
-	\cos \theta + u_y^2\left(1-\cos \theta\right) &
-	u_y u_z \left(1-\cos \theta\right) - u_x \sin \theta \\
-	u_z u_x \left(1-\cos \theta\right) - u_y \sin \theta &
-	u_z u_y \left(1-\cos \theta\right) + u_x \sin \theta &
-	\cos \theta + u_z^2\left(1-\cos \theta\right)
-	\end{bmatrix}
-	</math>
-	"""
-
-	cos_theta = cos(angle)
-	sin_theta = 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)
-
-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)
-
-def get_body_relative_vector(body, vector):
-	"""Returns the 3-vector vector transformed into the local coordinate system
-	of ODE body 'body'"""
-	return rotate3(transpose3(body.get_rotation()), vector)
-
 #===============================================================================
 # TESTS
 #===============================================================================
 	print('degrees: %f' % y)
 	print('difference: %f' % dif)
 
-def _test_rot_matrix_to_hom_transform():
-
-	import numpy as np
-	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 = rot_matrix_to_hom_transform(rot1)
-	ht2 = rot_matrix_to_hom_transform(rot2)
-	ht3 = rot_matrix_to_hom_transform(rot3)
-
-	#print(rot1)
-	#print(rot2)
-	print(ht1)
-	print(ht2)
-	print(ht3)
-
-def _test_Transform():
-
-	pos = (10,20,30)
-	rot = (0,0,1,1,0,0,0,1,0)
-	t = Transform(pos, rot)
-
-	print(pos)
-	print(rot)
-	print(t)
-	print(t.get_long_tuple())
-
 if __name__ == '__main__':
 
-	_test_rot_matrix_to_hom_transform()
 	_test_angular_conversions(2.0/3*math.pi)
 	_test_angular_conversions(4.68*math.pi)
 
 	print(vec3_radians_to_degrees(radians_))
 	print(vec3_degrees_to_radians(degrees_))
 	print(radians_)
-
-	_test_Transform()

File tests/utilities_geometry.py

+
+# TODO: create a test for calc_rotation_matrix
+
+import numpy as np
+
+import ars.utils.geometry as gemut
+
+
+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)
+
+if __name__ == "__main__":
+
+	_test_rot_matrix_to_hom_transform()
+
+	_test_Transform()