forked from interpid2/ROS
-
Notifications
You must be signed in to change notification settings - Fork 0
/
abbCmd.py
executable file
·65 lines (62 loc) · 1.96 KB
/
abbCmd.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
#!/usr/bin/env python
from abblib.abbCtrl import abbRobot
from abblib.abbCtrl import clear
import rospy as rp
import numpy as np
from math import pi
import time as t
#0.94002,0.00003,1.45504
rp.init_node('abbMove_Main',anonymous=False)
np.random.seed(1)
robot=abbRobot()
while not rp.is_shutdown():
choice=input("Choose: "+
"\n0: Init joints"+
"\n1: Demo mode - end effector point space"+
"\n2: Demo mode - end effector point space using constraints"+
"\n3: Demo mode - joint space"
"\n4: Manual end effector points"+
"\n5: Manual joints"+
"\nothers: exit"+
"\n>> ")
if(choice==0):
robot.jointAction([[0,0,0,0,0,0]])
elif(choice==1):
jointList=[]
for i in xrange(10):
x=1
y=np.random.uniform(-1,1,1)
z=np.random.uniform(1,1.5,1)
jointList.append([x,y[0],z[0]])
robot.move2Point(jointList)
elif(choice==2):
waypoints=[]
for i in xrange(10):
if not rp.is_shutdown():
x=1
y=np.random.uniform(0,-1.5,1)[0]
z=np.random.uniform(1,1.5,1)[0]
waypoints.append([x,y,z])
robot.cartesian2Point(waypoints,[0,0,0])
elif(choice==3):
jointList=[]
for i in xrange(10):
jointList.append([0,0,0,0,0,0])
jointList[i][np.random.randint(0,6)]=np.random.uniform(-pi/4,pi/4,1)[0]
robot.jointAction(jointList)
elif(choice==4):
x=input("Enter x coordinate: ")
y=input("Enter y coordinate: ")
z=input("Enter z coordinate: ")
robot.move2Point([[x,y,z]])
elif(choice==5):
jointList=[[0,0,0,0,0,0]]
print "Enter the joint values in degress"
for i in xrange(6):
temp=input("Enter the %i. joint value: "%(i+1))*pi/180
jointList[0][i]=temp
robot.jointAction(jointList)
else:
break
# t.sleep(5)
# clear()