-
Notifications
You must be signed in to change notification settings - Fork 0
/
scene_object.py
89 lines (81 loc) · 2.95 KB
/
scene_object.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
import vrep
import time
from functools import reduce
from math import sqrt
class Object:
def __init__(self, client_id, name):
self.client_id = client_id
self.name = name
_, self.handle = vrep.simxGetObjectHandle(
self.client_id,
self.name,
operationMode=vrep.simx_opmode_blocking
)
def move_to(self, pose):
res = vrep.simxCallScriptFunction(
clientID=self.client_id,
scriptDescription="UR5",
options=vrep.sim_scripttype_childscript,
operationMode=vrep.simx_opmode_blocking,
functionName='pyMoveToPosition',
inputInts=[self.handle],
inputFloats=pose,
inputStrings=[],
inputBuffer=''
)
print(res)
self.wait_until_stop(self.handle)
def get_position(self):
_, position = vrep.simxGetObjectPosition(
self.client_id,
self.handle,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking
)
return position
def set_position(self, position):
vrep.simxSetObjectPosition(
self.client_id,
self.handle,
relativeToObjectHandle=-1,
position=position,
operationMode=vrep.simx_opmode_oneshot
)
self.wait_until_stop(self.handle)
def get_quaternion(self):
_, quaternion = vrep.simxGetObjectQuaternion(
self.client_id,
self.handle,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking
)
return quaternion
def set_quaternion(self, quaternion):
vrep.simxSetObjectQuaternion(
self.client_id,
self.handle,
relativeToObjectHandle=-1,
quaternion=quaternion,
operationMode=vrep.simx_opmode_oneshot
)
self.wait_until_stop(self.handle)
def get_orientation(self):
_, orientation = vrep.simxGetObjectOrientation(
self.client_id,
self.handle,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking
)
return orientation
def wait_until_stop(self, handle, threshold=0.01):
while True:
_, pos1 = vrep.simxGetObjectPosition(self.client_id, handle, -1, vrep.simx_opmode_blocking)
_, quat1 = vrep.simxGetObjectQuaternion(self.client_id, handle, -1, vrep.simx_opmode_blocking)
time.sleep(0.7)
_, pos2 = vrep.simxGetObjectPosition(self.client_id, handle, -1, vrep.simx_opmode_blocking)
_, quat2 = vrep.simxGetObjectQuaternion(self.client_id, handle, -1, vrep.simx_opmode_blocking)
pose1 = pos1 + quat1
pose2 = pos2 + quat2
theta = 0.5 * sqrt(reduce(lambda x, y: x + y, map(lambda x, y: (x - y) ** 2, pose1, pose2)))
if theta < threshold:
return