easy_ur/examples/examples.py at main · achuwilson/easy_ur · 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
from easyUR import UR
import math
import time
import numpy as np

robot=UR()


#################### EE POSITION CONTROL ######################
current_position, current_orientation = robot.get_pose()

print("current robot position :",current_position)
print("current robot orientation :",current_orientation)
#increment x axis
next_position = [current_position[0]+0.05, current_position[1], current_position[2]]
#move to position
robot.set_pose(next_position, current_orientation)
#move asyncronously -  it returns control immediately and doesnt wait until
#you reach the position -  you can use
robot.set_pose(current_position, current_orientation, async = True)
time.sleep(1.0)
#during asynchronous move, you can stop robot motion in between
robot.stop()

#set end effector speed and acceleration to a low value and move
#values are in m/s and m/s2
print("LOW SPEED EE MOVE")
robot.set_ee_speed(0.05)
robot.set_ee_acceleration(0.2)
robot.set_pose(next_position, current_orientation)
robot.set_pose(current_position, current_orientation)

#set end effector speed and acceleration to a high value and move
print("HIGH SPEED EE MOVE")
robot.set_ee_speed(0.5)
robot.set_ee_acceleration(1.5)
robot.set_pose(next_position, current_orientation)
robot.set_pose(current_position, current_orientation)

##set end effector speed and acceleration to a low value
robot.set_ee_speed(0.1)
robot.set_ee_acceleration(0.5)

################## JOINT CONTROL ##########################
print("JOINT CONTROL MODE")
#get the joint positions
joint_poses = robot.get_joint_positions()
print("Current joint positions", joint_poses)
#increment the last axis
next_q = joint_poses
next_q[5] = next_q[5] + math.radians(20)
#set joint positions
robot.set_joint_positions(next_q)
next_q[5] = next_q[5] - math.radians(20)
robot.set_joint_positions(next_q, async=True)
time.sleep(1.0)
#during asynchronous move, you can stop robot motion in between
robot.stop()

#joint speed control
print("LOW SPEED JOINT MOVE")
#values are in radians/s and radians/s2
robot.set_joint_speed(0.1)
robot.set_joint_acceleration(0.5)
#increment the last axis
next_q = joint_poses
next_q[5] = next_q[5] + math.radians(20)
#set joint positions
robot.set_joint_positions(next_q)
next_q[5] = next_q[5] - math.radians(20)
robot.set_joint_positions(next_q, async=True)
time.sleep(1.0)
#during asynchronous move, you can stop robot motion in between
robot.stop()


################## FREEDRIVE MODE ##############################

robot.freedrive(True)
print("FREEDRIVE MODE ON")
time.sleep(0.5)
for i in range(1000):
	time.sleep(0.01)
	print(robot.get_pose()[0])
	#print(robot.get_joint_positions())

robot.freedrive(False)
print("FREEDRIVE MODE OFF")

################################ SERVOING #################################
print("SERVO MODE ON")
#get current pos
#play around with t_delay and the interpolation distance
current_position, current_orientation = robot.get_pose()
next_position = [current_position[0]+0.1, current_position[1], current_position[2]]

dist = np.linalg.norm(np.array(current_position)-np.array(next_position))
num_points = int(dist/0.00055)
lin_path = np.linspace(current_position,next_position,num_points)

step_count=0
t_delay=1.0/500.0 # 500 Hz control loop

print("Num_points Servo= ", num_points)
while step_count<num_points:
    robot.servo(lin_path[step_count],current_orientation)
    time.sleep(t_delay)
    step_count=step_count+1
    print("servo move ", step_count)
    if(step_count==120):
        time.sleep(t_delay)# use delay before breaking
        print("breaking")
        break

############################### Moving Back #################################
time.sleep(2)
print("moving to start pos")
robot.set_pose(current_position, current_orientation)