German Larrain avatar German Larrain committed 1616299

utils.geometry: added '_rot_matrix_to_rpy_angles' (and 'rtb_license'); changed completely 'calc_inclination'.

Comments (0)

Files changed (1)

ars/utils/geometry.py

 import generic as gut
 
 
+rtb_license = """RTB (The Robotics Toolbox for Matlab) is free software: you
+can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+"""
+
+
+def _rot_matrix_to_rpy_angles(rot_matrix, zyx=False):
+	"""The roll-pitch-yaw angles corresponding to a rotation matrix.
+	The 3 angles RPY correspond to sequential rotations about the X, Y and Z
+	axes respectively.
+
+	WARNING: for the convention where Y axis points upwards, swap the returned
+	pitch and yaw. The input remains the same.
+
+
+	Translated to Python by German Larrain.
+
+	Original version in Matlab, part of	'The Robotics Toolbox for Matlab (RTB)'
+	as '/robot/tr2rpy.m'
+	Copyright (C) 1993-2011, by Peter I. Corke. See `rtb_license`.
+
+	"""
+	m = mut.matrix_as_3x3_tuples(rot_matrix)
+
+	# "eps: distance from 1.0 to the next largest double-precision number"
+	eps = 2e-52 # http://www.mathworks.com/help/techdoc/ref/eps.html
+
+	rpy_1 = 0.0
+	rpy_2 = 0.0
+	rpy_3 = 0.0
+
+	if not zyx:
+		# XYZ order
+		if abs(m[2][2]) < eps and abs(m[1][2]) < eps:  # if abs(m(3,3)) < eps && abs(m(2,3)) < eps
+			# singularity
+			rpy_1 = 0.0
+			rpy_2 = mut.atan2(m[0][2], m[2][2])  # atan2(m(1,3), m(3,3))
+			rpy_3 = mut.atan2(m[1][0], m[1][1])  # atan2(m(2,1), m(2,2))
+		else:
+			rpy_1 = mut.atan2(-m[1][2], m[2][2])  # atan2(m(2,1), m(2,2))
+			# compute sin/cos of roll angle
+			sr = mut.sin(rpy_1)  # sr = sin(rpy(1))
+			cr = mut.cos(rpy_1)  # cr = cos(rpy(1))
+			rpy_2 = mut.atan2(m[0][2], cr * m[2][2] - sr * m[1][2])  # atan2(m(1,3), cr * m(3,3) - sr * m(2,3))
+			rpy_3 = mut.atan2(-m[0][1], m[0][0])  # atan2(-m(1,2), m(1,1))
+	else:
+		# old ZYX order (as per Paul book)
+		if abs(m[0][0]) < eps and abs(m[1][0]) < eps:  # if abs(m(1,1)) < eps && abs(m(2,1)) < eps
+			# singularity
+			rpy_1 = 0.0
+			rpy_2 = mut.atan2(-m[2][0], m[0][0])  # atan2(-m(3,1), m(1,1))
+			rpy_3 = mut.atan2(-m[1][2], m[1][1])  # atan2(-m(2,3), m(2,2))
+		else:
+			rpy_1 = mut.atan2(m[1][0], m[0][0])  	# atan2(m(2,1), m(1,1))
+			sp = mut.sin(rpy_1)  					# sp = sin(rpy(1))
+			cp = mut.cos(rpy_1)  					# cp = cos(rpy(1))
+			rpy_2 = mut.atan2(-m[2][0],  			# atan2(-m(3,1),
+				cp * m[0][0] + sp * m[1][0])  		# cp * m(1,1) + sp * m(2,1))
+			rpy_3 = mut.atan2(sp * m[0][2] - cp * m[1][2],  # atan2(sp * m(1,3) - cp * m(2,3),
+				cp * m[1][1] - sp * m[0][1])  				# cp*m(2,2) - sp*m(1,2))
+
+	return rpy_1, rpy_2, rpy_3
+
+
 class Transform:
 	def __init__(self, position, rot_matrix):
 		"""
 	return angles
 
 
-def calc_inclination(rot_matrix, axis=mut.upAxis):
-	"""Returns the inclination of an orientation -defined by the rotation matrix
-	``rot_matrix``- with respect to a plane orthogonal to the given axis, which
-	defaults to the unit vector pointing upwards as defined in this module.
+def calc_inclination(rot_matrix):
+	"""Returns the inclination (as `pitch` and `roll`) inherent of a rotation
+	matrix ``rot_matrix``, with respect to the plane (XZ, since the	vertical
+	axis is Y). `pitch` is the rotation around Z axis and `roll`
+	around Y.
 
-	Example:
-	If rot_matrix = calc_rotation_matrix(rightAxis, pi/4)
+	Examples:
+		rot_matrix = calc_rotation_matrix((1.0, 0.0, 0.0), pi/6)
+		pitch, roll = gemut.calc_inclination(rot_matrix)
+		pitch: 0.0, roll: pi/6
+
+		rot_matrix = calc_rotation_matrix((0.0, 1.0, 0.0), whatever)
+		pitch, roll = gemut.calc_inclination(rot_matrix)
+		pitch: 0.0, roll: 0.0
+
+		rot_matrix = calc_rotation_matrix((0.0, 0.0, 1.0), pi/6)
+		pitch, roll = gemut.calc_inclination(rot_matrix)
+		pitch: pi/6, roll: 0.0
+
 	"""
-	# TODO: complete docstrings
-	# TODO: check tests
-	rotated_axis = mut.rotate3(rot_matrix, axis)
-	angle = mut.acos_dot3(axis, rotated_axis)
-	return angle
+	# THE FOLLOWING worked only in some cases, damn
+	#y_up = mut.upAxis
+	#z_front = mut.bkwdAxis
+	#x_right = mut.rightAxis
+	#
+	#up_rotated = mut.rotate3(rot_matrix, y_up)
+	#pitch_proj = mut.dot_product(mut.cross_product(y_up, up_rotated), x_right)
+	#pitch =  mut.sign(pitch_proj) * mut.acos_dot3(y_up, up_rotated)
+	#
+	#front_rotated = mut.rotate3(rot_matrix, z_front)
+	#roll_proj = mut.dot_product(mut.cross_product(z_front, front_rotated), y_up)
+	#roll = mut.sign(roll_proj) * mut.acos_dot3(z_front, front_rotated)
+	#
+	#return pitch, roll
+
+	roll_x, pitch_y, yaw_z = _rot_matrix_to_rpy_angles(rot_matrix)
+	roll = roll_x
+	pitch = yaw_z
+	#yaw = pitch_y  # we don't need it
+	return pitch, roll
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.