-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathtest.py
executable file
·100 lines (74 loc) · 3.35 KB
/
test.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
import kinovapy
import sys
if kinovapy.InitAPI() != kinovapy.NO_ERROR_KINOVA:
print 'Error initializing Kinova API'
sys.exit(1)
deviceCount = kinovapy.GetNumDevices()
print 'deviceCount', deviceCount
if deviceCount < 0:
print 'Error getting device count'
sys.exit(2)
print 'device count', deviceCount
print 'Found %d devices' % deviceCount
for i in xrange(0, deviceCount):
print 'Selecting arm #%d' % (i)
kinovapy.SetActiveDeviceNum(i)
status = kinovapy.QuickStatus()
kinovapy.GetQuickStatus(status)
print 'Homing arm and fingers...'
kinovapy.MoveHome()
kinovapy.InitFingers()
actualPosition = kinovapy.AngularPosition()
kinovapy.GetAngularCommand(actualPosition)
pointToSend = kinovapy.TrajectoryPoint()
pointToSend.InitStruct()
pointToSend.Position.Type = kinovapy.ANGULAR_POSITION
pointToSend.LimitationsActive = 1
# Set velocity limitation to 30 degrees per second for joint 1, 2 and 3.
pointToSend.Limitations.speedParameter1 = 30.0
# Set velocity limitation to 20 degrees per second for joint 4, 5 and 6
pointToSend.Limitations.speedParameter2 = 20.0
pointToSend.Position.Actuators.Actuator1 = actualPosition.Actuators.Actuator1 + 50
pointToSend.Position.Actuators.Actuator2 = actualPosition.Actuators.Actuator2
pointToSend.Position.Actuators.Actuator3 = actualPosition.Actuators.Actuator3
pointToSend.Position.Actuators.Actuator4 = actualPosition.Actuators.Actuator4
pointToSend.Position.Actuators.Actuator5 = actualPosition.Actuators.Actuator5
pointToSend.Position.Actuators.Actuator6 = actualPosition.Actuators.Actuator6
if status.RobotType == 0 or status.RobotType == 3:
# If the robotic arm is a JACO or JACO2, we use those value for the fingers.
pointToSend.Position.Fingers.Finger1 = 45.0
pointToSend.Position.Fingers.Finger2 = 45.0
pointToSend.Position.Fingers.Finger3 = 45.0
elif status.RobotType == 1:
# If the robotic arm is a MICO, we use those value for the fingers.
pointToSend.Position.Fingers.Finger1 = 4500.0
pointToSend.Position.Fingers.Finger2 = 4500.0
pointToSend.Position.Fingers.Finger3 = 4500.0
else:
pointToSend.Position.Fingers.Finger1 = 0.0
pointToSend.Position.Fingers.Finger2 = 0.0
pointToSend.Position.Fingers.Finger3 = 0.0
print "Sending trajectory 1"
# Add the point to the robot's FIFO
kinovapy.SendAdvanceTrajectory(pointToSend)
# Modify the position of the actuator #3
print 'Moving joint 1 back by -100.0'
pointToSend.Position.Actuators.Actuator1 = actualPosition.Actuators.Actuator1 - 100.0
# Add the point to the robot's FIFO
kinovapy.SendAdvanceTrajectory(pointToSend)
# Modify the position of the actuator #1
print 'Moving joint 1 forward by 100'
pointToSend.Position.Actuators.Actuator1 = actualPosition.Actuators.Actuator1 + 100.0
# Add the point to the robot's FIFO
kinovapy.SendAdvanceTrajectory(pointToSend)
# Modify again the position of the actuator #3
print 'Moving joint 3 up by 20'
pointToSend.Position.Actuators.Actuator3 = actualPosition.Actuators.Actuator3 + 20
kinovapy.SendAdvanceTrajectory(pointToSend)
# Modify again the position of the actuator #3
print 'Moving joint 3 down by 10'
pointToSend.Position.Actuators.Actuator3 = actualPosition.Actuators.Actuator3 - 10
# Add the point to the robot's FIFO
kinovapy.SendAdvanceTrajectory(pointToSend)
print 'End of example'
kinovapy.CloseAPI()