-
Notifications
You must be signed in to change notification settings - Fork 1
/
IK_demo.py
46 lines (38 loc) · 1.4 KB
/
IK_demo.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
# Demonstrates Inverse Kinematics on UR3
import vrep
import time
import numpy as np
from numpy import cos, sin
from scipy.linalg import expm,logm
import utils.math_utils as math_utils
import utils.ur3_utils as ur3_utils
import utils.vrep_utils as vrep_utils
PI = np.pi
# Get T transformation matrix of the object to be picked up
# This is the end-effector position we are trying to reach
def get_target_T(clientID, jointHandles):
# Dummy test function
# [0, PI/2, -0.5, 0, 0, 0]
T = np.array([[ 1. , 0. , 0., -0.112],
[ 0. , 1. , 0., 0.506],
[ 0. , 0. , 1., 0.251],
[ 0. , 0. , 0., 1. ]], dtype=np.float32)
return T
def main():
clientID = vrep_utils.start_simulation()
ur3_utils.check_UR3_exists(clientID)
jointHandles = ur3_utils.get_joint_handles(clientID)
print("> V-REP and UR-3 now setup")
print("> Target position")
targetT = get_target_T(clientID, jointHandles)
time.sleep(1)
print("> Inverse Kinematics")
IKsuccess, calculatedThetaList = ur3_utils.UR3_inverse_kinematics(targetT, clientID, jointHandles)
if not IKsuccess:
print("\t Target position unreachable either due to current algorithm or due to physical constraints")
time.sleep(3)
print("> Exiting Simulation")
vrep_utils.end_simulation(clientID)
return
if __name__ == '__main__':
main()