Commits

German Larrain committed d5971fe

DHLink.calc_transform: implemented for the 'modified' convention.

  • Participants
  • Parent commits b583a54
  • Branches dev-DH

Comments (0)

Files changed (1)

File ars/model/robot/dh.py

 		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`.
+		to transform points from frame `n` to `n-1`. For the standard
+		convention:
 
 		.. math::
 
 		:rtype: tuple of tuples
 
 		"""
-		if self.convention != 's':
-			raise NotImplementedError('calc_transform is implemented only for'
-										'standard convention')
-
+		conv = self.convention
 		a = self.a
 		d = self.d
 		c_th = cos(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))
+		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