-
Notifications
You must be signed in to change notification settings - Fork 13
/
goToTarget.py
105 lines (72 loc) · 5.53 KB
/
goToTarget.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
100
101
102
103
104
105
import vrep
import numpy as np
import math
# data = np.genfromtxt("data.txt")
# data = np.around(data, decimals=4)
# data = data.tolist()
print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19997,True,True,-500000,5) # Connect to V-REP, set a very large time-out for blocking commands
if clientID!=-1:
print ('Connected to remote API server')
emptyBuff = bytearray()
#Start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/v_rep/qrelease/release/test.ttm'],emptyBuff,vrep.simx_opmode_oneshot_wait)
# robotHandle=retInts[0]
# Retrieve some handles:
res,robotHandle=vrep.simxGetObjectHandle(clientID,'UR5#',vrep.simx_opmode_oneshot_wait)
res,target1=vrep.simxGetObjectHandle(clientID,'testPose1#',vrep.simx_opmode_oneshot_wait)
#### original four points
tryConfig_1 = [-1.0110, -1.1291, 0.7916, 0.3375, 2.1395, 0.7224]
tryConfig_2 = [0.7278, -1.8713, -1.2708, 0.0005, 0.8948, -2.4192]
tryConfig_3 = [-0.5999, -2.3992, -1.7141, 0.9717, 2.2225, -2.4192]
tryConfig_4 = [-1.0094, -1.1576, 1.3857, -0.2281, 2.1271, -2.4192]
###right side (1,1)
tryConfig1_1_r= [0.9226, -2.3992, -1.7141, 0.9717, 0.700, -2.4192]
##left side (2,1)
tryConfig2_1_l = [-2.5410, -1.1576, 1.3857, -0.2281, 0.6095, -2.4192]
###left side(2,2)
tryConfig2_2_l = [-2.2910, -1.4576, 1.6857, -0.2281, 0.8595, -2.4192]
###left side (1,1)
tryConfig1_1_l = [-2.5410, -0.9576, 1.5857, -0.6281, 0.6095, -2.4192]
#### deep shelf
tryConfig_d_1 = [2.7915,-0.8713,0.0708,0.8005,1.2208,-2.4192]
tryConfig_origin = [0.0059814453125, -1.5647833347320557, 0.0019979476928710938, 0.006038188934326172, 0.005944967269897461, -0.0059778690338134766]
tryConfig_d_2 = [-0.1,-1.8013,-1.6908,0.4521,1.67,-2.4192]
# Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene
res,retInts,target1Pose,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'getObjectPose',[target1],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
res, retInts, robotInitialState, retStrings, retBuffer = vrep.simxCallScriptFunction(clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
# Some parameters:
approachVector = [0, 0, 0] #often a linear approach is required. This should also be part of the calculations when selecting an appropriate state for a given pose
maxConfigsForDesiredPose = 100 # we will try to find 10 different states corresponding to the goal pose and order them according to distance from initial state
maxTrialsForConfigSearch=300 # a parameter needed for finding appropriate goal states
searchCount=8 # how many times OMPL will run for a given task
minConfigsForPathPlanningPath=400 # interpolation states for the OMPL path
minConfigsForIkPath=100 # interpolation states for the linear approach path
collisionChecking=1 # whether collision checking is on or off
inInts=[robotHandle,collisionChecking,minConfigsForPathPlanningPath,searchCount]
res, retInts, robotCurrentConfig, retStrings ,retBuffer = vrep.simxCallScriptFunction(clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript, 'getRobotState',[robotHandle],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
inFloats = robotCurrentConfig + tryConfig_1
# inFloats = robotCurrentConfig + tryConfig_d_1
res,retInts,path,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'findPath_goalIsState',inInts,inFloats,[],emptyBuff,vrep.simx_opmode_oneshot_wait)
if (res==0) and len(path)>0:
# Visualize the path:
res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'visualizePath',[robotHandle,255,0,255],path,[],emptyBuff,vrep.simx_opmode_oneshot_wait)
lineHandle=retInts[0]
# Make the robot follow the path:
res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'runThroughPath',[robotHandle],path,[],emptyBuff,vrep.simx_opmode_oneshot_wait)
# Wait until the end of the movement:
runningPath=True
while runningPath:
res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
runningPath=retInts[0]==1
# Clear the path visualization:
res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'removeLine',[lineHandle],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
# Stop simulation:
vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print ('Failed connecting to remote API server')
print ('Program ended')