Source

ars / ars / model / robot / dh.py

Full commit
r"""This module contains Denavit-Hartenberg related code.

For a serial chain of links (defined by D-H parameters), matrix `T` defines the
pose (position and orientation) of the end effector with respect to the base,
based in the joints' `n` coordinates.

.. math::

	T = \operatorname{}^{0}A_1 \cdot \operatorname{}^{1}A_2 \cdots \operatorname{}^{n - 1}A_n

"""
from ars.utils.mathematical import sin, cos, matrix_multiply


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'``, default) and Craig's
	modified (``'m'``).

	>>> (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 link homogeneous transformation matrix.

		This matrix, designated as :math:`\operatorname{}^{n - 1}T_n`, is useful
		to transform points from frame `n` to `n-1`. For the standard
		convention:

		.. 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

		:return: 4x4 matrix
		:rtype: tuple of tuples

		"""
		conv = self.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)

		if conv == 's':
			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))
		elif conv == 'm':
			return (
				(c_th,        -s_th,       0.0,   a),
				(c_al * s_th, c_al * c_th, -s_al, -s_al * d),
				(s_al * s_th, s_al * c_th, c_al,  c_al * d),
				(0.0,         0.0,         0.0,   1.0))
		else:
			raise ValueError('Invalid convention argument: %s' % conv)

	###########################################################################
	# 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


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()