# Commits

committed 1616299

• Participants
• Parent commits 5042d1c
• Branches dev

# File ars/utils/geometry.py

• Ignore whitespace
` 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`