Commits

German Larrain  committed f4bcdd0

robot.dh: added class 'SerialLink'

  • Participants
  • Parent commits 0e91547
  • Branches dev-DH

Comments (0)

Files changed (1)

File ars/model/robot/dh.py

 	T = \operatorname{}^{0}A_1 \cdot \operatorname{}^{1}A_2 \cdots \operatorname{}^{n - 1}A_n
 
 """
-from ars.utils.mathematical import sin, cos
+from ars.utils.mathematical import sin, cos, matrix_multiply
 
 
 class DHLink:
 	def theta(self):
 		"""Synonym of :meth:`angle`."""
 		return self._angle
+
+
+class SerialLink:
+
+	"""Serial chain of links (intances of :class:`DHLink`).
+
+	It contains a list of them. A chain is created this way:
+
+	>>> link1 = DHLink()
+	>>> link2 = DHLink()
+	>>> sl = SerialLink([link1, link2])
+
+	"""
+
+	def __init__(self, links=None):
+		self._links = links
+
+	###########################################################################
+	# properties
+	###########################################################################
+
+	@property
+	def links(self):
+		return self._links
+
+	@property
+	def T(self):
+		"""Synonym of :meth:`calc_transform`."""
+		return self.calc_transform()
+
+	###########################################################################
+	# methods
+	###########################################################################
+
+	def calc_transform(self):
+		r"""Calculate chain's homogeneous transformation matrix.
+
+		Matrix `T` defines the pose (position and orientation) of the end
+		effector with respect to the base.
+
+		.. math::
+
+			T = \operatorname{}^{0}A_1 \cdot \operatorname{}^{1}A_2 \cdots \operatorname{}^{n - 1}A_n
+
+		:return: 4x4 matrix
+		:rtype: tuple of tuples
+
+		"""
+		T = ((1, 0, 0, 0), (0, 1, 0, 0), (0, 0, 1, 0), ( 0, 0, 0, 1))
+
+		for link in self._links:
+			T = matrix_multiply(T, link.calc_transform())
+
+		return T
+
+	def concatenate(self, serial_link):
+		"""Attach ``serial_link`` to the end of this object.
+
+		:type serial_link: :class:`SerialLink`
+
+		"""
+		raise NotImplementedError()