You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
161 lines
12 KiB
161 lines
12 KiB
# This example illustrates how to use the path/motion
|
|
# planning functionality from a remote API client.
|
|
#
|
|
# Load the demo scene 'motionPlanningServerDemo.ttt' in CoppeliaSim
|
|
# then run this program.
|
|
#
|
|
# IMPORTANT: for each successful call to simxStart, there
|
|
# should be a corresponding call to simxFinish at the end!
|
|
|
|
import sim
|
|
|
|
print ('Program started')
|
|
sim.simxFinish(-1) # just in case, close all opened connections
|
|
clientID=sim.simxStart('127.0.0.1',19997,True,True,-500000,5) # Connect to CoppeliaSim, set a very large time-out for blocking commands
|
|
if clientID!=-1:
|
|
print ('Connected to remote API server')
|
|
|
|
emptyBuff = bytearray()
|
|
|
|
# Start the simulation:
|
|
sim.simxStartSimulation(clientID,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/coppeliaRobotics/qrelease/release/test.ttm'],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
# robotHandle=retInts[0]
|
|
|
|
# Retrieve some handles:
|
|
res,robotHandle=sim.simxGetObjectHandle(clientID,'IRB4600#',sim.simx_opmode_oneshot_wait)
|
|
res,target1=sim.simxGetObjectHandle(clientID,'testPose1#',sim.simx_opmode_oneshot_wait)
|
|
res,target2=sim.simxGetObjectHandle(clientID,'testPose2#',sim.simx_opmode_oneshot_wait)
|
|
res,target3=sim.simxGetObjectHandle(clientID,'testPose3#',sim.simx_opmode_oneshot_wait)
|
|
res,target4=sim.simxGetObjectHandle(clientID,'testPose4#',sim.simx_opmode_oneshot_wait)
|
|
|
|
# Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene
|
|
res,retInts,target1Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target1],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
res,retInts,target2Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target2],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
res,retInts,target3Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target3],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
res,retInts,target4Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target4],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Get the robot initial state:
|
|
res,retInts,robotInitialState,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Some parameters:
|
|
approachVector=[0,0,1] # often a linear approach is required. This should also be part of the calculations when selecting an appropriate state for a given pose
|
|
maxConfigsForDesiredPose=10 # 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=2 # 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
|
|
|
|
# Display a message:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayMessage',[],[],['Computing and executing several path planning tasks for a given goal pose.&&nSeveral goal states corresponding to the goal pose are tested.&&nFeasability of a linear approach is also tested. Collision detection is on.'],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Do the path planning here (between a start state and a goal pose, including a linear approach phase):
|
|
inInts=[robotHandle,collisionChecking,minConfigsForIkPath,minConfigsForPathPlanningPath,maxConfigsForDesiredPose,maxTrialsForConfigSearch,searchCount]
|
|
inFloats=robotInitialState+target1Pose+approachVector
|
|
res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findPath_goalIsPose',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
if (res==0) and len(path)>0:
|
|
# The path could be in 2 parts: a path planning path, and a linear approach path:
|
|
part1StateCnt=retInts[0]
|
|
part2StateCnt=retInts[1]
|
|
path1=path[:part1StateCnt*6]
|
|
|
|
# Visualize the first path:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,255,0,255],path1,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
line1Handle=retInts[0]
|
|
|
|
# Make the robot follow the path:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path1,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Wait until the end of the movement:
|
|
runningPath=True
|
|
while runningPath:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
runningPath=retInts[0]==1
|
|
|
|
path2=path[part1StateCnt*6:]
|
|
|
|
# Visualize the second path (the linear approach):
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,0,255,0],path2,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
line2Handle=retInts[0]
|
|
|
|
# Make the robot follow the path:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path2,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Wait until the end of the movement:
|
|
runningPath=True
|
|
while runningPath:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
runningPath=retInts[0]==1
|
|
|
|
# Clear the paths visualizations:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[line1Handle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[line2Handle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Get the robot current state:
|
|
res,retInts,robotCurrentConfig,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Display a message:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayMessage',[],[],['Computing and executing several path planning tasks for a given goal state. Collision detection is on.'],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Do the path planning here (between a start state and a goal state):
|
|
inInts=[robotHandle,collisionChecking,minConfigsForPathPlanningPath,searchCount]
|
|
inFloats=robotCurrentConfig+robotInitialState
|
|
res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findPath_goalIsState',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
if (res==0) and len(path)>0:
|
|
# Visualize the path:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,255,0,255],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
lineHandle=retInts[0]
|
|
|
|
# Make the robot follow the path:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Wait until the end of the movement:
|
|
runningPath=True
|
|
while runningPath:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
runningPath=retInts[0]==1
|
|
|
|
# Clear the path visualization:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[lineHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Collision checking off:
|
|
collisionChecking=0
|
|
|
|
# Display a message:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayMessage',[],[],['Computing and executing several linear paths, going through several waypoints. Collision detection is OFF.'],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Find a linear path that runs through several poses:
|
|
inInts=[robotHandle,collisionChecking,minConfigsForIkPath]
|
|
inFloats=robotInitialState+target2Pose+target1Pose+target3Pose+target4Pose
|
|
res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findIkPath',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
if (res==0) and len(path)>0:
|
|
# Visualize the path:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,0,255,255],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
line1Handle=retInts[0]
|
|
|
|
# Make the robot follow the path:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Wait until the end of the movement:
|
|
runningPath=True
|
|
while runningPath:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
runningPath=retInts[0]==1
|
|
|
|
# Clear the path visualization:
|
|
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[line1Handle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Stop simulation:
|
|
sim.simxStopSimulation(clientID,sim.simx_opmode_oneshot_wait)
|
|
|
|
# Now close the connection to CoppeliaSim:
|
|
sim.simxFinish(clientID)
|
|
else:
|
|
print ('Failed connecting to remote API server')
|
|
print ('Program ended')
|