-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmoveTest.py
154 lines (89 loc) · 3.89 KB
/
moveTest.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
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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
from robot import Robot
from time import *
from kinematics import ForwardKinematics, InverseKinematics
from inverse_kin import pose_for_location
import numpy as np
arm = Robot()
origin = [-8.35, 0, 2.4]
joints = [[4.05, 0, 6], [9, 0, 0], [-0.85, 0, 2.165],
[15.8, 0, 0], [1.825, 0, 0.85], [1, -6.75, 0]]
kin = ForwardKinematics(joints, origin)
physical_angles = [90, 150, 140, 98, 75, 0] # true acuator angles used to drive the model
physical_angles = [90, 117, 105, 98, 73, 85] # 105
physical_angles = [90, 117, 105, 98, 73, 85] # 105
physical_angles = [np.radians(i) for i in physical_angles]
B = kin.jointLimitsPhysical(physical_angles)
print(B)
# physical_angles = [np.radians(i) for i in [90, 100, 170, 98, 75, 0]]
logical_angles = kin.physicalToLogicalAngles(physical_angles) # the angles used in the serial linkage model
arm.move2pose(logical_angles)
def goto_phys(angles):
angles = [np.radians(i) for i in angles]
logical_angles = kin.physicalToLogicalAngles(angles)
arm.move2pose(logical_angles)
sleep(1)
arm.print_status()
def goto_logical(angles):
angles = [np.radians(i) for i in angles]
arm.move2pose(angles)
sleep(1)
arm.print_status()
# goto_phys([90, 100, 170, 98, 75, 0])
# moves = [[160, 116, 132, 90, 75, 95], [157, 120, 170, 90, 120, 95], [157, 90, 180, 90, 120, 95], [20, 116, 132, 90, 75, 95], [17, 120, 170, 90, 120, 95], [17, 90, 180, 90, 120, 95]]
# moves = []
# sleep(1)
# arm.dipPaint()
# for move in moves:
# physical_angles = [np.radians(i) for i in move]
# logical_angles = kin.physicalToLogicalAngles(physical_angles) # the angles used in the serial linkage model
# arm.move2pose(logical_angles)
# print(move)
# sleep(0.3)
# physical_angles = [90, 100, 90, 98, 68, 0] # true acuator angles used to drive the model
# physical_angles = [np.radians(i) for i in physical_angles]
# arm.move2pose(physical_angles, physical=True)
# arm = Robot()
# physical_angles = [1.570, 2.443, 2.094, 1.570, 2.792, 3.141] # ture acuator angles used to drive the model
# logical_angles = kin.physicalToLogicalAngles(physical_angles)
# Output = ["%.2f" % np.degrees(elem) for elem in physical_angles]
# print("start_logical: ", Output)
# physical = kin.logicalToPhysicalAngles(logical_angles)
# Output = ["%.2f" % np.degrees(elem) for elem in physical]
# print("start_physical: ", Output)
# Output = ["%.2f" % np.degrees(elem) for elem in physical_angles]
# print("start_physical_real: ", Output)
# start_pose = logical_angles
# goal = np.array([-3, 14.5, 3])
# logical = pose_for_location(kin, start_pose, goal)
# physical = kin.logicalToPhysicalAngles(logical)
# # Output = ["%.2f" % np.degrees(elem) for elem in logical]
# # print("move to: ", Output)
# Output = ["%.2f" % np.degrees(elem) for elem in physical]
# print("move to: ", Output)
# user = input("start: ")
# arm.move2pose(physical)
# #116.4
# # pose = [1.571, 1.571, 1.571, 1.571, 2.793, 1.571]
# pose = [np.radians(pose[i]) for i in range(6)]
# arm.move2pose(pose)
# origin = [-8.35, 0, 2.6]
# # cheack [4.05, 0, 6.25]
# # joints = [[4.05, 0, 6.25], [0, 0, 9], [-0.85, 0, 2.165],
# # [15.8, 0, 0], [1.825, 0, 0.85], [0, 0, 0]]
# joints = [[4.05, 0, 6.75], [9, 0, 0], [-2.165, 0, 0.85],
# [15.8, 0, 0], [1.825, 0, 0.85], [1.55, 0, -6.75]]
# kin = ForwardKinematics(joints, origin)
# logical = kin.physicalToLogicalAngles(pose)
# Output = ["%.2f" % np.degrees(elem) for elem in logical]
# print("logical: ", Output)
# physical = kin.logicalToPhysicalAngles(logical)
# Output = ["%.2f" % np.degrees(elem) for elem in physical]
# print("physical: ", Output)
# pos = kin.getPos(logical)
# print("cord: ", pos[0:3])
# physical = kin.logicalToPhysicalAngles(logical)
# Output = ["%.2f" % np.degrees(elem) for elem in physical]
# print(Output)
# test_pose = kin.logicalToPhysicalAngles(pose_new)
# user = input("stop: ")
# arm.stopRobot()