Commits

German Larrain  committed 53ca1ca

model.robot: added module 'dh', which contains class DHLink (Denavit-Hartenberg).

  • Participants
  • Parent commits 98b979c
  • Branches dev-DH

Comments (0)

Files changed (1)

File ars/model/robot/dh.py

+"""Denavit-Hartenberg related code
+
+"""
+from ars.utils.mathematical import sin, cos
+
+
+class DHLink:
+
+	r"""Denavit-Hartenberg link.
+
+	It is defined by 4 parameters:
+
+	* :attr:`length`: a.k.a. 'a', 'r' and radius
+	* :attr:`offset`: a.k.a. 'd' and depth
+	* :attr:`twist`: a.k.a. :math:`\alpha`
+	* :attr:`angle`: a.k.a. :math:`\theta`
+
+	On the other hand, :attr:`convention` stores which convention for the
+	parameters is used. There are two: standard (``'s'``) and Craig's modified
+	(``'m'``). By default the former is used.
+
+	>>> (length, offset, twist, angle) = (0.0, 0.0, 0.0, 0.0)
+	>>> DHLink(*(length, offset, twist, angle))
+
+	"""
+
+	def __init__(self, length=0.0, offset=0.0, twist=0.0, angle=0.0,
+					convention='s'):
+		"""Constructor.
+
+		:param length: a.k.a. 'a', 'r' and radius
+		:param offset: a.k.a. 'd' and depth
+		:param twist: a.k.a. 'alpha'
+		:param angle: a.k.a. 'theta'
+		:param convention: DH convention, 's' (standard) or 'm' (modified)
+
+		"""
+		self._length = length
+		self._offset = offset
+		self._twist = twist
+		self._angle = angle
+
+		if convention in ('s', 'm'):
+			self._convention = convention
+		else:
+			raise ValueError('Invalid convention argument: %s' % convention)
+
+	###########################################################################
+	# properties
+	###########################################################################
+
+	@property
+	def length(self):
+		"""Link length a.k.a. `a`, `r` and radius.
+
+		:rtype: float
+
+		"""
+		return self._length
+
+	@property
+	def offset(self):
+		"""Link offset a.k.a. `d` and depth.
+
+		:rtype: float
+
+		"""
+		return self._offset
+
+	@property
+	def twist(self):
+		r"""Link twist a.k.a. :math:`\alpha`.
+
+		:rtype: float
+
+		"""
+		return self._twist
+
+	@property
+	def angle(self):
+		r"""Link angle: a.k.a. :math:`\theta`.
+
+		:rtype: float
+
+		"""
+		return self._angle
+
+	@property
+	def convention(self):
+		"""DH convention, ``'s'`` (standard) or ``'m'`` (modified).
+
+		:rtype: string
+
+		"""
+		return self._convention
+
+	@property
+	def T(self):
+		"""Synonym of :meth:`calc_transform`."""
+		return self.calc_transform()
+
+	###########################################################################
+	# methods
+	###########################################################################
+
+	def calc_transform(self):
+		r"""Calculate the corresponding transformation matrix.
+
+		This matrix, designated by :math:`\operatorname{}^{n - 1}T_n`, is useful
+		to transform points from frame `n` to `n-1`.
+
+		.. math::
+
+			\operatorname{}^{n - 1}T_n =
+			\left[
+			\begin{array}{ccc|c}
+				\cos\theta_n & -\sin\theta_n \cos\alpha_n & \sin\theta_n \sin\alpha_n & a_n \cos\theta_n \\
+				\sin\theta_n & \cos\theta_n \cos\alpha_n & -\cos\theta_n \sin\alpha_n & a_n \sin\theta_n \\
+				0 & \sin\alpha_n & \cos\alpha_n & d_n \\
+				\hline
+				0 & 0 & 0 & 1
+			\end{array}
+			\right]
+
+		source:
+		http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters#Denavit-Hartenberg_matrix
+
+		:rtype: tuple of tuples
+
+		"""
+		if self.convention != 's':
+			raise NotImplementedError('calc_transform is implemented only for'
+										'standard convention')
+
+		a = self.a
+		d = self.d
+		c_th = cos(self.theta)
+		s_th = sin(self.theta)
+		c_al = cos(self.alpha)
+		s_al = sin(self.alpha)
+
+		return (
+			(c_th, -c_al * s_th, s_al * s_th, a * c_th),
+			(s_th, c_al * c_th, -s_al * c_th, a * s_th),
+			(0.0, s_al, c_al, d),
+			(0.0, 0.0, 0.0, 1.0))
+
+	###########################################################################
+	# properties alternative names
+	###########################################################################
+
+	@property
+	def a(self):
+		"""Synonym of :meth:`length`."""
+		return self._length
+
+	@property
+	def d(self):
+		"""Synonym of :meth:`offset`."""
+		return self._offset
+
+	@property
+	def alpha(self):
+		"""Synonym of :meth:`twist`."""
+		return self._twist
+
+	@property
+	def theta(self):
+		"""Synonym of :meth:`angle`."""
+		return self._angle