# Commits

committed f4bcdd0

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

# File ars/model/robot/dh.py

• Ignore whitespace
 	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()