def init_viewer(self,
env_name,
env_package="hpp_environments",
reduce_sizes=[0, 0, 0],
visualize_affordances=[]):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
:param env_package: name of the package containing this urdf (default to hpp_environments)
:param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
(in order to avoid putting contacts closes to the edges of the surface)
:param visualize_affordances: list of affordances type to visualize, default to none
"""
vf = ViewerFactory(self.ps)
self.afftool = AffordanceTool()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" +
env_name + ".urdf",
"planning",
vf,
reduceSizes=reduce_sizes)
self.v = vf.createViewer(ghost=True, displayArrows=True)
self.pp = PathPlayer(self.v)
for aff_type in visualize_affordances:
self.afftool.visualiseAffordances(aff_type, self.v,
self.v.color.lightBrown)
def initScene(Robot, envName="multicontact/ground", genLimbsDB=True):
from hpp.gepetto import Viewer, ViewerFactory
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver import ProblemSolver
fullBody = Robot()
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.setJointBounds("root_joint", [-100, 100, -100, 100, -100, 100])
fullBody.client.robot.setExtraConfigSpaceBounds(
[-100, 100, -100, 100, -100, 100, -100, 100, -100, 100, -100, 100])
fullBody.setReferenceConfig(fullBody.referenceConfig[::] + [0] * 6)
fullBody.setPostureWeights(fullBody.postureWeights[::] + [0] * 6)
try:
if genLimbsDB:
fullBody.loadAllLimbs("static", nbSamples=100)
else:
fullBody.loadAllLimbs("static", nbSamples=1)
except AttributeError:
print(
"WARNING initScene : fullBody do not have loadAllLimbs, some scripts may fails."
)
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
vf.loadObstacleModel(
"package://hpp_environments/urdf/" + envName + ".urdf", "planning")
v = vf.createViewer(ghost=True, displayCoM=True)
v(fullBody.getCurrentConfig())
return fullBody, v
def createViewer(self,
ViewerClass=Viewer,
viewerClient=None,
host=None,
collisionURDF=False):
if host is not None and viewerClient is None:
from gepetto.corbaserver import Client as GuiClient
viewerClient = GuiClient(host=host)
v = Parent.createViewer(self, ViewerClass, viewerClient, host,
collisionURDF)
return v
def initScene(Robot, envName="multicontact/ground"):
from hpp.gepetto import Viewer, ViewerFactory
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver import ProblemSolver
fullBody = Robot()
fullBody.client.robot.setDimensionExtraConfigSpace(6)
try:
fullBody.loadAllLimbs("static", nbSamples=1)
except AttributeError:
print "WARNING initScene : fullBody do not have loadAllLimbs, some scripts may fails."
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
vf.loadObstacleModel("hpp_environments", envName, "planning")
v = vf.createViewer(displayCoM=True)
v(fullBody.getCurrentConfig())
return fullBody, v
def initScene(Robot,
envName="multicontact/ground",
genLimbsDB=True,
context=None):
"""
Create and initialize a hpp.gepetto.Viewer object with a hpp.corbaserver.rbprm.FullBody object
:param Robot: a Robot configuration class (eg. the class defined in talos_rbprm.talos.Robot)
:param envName: the name of the environment file to load. Load a flat floor at z = 0 by default
Currently only work with environments from the hpp-environment package
:param genLimbsDB: If True (default value) generate the limbs database of the fullbody object
:param context: An optional string that give a name to a corba context instance
:return: a Fullbody object and a Viewer object
"""
from hpp.gepetto import ViewerFactory
fullBody, ps = build_fullbody(Robot, genLimbsDB, context)
vf = ViewerFactory(ps)
vf.loadObstacleModel(
"package://hpp_environments/urdf/" + envName + ".urdf", "planning")
v = vf.createViewer(ghost=True, displayCoM=True)
v(fullBody.getCurrentConfig())
return fullBody, v
def init_viewer(self,
env_name,
env_package="hpp_environments",
reduce_sizes=[0, 0, 0],
visualize_affordances=[],
min_area=None):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
:param env_package: name of the package containing this urdf (default to hpp_environments)
:param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
(in order to avoid putting contacts closes to the edges of the surface)
:param visualize_affordances: list of affordances type to visualize, default to none
:param min_area: list of couple [affordanceType, size]. If provided set the minimal area for each affordance
"""
vf = ViewerFactory(self.ps)
if self.context:
self.afftool = AffordanceTool(context=self.context)
self.afftool.client.affordance.affordance.resetAffordanceConfig(
) # FIXME: this should be called in afftool constructor
else:
self.afftool = AffordanceTool()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
if min_area is not None:
for (aff_type, min_size) in min_area:
self.afftool.setMinimumArea(aff_type, min_size)
self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" +
env_name + ".urdf",
"planning",
vf,
reduceSizes=reduce_sizes)
self.v = vf.createViewer(ghost=True, displayArrows=True)
self.pp = PathPlayer(self.v)
for aff_type in visualize_affordances:
self.afftool.visualiseAffordances(aff_type, self.v,
self.v.color.lightBrown)
raise RuntimeError ("Failed to apply constraint.")
ps.selectPathValidation ("Progressive", 0.025)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
ps.client.problem.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q1proj)
ps.addGoalConfig (q2proj)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
totalTime += t2 - t1
print (t2-t1)
n = len (ps.client.problem.nodes ())
totalNumberNodes += n
print ("Number nodes: " + str(n))
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float (N)))
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
r = vf.createViewer()
from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client, r)
q_goal [rank] = -0.5
vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.selectPathValidation ("Progressive", 0.025)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
totalTime += t2 - t1
print (t2-t1)
n = len (ps.client.problem.nodes ())
totalNumberNodes += n
print ("Number nodes: " + str(n))
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float (N)))
r = vf.createViewer(); r (q_init)
pp = PathPlayer (robot.client, r)
class HppServerInitializer(HppClient):
subscribersDict = {
"hpp": {
"reset": [Bool, "reset"],
"create_viewer": [Empty, "_createViewer"],
"configure": [Empty, "configure"],
"load_environment": [String, "loadEnvironment"],
},
}
def __init__(self):
super(HppServerInitializer, self).__init__()
self.subscribers = self._createTopics("", self.subscribersDict, True)
self.setHppUrl()
self.robot = None
self.ps = None
self.tfListener = TransformListener()
self.world_frame = "/world"
def reset(self, msg):
self.initialize()
self.loadRobot()
if msg.data:
self.loadEnvironment(param="/environment/description",
paramName="/environment/name")
self.configure()
try:
self.createViewer()
except:
rospy.loginfo("Could not reach the Gepetto-viewer")
def initialize(self):
self.setHppUrl()
hpp = self._hpp()
# Reset the problem if necessary
if self.robot is not None:
hpp.problem.resetProblem()
self.robot = None
self.ps = None
self.vf = None
def loadRobot(self):
hppClient = self._hpp()
# Load the robot
robotName = rospy.get_param("/robot_name", "robot")
rootJointType = rospy.get_param("/robot_root_joint_type", "anchor")
urdfString = rospy.get_param("/robot_description")
srdfString = rospy.get_param("/robot_description_semantic", "")
hppClient.robot.loadRobotModelFromString(robotName, rootJointType,
urdfString, srdfString)
self.robot = hpp.corbaserver.robot.Robot(robotName,
rootJointType,
client=hppClient,
load=False)
self.ps = hpp.corbaserver.ProblemSolver(self.robot)
self.vf = ViewerFactory(self.ps)
self.vf.loadRobot(urdfString)
# Set the root joint position in the world.
base = self.robot.getLinkNames("root_joint")[0]
p, q = self.tfListener.lookupTransform(self.world_frame, base,
rospy.Time(0))
self.robot.setJointPosition("root_joint", p + q)
def loadEnvironment(self,
param=None,
xmlString=None,
paramName=None,
name=None):
if name is None:
name = rospy.get_param(
paramName) if paramName is not None else "obstacle"
if param is not None:
self.vf.loadObstacleModel("", rospy.get_param(param), name)
elif xmlString is not None:
self.vf.loadObstacleModel("", xmlString, name)
def configure(self):
hpp = self._hpp()
# Setup the planner
setParameters(hpp, "", rospy.get_param("/hpp/parameters", dict()))
if rospy.has_param("/hpp/path_validation/method"):
ps.selectPathValidation(
rospy.get_param("/hpp/path_validation/method"),
rospy.get_param("/hpp/path_validation/tolerance", 0.05))
self.ps.clearPathOptimizers()
if rospy.has_param("/hpp/path_optimizers"):
pathOpts = rospy.get_param("/hpp/path_optimizers",
["RandomShortcut"])
if isinstance(pathOpts, list):
for n in pathOpts:
self.ps.addPathOptimizer(n)
else:
rospy.logerr(
"Parameter /hpp/path_optimizers shoud be a list of strings."
)
def _createViewer(self, msg):
self.createViewer()
def createViewer(self, *args, **kwargs):
host = rospy.get_param("/gepetto_viewer/host", "localhost")
try:
return self.vf.createViewer(host=host, *args, **kwargs)
except:
rospy.loginfo("Could not reach the Gepetto-viewer")
def createViewer(self, ViewerClass=Viewer, *args, **kwargs):
return Parent.createViewer(self, ViewerClass, *args, **kwargs)
from hpp.corbaserver.practicals.ur5 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.gepetto import ViewerFactory, PathPlayer
robot = Robot('ur5')
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
vf.loadObstacleModel("hpp_practicals", "ur_benchmark/obstacles", "obstacles")
vf.loadObstacleModel("hpp_practicals", "ur_benchmark/table", "table")
vf.loadObstacleModel("hpp_practicals", "ur_benchmark/wall", "wall")
v = vf.createViewer()
q1 = [0, -1.57, 1.57, 0, 0, 0]
q2 = [0.2, -1.57, -1.8, 0, 0.8, 0]
q3 = [1.57, -1.57, -1.8, 0, 0.8, 0]
v(q2)
v(q3)
ps.setInitialConfig(q2)
ps.addGoalConfig(q3)
from motion_planner import MotionPlanner
m = MotionPlanner(robot, ps)
pathId = m.solveBiRRT(maxIter=1000)
pp = PathPlayer(v)
def end():
gameWidget.end()
sys.argv = []
gui = ViewerClient()
gui.gui.createWindow("ia")
gui.gui.createWindow("player")
iaRobot = Buggy("buggy_ia")
ips = ProblemSolver(iaRobot)
ivf = ViewerFactory(ips)
ivf.loadObstacleModel('hpp_environments', "scene", "scene_ia")
iv = ivf.createViewer(viewerClient=gui)
iaRobot.client.problem.selectProblem("player")
playerRobot = Buggy("buggy_player")
pps = ProblemSolver(playerRobot)
pvf = ViewerFactory(pps)
pvf.loadObstacleModel('hpp_environments', "scene", "scene_player")
pvf(playerRobot.client.robot.getCurrentConfig())
pv = pvf.createViewer(viewerClient=gui)
gui.gui.addSceneToWindow("buggy_player", 1)
gui.gui.addSceneToWindow("scene_player", 1)
gui.gui.addSceneToWindow("buggy_ia", 0)
gui.gui.addSceneToWindow("scene_ia", 0)
# To launch the game
gameTimer = QtCore.QTimer()
def createViewer (self, ViewerClass = Viewer, **kwargs):
return Parent.createViewer (self, ViewerClass, **kwargs)
q_init [0:3] = [6.5,-4,0.4] #root position
q_init[3:7] = [0,0,0,1] #root rotation
#set initial velocity (along x,y,z axis) :
q_init[-6:-3]=[0,0,0]
#q_goal[0:3] = [6.5,-1,0.4] # straight line
q_goal [0:3] = [3,-4,0.4] # easy goal position
#q_goal[0:3]=[-4.5,-4.8,0.4]# harder goal position
#set goal velocity (along x,y,z axis) :
q_goal[-6:-3]=[0,0,0]
vf.loadObstacleModel ("iai_maps", "room", "room")
# with displayArrow parameter the viewer will display velocity and acceleration of the center of the robot with 3D arrow. The length scale with the amplitude with a length of 1 for the maximal amplitude
v = vf.createViewer(displayArrows = True)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.addPathOptimizer ("RandomShortcut")
#select kinodynamic methods :
ps.selectSteeringMethod("Kinodynamic")
ps.selectDistance("Kinodynamic")
# the Kinodynamic steering method require a planner that build directionnal roadmap (with oriented edges) as the trajectories cannot always be reversed.
ps.selectPathPlanner("BiRRTPlanner")
print (ps.solve ())
# display the computed roadmap. Note that the edges are all represented as straight line and may not show the real motion of the robot between the nodes :
v.displayRoadmap("rm")
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(6)
vMax = 0.5 # linear velocity bound for the root
aMax = 0.5 # linear acceleration bound for the root
fullBody.client.robot.setExtraConfigSpaceBounds(
[-vMax, vMax, -vMax, vMax, 0, 0, -aMax, aMax, -aMax, aMax, 0, 0])
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
ps.setParameter("Kinodynamic/velocityBound", vMax)
ps.setParameter("Kinodynamic/accelerationBound", aMax)
ps.setRandomSeed(random.SystemRandom().randint(0, 999999))
#load the viewer
try:
v = vf.createViewer(displayCoM=True)
except Exception:
print("No viewer started !")
class FakeViewer():
sceneName = ""
def __init__(self):
return
def __call__(self, q):
return
def addLandmark(self, a, b):
return
ps.lockJoint ('base_joint_xy', [0,0])
ps.lockJoint ('base_joint_rz', [1,0])
res = ps.applyConstraints (q_init)
if res [0]:
q_init = res [1]
else:
raise RuntimeError ('Failed to apply constraints on init config.')
res = ps.applyConstraints (q_goal)
if res [0]:
q_goal = res [1]
else:
raise RuntimeError ('Failed to apply constraints on goal config.')
robot.setCurrentConfig (q_init)
r = vf.createViewer (collisionURDF = True)
r (q_init)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathPlanner ("DiffusingPlanner")
ps.selectPathValidation("Progressive", 0.01)
#ps.selectPathValidation("Progressive", 0.05)
#ps.readRoadmap ('/home/florent/devel/fiad/data/roadmap.rdm')
# ps.solve ()
pp = PathPlayer (ps.client, r)
q_goal [rank] = -0.5
vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.selectPathValidation ("Dichotomy", 0)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
totalTime += t2 - t1
print (t2-t1)
n = len (ps.client.problem.nodes ())
totalNumberNodes += n
print ("Number nodes: " + str(n))
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float (N)))
r = vf.createViewer(); r (q_init)
pp = PathPlayer (robot.client, r)
def play():
gameWidget.update()
def end():
gameWidget.end()
sys.argv = []
gui = ViewerClient()
gui.gui.createWindow("ia")
gui.gui.createWindow("player")
iaRobot = Buggy("buggy_ia")
ips = ProblemSolver(iaRobot)
ivf = ViewerFactory(ips)
ivf.loadObstacleModel('hpp_environments', "scene", "scene_ia")
iv = ivf.createViewer(viewerClient = gui)
iaRobot.client.problem.selectProblem("player")
playerRobot = Buggy("buggy_player")
pps = ProblemSolver(playerRobot)
pvf = ViewerFactory(pps)
pvf.loadObstacleModel('hpp_environments', "scene", "scene_player")
pvf(playerRobot.client.robot.getCurrentConfig())
pv = pvf.createViewer(viewerClient = gui)
gui.gui.addSceneToWindow("buggy_player", 1)
gui.gui.addSceneToWindow("scene_player", 1)
gui.gui.addSceneToWindow("buggy_ia", 0)
gui.gui.addSceneToWindow("scene_ia", 0)
# To launch the game
gameTimer = QtCore.QTimer()
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
# load the module to analyse the environnement and compute the possible contact surfaces
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel("hpp_environments", "multicontact/ground",
"planning", vf)
#load the viewer
try:
v = vf.createViewer(displayArrows=True)
except Exception:
print "No viewer started !"
class FakeViewer():
sceneName = ""
def __init__(self):
return
def __call__(self, q):
return
def addLandmark(self, a, b):
return
class Platform():
# main_agent = None
agents = []
# problem solver
ps = None
# path player
pp = None
# view factory
vf = None
# viewer
r = None
env = None
# a dictionary to get the agent's index
index_dic = {}
#for tree searching
tree = None # the root of the three
current_node = None
# pp = PathPlayer (rbprmBuilder.client.basic,ls r)
def __init__(self, agents):
self.agents = agents
for i in range(len(agents)):
a = agents[i]
self.index_dic[agents[i].robot.name] = i
self.agents[i].registerPlatform(self, i)
print 'the agent ', agents[
i].robot.name, ' is now registered with the index ', self.getInidex(
agents[i].robot.name)
self.tree = self.getStartNode()
self.current_node = self.tree
def getStartNode(self):
init_configs = []
for a in self.agents:
init_configs.append(a.start_config)
return Node(init_configs)
def start(self):
# self.problem.selectProblem(0)
self.ps = ProblemSolver(self.agents[0].robot)
self.vf = ViewerFactory(self.ps)
if self.env != None:
self.vf.loadObstacleModel(self.env.packageName, self.env.urdfName,
self.env.name)
self.r = self.vf.createViewer()
for a in self.agents:
a.startDefaultSolver()
a.setBounds()
a.setEnvironment()
a.solve()
a.storePath()
# self.loadAgentView(a.index)
# self.r(a.start_config)
print 'the agent ', a.robot.name, ' now has a backup plan of length', a.getProposedPlanLength(
)
# self.pp = PathPlayer (self.agents[0], self.r)
def loadAgentView(self, index, default=False): #default position or not
self.ps = self.agents[index - 1].ps
self.vf = ViewerFactory(self.ps)
# self.vf.loadObstacleModel(self.env.packageName, self.env.urdfName, self.env.name)
self.r = self.vf.createViewer()
# print '---------------->', len(self.agents[index - 1].init_config)
if default:
self.r(self.agents[index - 1].current_config)
# self.r.computeObjectPosition()
def getInidex(self, robot_name):
return self.index_dic[robot_name]
def setEnvironment(self, env):
self.env = env
# self.ps.moveObstacle('airbase_link_0', [0,0, -3, 1,0,0,0])
# self.r = self.vf.createViewer()
# def startViewer(self):
# self.r = vf.createViewer()
def updateViewAtTime(self, t):
config = []
for a in self.agents:
config.append(a.getConfigOfProposedPlanAtTime(t))
self.r(config)
def playAllProposedPath(self):
print 'play proposed path'
max_time = 0
for a in self.agents:
l = a.getProposedPlanLength()
if l > max_time:
max_time = l
for t in range(max_time):
# print 'time is ', t
for i in range(len(self.agents)):
a = self.agents[i]
if a.getProposedPlanLength() > t:
# print 'agent ', a.index,
self.loadAgentView(i + 1)
# and then set the agent to its current configuration
self.r(a.getConfigOfProposedPlanAtTime(t))
# sleep(0.003)
def playAllPermittedPath(self):
max_time = 0
for a in self.agents:
l = a.getPermittedPlanLength()
if l > max_time:
max_time = l
for t in range(max_time):
# print 'time is ', t
for i in range(len(self.agents)):
a = self.agents[i]
if a.getPermittedPlanLength() > t:
# print 'agent ', a.index,
self.loadAgentView(i + 1)
# and then set the agent to its current configuration
self.r(a.getConfigOfPermittedPlanAtTime(t))
# sleep(0.003)
def validateAllPaths(self, agents_remained):
print '******* start validation **********'
# print agents_remained
max_time = 0
for i in agents_remained:
a = self.agents[i]
a.startDefaultSolver()
a.setBounds()
a.setEnvironment()
a.loadOtherAgents()
l = a.getProposedPlanLength()
if l > max_time:
max_time = l
for t in range(max_time):
print '\n this is time ', t
for i in agents_remained:
a = self.agents[i]
a.startDefaultSolver()
a.setBounds()
a.setEnvironment()
a.loadOtherAgents()
# print 'this is robot ', a.robot.name
# a1.obstacle.getObstacleNames(False, 1000)
if a.getProposedPlanLength() > t:
# myconfig = a.getConfigOfProposedPlanAtTime(t)
# myspec = a.getMoveSpecification(myconfig)
# print 'the agent is at ', myspec[0], myspec[1]
# first of all, move all the obstacles
for oa in self.agents: # other agents
if a.index != oa.index:
# print '\t and moving the ghost of ', oa.robot.name
if not (oa.index in agents_remained
) or oa.getProposedPlanLength() <= t:
config = oa.end_config
else:
config = oa.getConfigOfProposedPlanAtTime(t)
spec = oa.getMoveSpecification(config)
a.obstacle.moveObstacle(
oa.robot.name + 'base_link_0', spec)
print '\tmove ghost', oa.robot.name, ' to ', spec[
0], spec[1]
# secondly, test if the configuration is valid
(result, _) = a.robot.isConfigValid(
a.getConfigOfProposedPlanAtTime(t))
if not result:
return t
# if everything is fine at each time slot, return -1
return -1
def construct_tree(self, iteration):
print '******************* this is iternation ', iteration, ' ***********************'
self.current_node.printInformation()
if iteration > 0:
#expand the tree by doing planning for each agent and find the collision momment
for i in self.current_node.getAgentsRemained():
a = self.agents[i]
print '>>>>>>>>>>>>this is agent', a.robot.name, ' computing '
if (a.computePlan(self.current_node) == -1):
print 'the agent is now using its backup plan/plan from last time'
# line = input()
# self.playAllProposedPath()
t = self.validateAllPaths(self.current_node.getAgentsRemained())
print 'in this iteration, the collision appears at time ', t
if t == -1: # the path is valid! we terminate the process
paths = []
indexes_and_paths = []
for i in self.current_node.agent_remained:
a = self.agents[i]
indexes_and_paths.append((a.index, a.obtainProposedPlan()))
child = self.current_node.expand(indexes_and_paths, [])
return (True, child.paths)
else:
reached = []
indexes_and_paths = []
for i in self.current_node.getAgentsRemained():
a = self.agents[i]
if a.getProposedPlanLength(
) > t - 1: # up to t, because t is the moment of collision!
path = a.obtainProposedPlan()[:t - 1]
indexes_and_paths.append((i, path))
else:
reached.append(
i
) # reached, therefore remove from the remaining list
indexes_and_paths.append((i, a.obtainProposedPlan()))
self.current_node = self.current_node.expand(
indexes_and_paths, reached)
return self.construct_tree(iteration - 1)
else: # can not find a path for each agent within limited iteration
return (False, None)
# remove those who has already got to where they suppose to be
# at least one step ----------- frangment