libWall/gl_frame.py at master · pchalas1/libWall · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
from .gl_vec3 import Vec3
from .gl_utils import *

import PyKDL
import math

class Frame(object):

	"""	@author Preetham Chalasani
	@brief Frame with a translation and a rotation. Helps to store a transformation matrix.
	"""

	def __init__(self, rot, trans):

		"""Intialize a frame with a rotation and a translation
		@param rot : Rotation vector (degrees), Vec3
		@param trans : Translation Vector, Vec3
		"""

		self.__rotationVec = 0
		self.__transVec = 0
		self.__frame = 0

		if type(rot).__name__ == 'Vec3' and type(trans).__name__ == 'Vec3':
			pass
			self.__rotationVec = rot
			self.__transVec = trans

		elif type(rot).__name__ == 'list' and type(trans).__name__ == 'list':
			self.__rotationVec = Vec3(rot)
			self.__transVec = Vec3(trans)

		elif type(rot).__name__ == 'list' and type(trans).__name__ == 'Vec3':
			self.__rotationVec = Vec3(rot)
			self.__transVec = trans

		elif type(rot).__name__ == 'Vec3' and type(trans).__name__ == 'list':
			self.__rotationVec = rot
			self.__transVec = Vec3(trans)

		if not type(self.__transVec).__name__ == 'int' or not type(self.__rotationVec).__name__ == 'int' :
			self.__frame = PyKDL.Frame(PyKDL.Rotation.EulerZYX(self.__rotationVec[0]*math.pi/180,
											self.__rotationVec[1]*math.pi/180,
											self.__rotationVec[2]*math.pi/180),
								PyKDL.Vector(self.__transVec[0],
									   self.__transVec[1],
									   self.__transVec[2])
							   )
		pass

	def translation(self, value=None):

		"""Set/get a translation vector
		@param value : Vec3
		@return translation : Vec3
		"""

		if type(value).__name__ == 'Vec3':
			self.__transVec = value
		elif type(value).__name__ == 'list':
			self.__transVec = Vec3(value)
		elif value == None:
			return self.__transVec
		pass

	def rotation(self, value=None):

		"""Set/get a rotation vector
		@param value : Vec3
		@return rotation : Vec3
		"""

		if type(value).__name__ == 'Vec3':
			self.__rotationVec = value
		elif type(value).__name__ == 'list':
			self.__rotationVec = Vec3(value)
		elif value == None:
			return self.__rotationVec
		pass

	def matrix(self):

		"""Return the frame matrix
		@return matrix : Frame
		"""

		return self.__frame

	def __toFrame(self, kdlFrame):
		t = Vec3(kdlFrame.p[0], kdlFrame.p[1], kdlFrame.p[2])
		r = Vec3(kdlFrame.M.GetEulerZYX()[0],
				 kdlFrame.M.GetEulerZYX()[1],
				 kdlFrame.M.GetEulerZYX()[2])

		return Frame(r,t)

	def __mul__(self, a):

		"""Frame multiplication with a Vec3 or another Frame
		@param a : Vec3 or Frame
		@return val : Vec3 or Frame
		"""

		temp = 0
		if type(a).__name__ == 'Vec3':
			temp = self.__frame * PyKDL.Vector(a.X(), a.Y(), a.Z())
			return(Vec3(temp[0], temp[1], temp[2]))

		elif type(a).__name__ == 'list' and len(a) == 3:
			temp = self.__frame * PyKDL.Vector(a[0], a[1], a[2])
			return(Vec3(temp[0], temp[1], temp[2]))

		elif type(a).__name__ == 'Frame':
			tempVec = PyKDL.Vector(a.translation()[0], a.translation()[1], a.translation()[2])
			tempRot = PyKDL.Rotation.EulerZYX(a.rotation()[0], a.rotation()[1], a.rotation()[2])

			tempFrame = self.__frame * PyKDL.Frame(tempRot, tempVec)
			return self.__toFrame(tempFrame)

	def __str__(self):
		return(str(self.__transVec) + str(self.__rotationVec))