Commits

German Larrain committed 0e91547 Merge

Merge with dev

Comments (0)

Files changed (14)

ars/app/__init__.py

 	which will be called during the simulation creation.
 
 	To use it, only two statements are necessary:
-	-create an object of this class (i.e. sim_program = ProgramSubclass() )
-	-call its 'start' method (i.e. sim_program.start() )
+
+	* create an object of this class
+		>>> sim_program = ProgramSubclass()
+	* call its 'start' method
+		>>> sim_program.start()
 
 	"""
 
 	def __init__(self):
 		"""Constructor. Defines some attributes and calls some initialization
 		methods to:
-		-sets the basic mapping of key to action,
-		-create the visualization window according to class constants,
-		-create the simulation.
+
+		* set the basic mapping of key to action,
+		* create the visualization window according to class constants,
+		* create the simulation.
 
 		"""
 		self.do_create_window = True
 
 	def create_simulation(self, add_axes=True, add_floor=True):
 		"""Creates an empty simulation and:
-		-adds basic simulation objects (:meth:`add_basic_simulation_objects`),
-		-(if ``add_axes`` is ``True``) adds axes to the visualization at the
-		coordinates-system origin,
-		-(if ``add_floor`` is ``True``) adds a floor with a defined normal
-		vector and some visualization parameters,
-		-calls :meth:`create_sim_objects` (which must be implemented by
-		subclasses),
-		-gets the actors representing the simulation objects and adds them to
-		the graphics adapter.
+
+		#. adds basic simulation objects (:meth:`add_basic_simulation_objects`),
+		#. (if ``add_axes`` is ``True``) adds axes to the visualization at the
+			coordinates-system origin,
+		#. (if ``add_floor`` is ``True``) adds a floor with a defined normal
+			vector and some visualization parameters,
+		#. calls :meth:`create_sim_objects` (which must be implemented by
+			subclasses),
+		#. gets the actors representing the simulation objects and adds them to
+			the graphics adapter.
 
 		"""
 		# set up the simulation parameters
 	def create_sim_objects(self):
 		"""This method must be overriden (at least once in the inheritance tree)
 		by the subclass that will instatiated to run the simulator.
+
 		It shall contain statements calling its 'sim' attribute's methods for
-		adding objects (e.g. add_sphere). For example:
-		self.sim.add_sphere(0.5, (1,10,1), density=1)
+		adding objects (e.g. add_sphere).
+
+		For example:
+
+		>>> self.sim.add_sphere(0.5, (1,10,1), density=1)
 
 		"""
 		raise NotImplementedError()

ars/graphics/__init__.py

 	__metaclass__ = ABCMeta
 
 	@abstractmethod
-	def __init__(self, position=(0,0,0), cylinder_radius=0.05):
+	def __init__(self, pos=(0,0,0), cylinder_radius=0.05):
 		self._actor = None
 
 	@property
 	adapter = None
 
 	@abstractmethod
-	def __init__(self, center, rotation):
+	def __init__(self, center, rot):
 		self._position = center
-		self._rotation = rotation
+		self._rotation = rot
 		self._color = None
 		self._actor = None
 
-	def update_position_rotation(self, position, rotation):
-		self._position = position
-		self._rotation = rotation
-		self.adapter._update_body(self._actor, position, rotation)
+	def update_position_rotation(self, pos, rot):
+		self._position = pos
+		self._rotation = rot
+		self.adapter._update_body(self._actor, pos, rot)
 
 	@property
 	def actor(self):
 	__metaclass__ = ABCMeta
 
 	@abstractmethod
-	def __init__(self, size, position, rotation=None):
-		super(Box, self).__init__(position, rotation)
+	def __init__(self, size, pos, rot=None):
+		super(Box, self).__init__(pos, rot)
 
 
 class Cone(Body):
 	__metaclass__ = ABCMeta
 
 	@abstractmethod
-	def __init__(self, height, radius, center, rotation=None, resolution = 100):
-		super(Cone, self).__init__(center, rotation)
+	def __init__(self, height, radius, center, rot=None, resolution = 100):
+		super(Cone, self).__init__(center, rot)
 
 
 class Sphere(Body):
 	__metaclass__ = ABCMeta
 
 	@abstractmethod
-	def __init__(self, radius, center, rotation=None, phiResolution = 50, thetaResolution = 50):
-		super(Sphere, self).__init__(center, rotation)
+	def __init__(self, radius, center, rot=None, phiResolution = 50, thetaResolution = 50):
+		super(Sphere, self).__init__(center, rot)
 
 
 class Cylinder(Body):
 	__metaclass__ = ABCMeta
 
 	@abstractmethod
-	def __init__(self, length, radius, center, rotation=None, resolution=10):
-		super(Cylinder, self).__init__(center, rotation)
+	def __init__(self, length, radius, center, rot=None, resolution=10):
+		super(Cylinder, self).__init__(center, rot)
 
 
 class Capsule(Body):
 	__metaclass__ = ABCMeta
 
 	@abstractmethod
-	def __init__(self, length, radius, center, rotation=None, resolution=10):
-		super(Capsule, self).__init__(center, rotation)
+	def __init__(self, length, radius, center, rot=None, resolution=10):
+		super(Capsule, self).__init__(center, rot)
 
 
 class Trimesh(Body):
 	__metaclass__ = ABCMeta
 
 	@abstractmethod
-	def __init__(self, vertices, faces, position=None, rotation=None):
-		super(Trimesh, self).__init__(position, rotation)
+	def __init__(self, vertices, faces, pos=None, rot=None):
+		super(Trimesh, self).__init__(pos, rot)
 
 
 class Adapter:
 		self._window_started = False
 
 	@abstractmethod
-	def create_window(self, title, position, size, zoom, cam_position, color):
+	def create_window(self, title, pos, size, zoom, cam_position, color):
 		raise NotImplementedError()
 
 	@abstractmethod
 
 	@staticmethod
 	@abstractmethod
-	def _update_body(body, position, rotation):
+	def _update_body(body, pos, rot):
 		raise NotImplementedError()
 
 

ars/graphics/adapters.py

 		self._zoom = None
 		self._cam_position = None
 
-	def create_window(self, title, position=None, size=(1000,600), zoom=1.0, cam_position=(10,8,10),
+	def create_window(self, title, pos=None, size=(1000,600), zoom=1.0, cam_position=(10,8,10),
 					background_color=(0.1,0.1,0.4)):
 
 		self._title = title
 		raise exc.ArsError()
 
 	@staticmethod
-	def _update_body(body, position, rotation):
-		t = VtkAdapter._create_transform_matrix(position, rotation)
+	def _update_body(body, pos, rot):
+		t = VtkAdapter._create_transform_matrix(pos, rot)
 		VtkAdapter._set_object_transform_matrix(body, t)
 
 	def _timer_callback(self, obj, event):
 		obj.PokeMatrix(transMat)
 
 	@staticmethod
-	def _create_transform_matrix(position, rotMatrix):
+	def _create_transform_matrix(pos, rot):
 		"""
-		position: a 3-tuple
-		rotMatrix: a 9-tuple
+		pos: a 3-tuple
+		rot: a 9-tuple
 
 		"""
-		t = gemut.Transform(position, rotMatrix)
+		t = gemut.Transform(pos, rot)
 		vtk_matrix = vtk.vtkMatrix4x4()
 		vtk_matrix.DeepCopy(t.get_long_tuple())
 
 
 
 class Axes(VtkBody, gp.Axes):
-	def __init__(self, position=(0,0,0), cylinder_radius=0.05):
-		gp.Axes.__init__(self, position, cylinder_radius)
+	def __init__(self, pos=(0,0,0), cylinder_radius=0.05):
+		gp.Axes.__init__(self, pos, cylinder_radius)
 
 		# 2 different methods may be used here
 		# see http://stackoverflow.com/questions/7810632/
 
 
 class Box(VtkBody, gp.Box):
-	def __init__(self, size, position, rotation=None):
-		gp.Box.__init__(self, size, position, rotation)
+	def __init__(self, size, pos, rot=None):
+		gp.Box.__init__(self, size, pos, rot)
 
 		box = vtk.vtkCubeSource()
 		box.SetXLength(size[0])
 		boxMapper.SetInputConnection(box.GetOutputPort())
 		boxActor = vtk.vtkActor()
 
-		VtkAdapter._update_body(boxActor, position, rotation)
+		VtkAdapter._update_body(boxActor, pos, rot)
 		boxActor.SetMapper(boxMapper)
 
 		self._actor = boxActor
 
 
 class Cone(VtkBody, gp.Cone):
-	def __init__(self, height, radius, center, rotation=None, resolution = 20):
-		gp.Cone.__init__(self, height, radius, center, rotation, resolution)
+	def __init__(self, height, radius, center, rot=None, resolution = 20):
+		gp.Cone.__init__(self, height, radius, center, rot, resolution)
 
 		cone = vtk.vtkConeSource()
 		cone.SetHeight(height)
 		coneMapper.SetInputConnection(cone.GetOutputPort())
 		coneActor = vtk.vtkActor()
 
-		VtkAdapter._update_body(coneActor, center, rotation)
+		VtkAdapter._update_body(coneActor, center, rot)
 		coneActor.SetMapper(coneMapper)
 
 		self._actor = coneActor
 
 	"""
 
-	def __init__(self, radius, center, rotation=None, phiResolution = 20, thetaResolution = 20):
-		gp.Sphere.__init__(self, radius, center, rotation, phiResolution, thetaResolution)
+	def __init__(self, radius, center, rot=None, phiResolution = 20, thetaResolution = 20):
+		gp.Sphere.__init__(self, radius, center, rot, phiResolution, thetaResolution)
 
 		sphere = vtk.vtkSphereSource()
 		sphere.SetRadius(radius)
 		sphereMapper.SetInputConnection(sphere.GetOutputPort())
 		sphereActor = vtk.vtkActor()
 
-		VtkAdapter._update_body(sphereActor, center, rotation)
+		VtkAdapter._update_body(sphereActor, center, rot)
 		sphereActor.SetMapper(sphereMapper)
 
 		self._actor = sphereActor
 
 
 class Cylinder(VtkBody, gp.Cylinder):
-	def __init__(self, length, radius, center, rotation=None, resolution=20):
-		gp.Cylinder.__init__(self, length, radius, center, rotation, resolution)
+	def __init__(self, length, radius, center, rot=None, resolution=20):
+		gp.Cylinder.__init__(self, length, radius, center, rot, resolution)
 
 		# VTK: The axis of the cylinder is aligned along the global y-axis.
 		cyl = vtk.vtkCylinderSource()
 		cylMapper.SetInputConnection(transFilter.GetOutputPort())
 		cylActor = vtk.vtkActor()
 
-		VtkAdapter._update_body(cylActor, center, rotation)
+		VtkAdapter._update_body(cylActor, center, rot)
 		cylActor.SetMapper(cylMapper)
 
 		self._actor = cylActor
 
 
 class Capsule(VtkBody, gp.Capsule):
-	def __init__(self, length, radius, center, rotation=None, resolution=20):
-		gp.Capsule.__init__(self, length, radius, center, rotation, resolution)
+	def __init__(self, length, radius, center, rot=None, resolution=20):
+		gp.Capsule.__init__(self, length, radius, center, rot, resolution)
 		# simplify this construction using those corresponding to Cylinder and Sphere?
 
 		sphere1 = vtk.vtkSphereSource()
 		assembly.AddPart(sphereActor1)
 		assembly.AddPart(sphereActor2)
 
-		VtkAdapter._update_body(assembly, center, rotation)
+		VtkAdapter._update_body(assembly, center, rot)
 		self._actor = assembly
 
 
 class Trimesh(VtkBody, gp.Trimesh):
-	def __init__(self, vertices, faces, position, rotation=None):
-		gp.Trimesh.__init__(self, vertices, faces, position, rotation)
+	def __init__(self, vertices, faces, pos, rot=None):
+		gp.Trimesh.__init__(self, vertices, faces, pos, rot)
 
 		# create points
 		points = vtk.vtkPoints()
 
 		# actor: represents an object (geometry & properties) in a rendered scene
 		triangles_actor = vtk.vtkActor()
-		VtkAdapter._update_body(triangles_actor, position, rotation)
+		VtkAdapter._update_body(triangles_actor, pos, rot)
 		triangles_actor.SetMapper(mapper)
 
 		self._actor = triangles_actor

ars/model/collision/adapters.py

 	#==========================================================================
 
 	def get_position(self):
+		"""Get the position of the geom.
+
+		:return: position
+		:rtype: 3-sequence of floats
+
+		"""
 		return self._inner_object.getPosition()
 
 	def get_rotation(self):
+		"""Get the orientation of the geom.
+
+		:return: rotation matrix
+		:rtype: 9-sequence of floats
+
+		"""
 		return self._inner_object.getRotation()
 
 	def set_position(self, pos):
+		"""Set the position of the geom.
+
+		:param pos: position
+		:type pos: 3-sequence of floats
+
+		"""
 		self._inner_object.setPosition(pos)
 
 	def set_rotation(self, rot):
+		"""Set the orientation of the geom.
+
+		:param rot: rotation matrix
+		:type rot: 9-sequence of floats
+
+		"""
 		self._inner_object.setRotation(rot)
 
 

ars/model/collision/base.py

 
 	@abstractmethod
 	def get_position(self):
+		"""Get the position of the geom.
+
+		:return: position
+		:rtype: 3-sequence of floats
+
+		"""
 		pass
 
 	@abstractmethod
 	def get_rotation(self):
+		"""Get the orientation of the geom.
+
+		:return: rotation matrix
+		:rtype: 9-sequence of floats
+
+		"""
 		pass
 
 	@abstractmethod
 	def set_position(self, pos):
+		"""Set the position of the geom.
+
+		:param pos: position
+		:type pos: 3-sequence of floats
+
+		"""
 		pass
 
 	@abstractmethod
 	def set_rotation(self, rot):
+		"""Set the orientation of the geom.
+
+		:param rot: rotation matrix
+		:type rot: 9-sequence of floats
+
+		"""
 		pass
 
 
 class RayContactData:
 	"""Contact information of a collision between `ray` and `shape`"""
 
-	def __init__(self, ray=None, shape=None, position=None, normal=None, depth=None):
-		# -position: point at which the ray intersects the surface of the other
+	def __init__(self, ray=None, shape=None, pos=None, normal=None, depth=None):
+		# -pos: point at which the ray intersects the surface of the other
 		# 	shape/geom.
 		# -normal: surface normal of the other geom at the contact point.
 		# -depth: distance from the start of the ray to the contact point.
 		# 	TODO: is it true?
 		self.ray = ray
 		self.shape = shape
-		self.position = position
+		self.position = pos
 		self.normal = normal
 		self.depth = depth
 

ars/model/physics/adapters.py

 	#==========================================================================
 
 	def get_position(self):
+		"""Get the position of the body.
+
+		:return: position
+		:rtype: 3-sequence of floats
+
+		"""
 		return self._inner_object.getPosition()
 
 	def get_linear_velocity(self):
 		return self._inner_object.getLinearVel()
 
 	def get_rotation(self):
+		"""Get the orientation of the body.
+
+		:return: rotation matrix
+		:rtype: 9-sequence of floats
+
+		"""
 		return self._inner_object.getRotation()
 
 	def get_angular_velocity(self):
 		return self._inner_object.getMass().I
 
 	def set_position(self, pos):
+		"""Set the position of the body.
+
+		Sends :data:`signals.BODY_PRE_SET_POSITION` and
+		:data:`signals.BODY_POST_SET_POSITION`.
+
+		:param pos: position
+		:type pos: 3-sequence of floats
+
+		"""
 		dispatcher.send(signals.BODY_PRE_SET_POSITION, sender=self)
 		self._inner_object.setPosition(pos)
 		dispatcher.send(signals.BODY_POST_SET_POSITION, sender=self)
 
 	def set_rotation(self, rot):
+		"""Set the orientation of the body.
+
+		Sends :data:`signals.BODY_PRE_SET_ROTATION` and
+		:data:`signals.BODY_POST_SET_ROTATION`.
+
+		:param rot: rotation matrix
+		:type rot: 9-sequence of floats
+
+		"""
 		dispatcher.send(signals.BODY_PRE_SET_ROTATION, sender=self)
 		self._inner_object.setRotation(rot)
 		dispatcher.send(signals.BODY_POST_SET_ROTATION, sender=self)

ars/model/physics/base.py

 	__metaclass__ = ABCMeta
 
 	@abstractmethod
-	def __init__(self, mass=None, density=None, position=None, rotation=None,
+	def __init__(self, mass=None, density=None, pos=None, rot=None,
 			*args, **kwargs):
 
 		if mass is not None:
 
 	@abstractmethod
 	def get_position(self):
+		"""Get the position of the body.
+
+		:return: position
+		:rtype: 3-sequence of floats
+
+		"""
 		raise NotImplementedError()
 
 	@abstractmethod
 
 	@abstractmethod
 	def get_rotation(self):
+		"""Get the orientation of the body.
+
+		:return: rotation matrix
+		:rtype: 9-sequence of floats
+
+		"""
 		raise NotImplementedError()
 
 	@abstractmethod
 
 	@abstractmethod
 	def set_position(self, pos):
-		"""Subclasses implementing this method must send the correponding
-		signals, defined in :mod:`ars.model.physics.signals`.
+		"""Set the position of the body.
+
+		Sends :data:`signals.BODY_PRE_SET_POSITION` and
+		:data:`signals.BODY_POST_SET_POSITION`.
+
+		:param pos: position
+		:type pos: 3-sequence of floats
 
 		"""
 		raise NotImplementedError()
 
 	@abstractmethod
 	def set_rotation(self, rot):
-		"""Subclasses implementing this method must send the correponding
-		signals, defined in :mod:`ars.model.physics.signals`.
+		"""Set the orientation of the body.
+
+		Sends :data:`signals.BODY_PRE_SET_ROTATION` and
+		:data:`signals.BODY_POST_SET_ROTATION`.
+
+		:param rot: rotation matrix
+		:type rot: 9-sequence of floats
 
 		"""
 		raise NotImplementedError()

ars/model/robot/sensors.py

 
 		Useful mainly when it is not attached to a body.
 
+		:param pos: position
+		:type pos: 3-sequence of floats
+
 		"""
 		# TODO: if mounted, w.r.t what?
 		self._ray.set_position(pos)
 
 		Useful mainly when it is not attached to a body.
 
+		:param rot: rotation matrix
+		:type rot: 9-sequence of floats
+
 		"""
 		# TODO: if mounted, w.r.t what?
 		self._ray.set_rotation(rot)

ars/model/simulator/__init__.py

 
 	@abstractmethod
 	def get_position(self):
+		"""Get the position of the object.
+
+		:return: position
+		:rtype: 3-sequence of floats
+
+		"""
 		pass
 
 	@abstractmethod
-	def set_position(self, position):
+	def set_position(self, pos):
+		"""Set the orientation of the object.
+
+		:param pos: position
+		:type pos: 3-sequence of floats
+
+		"""
 		pass
 
 	@abstractmethod
 	def get_rotation(self):
+		"""Get the orientation of the object.
+
+		:return: rotation matrix
+		:rtype: 9-sequence of floats
+
+		"""
 		pass
 
 	@abstractmethod
-	def set_rotation(self, rot_matrix):
+	def set_rotation(self, rot):
+		"""Set the orientation of the object.
+
+		:param rot: rotation matrix
+		:type rot: 9-sequence of floats
+
+		"""
 		pass
 
+	def get_pose(self):
+		"""Get the pose (3D position and rotation) of the object.
+
+		:return: pose
+		:rtype: :class:`ars.utils.geometry.Transform`
+
+		"""
+		return gemut.Transform(self.get_position(), self.get_rotation())
+
+	def set_pose(self, pose):
+		"""Set the pose (3D position and rotation) of the object.
+
+		:param pose:
+		:type pose: :class:`ars.utils.geometry.Transform`
+
+		"""
+		self.set_position(pose.get_translation())
+		rot = pose.get_rotation()
+		self.set_rotation(mu.matrix_as_tuple(rot))
+
 
 class SimulatedBody(SimulatedPhysicsObject):
 	"""Class encapsulating the physics, collision and graphical objects
 		return self._body
 
 	def get_position(self):
+		"""Get the position of the body.
+
+		:return: position
+		:rtype: 3-sequence of floats
+
+		"""
 		return self._body.get_position()
 
-	def set_position(self, position):
-		self._body.set_position(position)
+	def set_position(self, pos):
+		"""Set the orientation of the body.
+
+		:param pos: position
+		:type pos: 3-sequence of floats
+
+		"""
+		self._body.set_position(pos)
 
 	def get_rotation(self):
+		"""Get the orientation of the body.
+
+		:return: rotation matrix
+		:rtype: 9-sequence of floats
+
+		"""
 		return self._body.get_rotation()
 
-	def set_rotation(self, rot_matrix):
-		self._body.set_rotation(rot_matrix)
+	def set_rotation(self, rot):
+		"""Set the orientation of the body.
+
+		:param rot: rotation matrix
+		:type rot: 9-sequence of floats
+
+		"""
+		self._body.set_rotation(rot)
 
 	def _get_attr_in_body(self, attr, *args):
 		"""Return attribute `attr` from object `self._body`.
 		# It is necessary to have this method, even if it's not implemented
 		raise NotImplementedError()
 
-	def set_position(self, position):
+	def set_position(self, pos):
 		# It is necessary to have this method, even if it's not implemented
 		raise NotImplementedError()
 
 		# It is necessary to have this method, even if it's not implemented
 		raise NotImplementedError()
 
-	def set_rotation(self, rot_matrix):
+	def set_rotation(self, rot):
 		# It is necessary to have this method, even if it's not implemented
 		raise NotImplementedError()

ars/utils/geometry.py

 """
 
 
-def _rot_matrix_to_rpy_angles(rot_matrix, zyx=False):
+def _rot_matrix_to_rpy_angles(rot, 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
 	Copyright (C) 1993-2011, by Peter I. Corke. See `rtb_license`.
 
 	"""
-	m = mut.matrix_as_3x3_tuples(rot_matrix)
+	m = mut.matrix_as_3x3_tuples(rot)
 
 	# "eps: distance from 1.0 to the next largest double-precision number"
 	eps = 2e-52 # http://www.mathworks.com/help/techdoc/ref/eps.html
 
 	"""
 
-	def __init__(self, position, rot_matrix):
+	def __init__(self, pos=None, rot=None):
 		"""Constructor.
 
-		:param position: a 3-tuple
-		:param rot_matrix: a 9-tuple
+		With empty arguments it's just a 4x4 identity matrix.
+
+		:param pos: a size 3 vector, or 3x1 or 1x3 matrix
+		:type pos: tuple, :class:`numpy.ndarray` or None
+		:param rot: 3x3 or 9x1 rotation matrix
+		:type rot: tuple, :class:`numpy.ndarray` or None
 
 		"""
-		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)
+		if pos is None:
+			pos = (0, 0, 0)
+		pos = np.array(pos)
 
-		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)
+		if pos.shape != (3, 1):
+			pos = pos.reshape((3, 1))
+
+		if rot is None:
+			rot = np.identity(3)
+		else:
+			rot = np.array(rot)
+
+		if rot.shape != (3, 3):
+			rot = rot.reshape((3, 3))
+
+		temp = np.hstack((rot, pos))
+		self._matrix = np.vstack((temp, np.array([0, 0, 0, 1])))
+
 
 	def __str__(self):
-		return str(self.matrix)
+		return str(self._matrix)
+
+	@property
+	def matrix(self):
+		r"""Return matrix that contains the transform values.
+
+		:return: 4x4 matrix
+		:rtype: :class:`numpy.ndarray`
+
+		"""
+		return self._matrix
 
 	def get_long_tuple(self):
-		return gut.nested_iterable_to_tuple(self.matrix)
+		return tuple(self._matrix.flatten())
 
-	def get_position(self):
-		raise NotImplementedError()
+	def get_translation(self, as_numpy=False):
+		"""Get the translation component (vector).
 
-	def get_rotation_matrix(self):
-		raise NotImplementedError()
+		:param as_numpy: whether to return a numpy object or a tuple
+		:return: 3-sequence
+		:rtype: tuple or :class:`numpy.ndarray`
 
+		"""
+		rot = self._matrix[0:3,3]
+		if as_numpy:
+			return rot
+		return tuple(rot)
 
-def rot_matrix_to_hom_transform(rot_matrix):
+	def get_rotation(self, as_numpy=False):
+		"""Get the rotation component (matrix).
+
+		:param as_numpy: whether to return a numpy object or a tuple
+		:return: 3x3 rotation matrix
+		:rtype: tuple of tuples or :class:`numpy.ndarray`
+
+		"""
+		rot = self._matrix[0:3,0:3]
+		if as_numpy:
+			return rot
+		return mut.np_matrix_to_tuple(rot)
+
+
+def rot_matrix_to_hom_transform(rot):
 	"""Convert a rotation matrix to a homogeneous transform.
 
 	source: transform.r2t in Corke's Robotic Toolbox (python)
 	
-	:param rot_matrix: 3x3 matrix
-	:type rot_matrix: a tuple, a tuple of tuples or :class:`numpy.mat`
+	:param rot: 3x3 rotation matrix
+	:type rot: a tuple, a tuple of tuples or :class:`numpy.ndarray`
 
 	"""
-	if isinstance(rot_matrix, tuple):
-		if len(rot_matrix) == 9:
-			rot_matrix = (rot_matrix[0:3], rot_matrix[3:6], rot_matrix[6:9])
+	if isinstance(rot, tuple):
+		if len(rot) == 9:
+			rot = (rot[0:3], rot[3:6], rot[6:9])
 
-	return np.concatenate((np.concatenate((rot_matrix, np.zeros((3, 1))), 1),
+	return np.concatenate((np.concatenate((rot, np.zeros((3, 1))), 1),
 							np.mat([0, 0, 0, 1])))
 
 
 		t * axis[2]**2 + cos_theta)
 
 
-def make_OpenGL_matrix(rotation, position):
-	"""Returns an OpenGL compatible (column-major, 4x4 homogeneous)
+def make_OpenGL_matrix(rot, pos):
+	"""Return an OpenGL compatible (column-major, 4x4 homogeneous)
 	transformation matrix from ODE compatible (row-major, 3x3) rotation matrix
 	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)
+	return (rot[0], rot[3], rot[6], 0.0,
+		rot[1], rot[4], rot[7], 0.0,
+		rot[2], rot[5], rot[8], 0.0,
+		pos[0], pos[1], pos[2], 1.0)
 
 
 def get_body_relative_vector(body, vector):
-	"""Returns the 3-vector vector transformed into the local coordinate system
+	"""Return the 3-vector vector transformed into the local coordinate system
 	of ODE body 'body'"""
 	return mut.rotate3(mut.transpose3(body.get_rotation()), vector)
 
 
-def rot_matrix_to_euler_angles(rot_matrix):
+def rot_matrix_to_euler_angles(rot):
 	r"""Return the 3-1-3 Euler angles `phi`, `theta` and `psi` (using the
-	x-convention) corresponding to the rotation matrix `rot_matrix`, which
+	x-convention) corresponding to the rotation matrix `rot`, which
 	is a tuple of three 3-element tuples, where each one is a row (what is
 	called row-major order).
 
  	http://en.wikipedia.org/wiki/Rotation_representation_(mathematics)%23Rotation_matrix_.E2.86.94_Euler_angles
 
 	"""
-	A = rot_matrix
+	A = rot
 	phi = mut.atan2(A[2][0], A[2][1])		# arctan2(A_{31}, A_{32})
 	theta = mut.acos(A[2][2])				# arccos(A_{33})
 	psi = -mut.atan2(A[0][2], A[1][2])		# -arctan2(A_{13}, A_{23})
 	return angles
 
 
-def calc_inclination(rot_matrix):
+def calc_inclination(rot):
 	"""Return the inclination (as ``pitch`` and ``roll``) inherent of rotation
-	matrix ``rot_matrix``, with respect to the plane (`XZ`, since the vertical
+	matrix ``rot``, with respect to the plane (`XZ`, since the vertical
 	axis is `Y`). ``pitch`` is the rotation around `Z` and ``roll`` around `Y`.
 
 	Examples:
 
-	>>> rot_matrix = calc_rotation_matrix((1.0, 0.0, 0.0), pi/6)
-	>>> pitch, roll = gemut.calc_inclination(rot_matrix)
+	>>> rot = calc_rotation_matrix((1.0, 0.0, 0.0), pi/6)
+	>>> pitch, roll = gemut.calc_inclination(rot)
 	0.0, pi/6
 
-	>>> rot_matrix = calc_rotation_matrix((0.0, 1.0, 0.0), whatever)
-	>>> pitch, roll = gemut.calc_inclination(rot_matrix)
+	>>> rot = calc_rotation_matrix((0.0, 1.0, 0.0), whatever)
+	>>> pitch, roll = gemut.calc_inclination(rot)
 	0.0, 0.0
 
-	>>> rot_matrix = calc_rotation_matrix((0.0, 0.0, 1.0), pi/6)
-	>>> pitch, roll = gemut.calc_inclination(rot_matrix)
+	>>> rot = calc_rotation_matrix((0.0, 0.0, 1.0), pi/6)
+	>>> pitch, roll = gemut.calc_inclination(rot)
 	pi/6, 0.0
 
 	"""
 	#z_front = mut.bkwdAxis
 	#x_right = mut.rightAxis
 	#
-	#up_rotated = mut.rotate3(rot_matrix, y_up)
+	#up_rotated = mut.rotate3(rot, 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)
+	#front_rotated = mut.rotate3(rot, 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_x, pitch_y, yaw_z = _rot_matrix_to_rpy_angles(rot)
 	roll = roll_x
 	pitch = yaw_z
 	#yaw = pitch_y  # we don't need it
 	return pitch, roll
 
 
-def calc_compass_angle(rot_matrix):
+def calc_compass_angle(rot):
 	"""Return the angle around the vertical axis with respect to the `X+` axis,
-	i.e. the angular orientation inherent of a rotation matrix ``rot_matrix``,
+	i.e. the angular orientation inherent of a rotation matrix ``rot``,
 	constrained to the plane aligned with the horizon (`XZ`, since the vertical
 	axis is `Y`).
 
 	"""
-	roll_x, pitch_y, yaw_z = _rot_matrix_to_rpy_angles(rot_matrix)
+	roll_x, pitch_y, yaw_z = _rot_matrix_to_rpy_angles(rot)
 	yaw = pitch_y
 	return yaw

ars/utils/mathematical.py

 	>>> np_matrix_to_tuple(arr)
 	((2, 2), (2, -2))
 
-	:param array_: Numpy 2D array (i.e. matrix)
+	:param array_: 2D array (i.e. matrix)
+	:type array_: :class:`numpy.ndarray`
 	:return: matrix as tuple of tuples
 
 	"""
 	a2 = np.array(matrix2)
 	result = np.dot(a1, a2)
 
-	return np_matrix_to_tuple(result)
+	if result.ndim == 1:
+		return tuple(result)
+	else:
+		return np_matrix_to_tuple(result)
 
 
 def matrix_as_tuple(matrix_):
 	"""Convert ``matrix_`` to a tuple.
 
-	:param matrix_:
-	:type matrix_: nested tuples, e.g. ((1,0),(1,1),(2,5))
+	Example:
+
+	>>> matrix_as_tuple(((1, 2), (3, 4)))
+	(1, 2, 3, 4)
+
+	:param matrix_: nested tuples
+	:type matrix_: tuple
+	:return: ``matrix_`` flattened as a tuple
+	:rtype: tuple
 
 	"""
 	#TODO: improve a lot
 		return acos(x)
 
 
-def rotate3(rot_matrix, vector):
+def rotate3(rot, vector):
 	"""Return the rotation of 3-dimension ``vector`` by 3x3 (row major) matrix
-	``rot_matrix``.
+	``rot``.
 
 	"""
-	return (vector[0] * rot_matrix[0] + vector[1] * rot_matrix[1] +
-			vector[2] * rot_matrix[2],
-			vector[0] * rot_matrix[3] + vector[1] * rot_matrix[4] +
-			vector[2] * rot_matrix[5],
-			vector[0] * rot_matrix[6] + vector[1] * rot_matrix[7] +
-			vector[2] * rot_matrix[8])
+	return (vector[0] * rot[0] + vector[1] * rot[1] +
+			vector[2] * rot[2],
+			vector[0] * rot[3] + vector[1] * rot[4] +
+			vector[2] * rot[5],
+			vector[0] * rot[6] + vector[1] * rot[7] +
+			vector[2] * rot[8])
 
 
 def transpose3(matrix):
 		matrix[2], matrix[5], matrix[8])
 
 
-def z_axis(rot_matrix):
+def z_axis(rot):
 	"""Return the z-axis vector from 3x3 (row major) rotation matrix
-	``rot_matrix``.
+	``rot``.
 
 	"""
 	#TODO: convert it so it can handle vector of any dimension, and any column
-	return (rot_matrix[2], rot_matrix[5], rot_matrix[8])
+	return (rot[2], rot[5], rot[8])
 
 #==============================================================================
 # TESTS

tests/graphics_adapters.py

 
 	axes1 = Axes()
 	axes2 = Axes(cylinder_radius=0.02)
-	box = Box(size=(0.5,1.0,2.0), position=(2,2,2))
+	box = Box(size=(0.5,1.0,2.0), pos=(2,2,2))
 	cone = Cone(1.0, 0.2, center=(1,1,1))
 	sphere = Sphere(0.5, center=(-2,-2,-2))
 	cyl = Cylinder(length=2, radius=0.25, center=(1,-1,1)) #, orientation=(mut.pi/3,mut.pi/3,mut.pi/3))

tests/utils_geometry.py

 	print(rot)
 	print(t)
 	print(t.get_long_tuple())
+	print(t.matrix)
+	print(t.get_rotation())
+	print(t.get_rotation(True))
+	print(t.get_translation())
+	print(t.get_translation(True))
 
 def _test_rot_matrix_to_hom_transform():
 
 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)
+	rot = gemut.calc_rotation_matrix(axis, angle)
+	pitch, roll = gemut.calc_inclination(rot)
 	print('axis: %s, angle: %f' % (axis, angle))
 	print('pitch: %f, roll: %f' % (pitch, roll))
 	print('')
 
 def test_calc_compass_angle(axis, angle):
-	rot_matrix = gemut.calc_rotation_matrix(axis, angle)
-	yaw = gemut.calc_compass_angle(rot_matrix)
+	rot = gemut.calc_rotation_matrix(axis, angle)
+	yaw = gemut.calc_compass_angle(rot)
 	print('axis: %s, angle: %f' % (axis, angle))
 	print('compass: %f' % yaw)
 	print('')

tests/utils_mathematical.py

 	arr = numpy.array(((2, 2), (2, -2)))
 	print(mut.np_matrix_to_tuple(arr))
 
+def test_matrix_as_tuple():
+	print(mut.matrix_as_tuple(((1, 2), (3, 4))))
+
 
 def test_matrix_multiply():
 	print (mut.matrix_multiply(((2,),(1,)), ((1,0,0,0,2,0,0,0,1), )))
 if __name__ == "__main__":
 
 	test_np_matrix_to_tuple()
+	test_matrix_as_tuple()
 	test_matrix_multiply()
 	test_XYZ_axes_for_orthogonality()
 	test_calc_acceleration()