-
Notifications
You must be signed in to change notification settings - Fork 0
/
Plate.py
91 lines (61 loc) · 3.88 KB
/
Plate.py
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
import numpy as np
from numpy import sin,cos,deg2rad
from CinematicaInversa.MatrixFunctions import rotate3d, translate3d
class Plate:
def __init__(self,height):
self.pointsDistance = 10 # Distance of points in relation to center, in cm
self.anglePoints = 120 # Angle between points
self.height = height
self.radius = 15 # radius of plate
self.xAngle = 0
self.yAngle = 0
self.centerPos = np.matrix([[0,
0,
self.height]]).T # Center of the plate, theoretically unchanged during all movements
self.magnet1PosZero = np.matrix([[0 + self.centerPos.A1[0],
self.pointsDistance + self.centerPos.A1[1],
self.centerPos.A1[2]]]).T
self.magnet2PosZero = rotate3d(self.magnet1PosZero,0,0,deg2rad(-self.anglePoints)) # Calculated as 11 cm from center and rotated 120 between them
self.magnet3PosZero = rotate3d(self.magnet1PosZero,0,0,deg2rad(-2*self.anglePoints))
def setHeight(self,height):
self.height = height
self.centerPos = np.matrix([[0,
0,
self.height]]).T # Center of the plate, theoretically unchanged during all movements
self.magnet1PosZero = np.matrix([[0 + self.centerPos.A1[0],
self.pointsDistance + self.centerPos.A1[1],
self.centerPos.A1[2]]]).T
self.magnet2PosZero = rotate3d(self.magnet1PosZero,0,0,deg2rad(-self.anglePoints)) # Calculated as 11 cm from center and rotated 120 between them
self.magnet3PosZero = rotate3d(self.magnet1PosZero,0,0,deg2rad(-2*self.anglePoints))
def rotatePlate(self,angleX,angleY): # in Degrees
self.xAngle = angleX
self.yAngle = angleY
oldPos = self.centerPos
newcenterPos = rotate3d(self.centerPos,deg2rad(angleX),deg2rad(angleY),0)
self.deltaPos = oldPos-newcenterPos
self.magnet1Pos = rotate3d(self.magnet1PosZero,deg2rad(angleX),deg2rad(angleY),0)
self.magnet1Pos = translate3d(self.magnet1Pos,self.deltaPos)
self.magnet2Pos = rotate3d(self.magnet2PosZero,deg2rad(angleX),deg2rad(angleY),0)
self.magnet2Pos = translate3d(self.magnet2Pos,self.deltaPos)
self.magnet3Pos = rotate3d(self.magnet3PosZero,deg2rad(angleX),deg2rad(angleY),0)
self.magnet3Pos = translate3d(self.magnet3Pos,self.deltaPos)
# def rotatePlate(self,angleX,angleY): # Optmized
# # Baseado no material escrito no caderno cinematica inversa
# self.xAngle = angleX
# self.yAngle = angleY
# angleX = deg2rad(angleX)
# angleY = deg2rad(angleY)
# #Obter o novo centro e calcular seu delta, para trasladar depois
# oldPos = self.centerPos
# newcenterPos = rotate3d(self.centerPos,deg2rad(angleX),deg2rad(angleY),0)
# self.deltaPos = oldPos-newcenterPos
# magPosZero = [self.magnet1PosZero,self.magnet2PosZero,self.magnet3PosZero]
# magPos = [1,2,3]
# for i in range(len(magPos)): # Fazer com as 3 juntas
# x,y,z = magPosZero[i].A1
# magPos[i] = np.matrix([[x*cos(angleY)+z*sin(angleY)],
# [x*sin(angleX)*sin(angleY)+y*cos(angleX)-z*sin(angleX)*cos(angleY)],
# [-x*cos(angleX)*sin(angleY)+y*sin(angleX)+z*cos(angleX)*cos(angleY)]])
# magPos[i] = translate3d(magPos[i],self.deltaPos)
# # print(magPos[i])
# self.magnet1Pos, self.magnet2Pos, self.magnet3Pos = magPos