ps.pathLength(0)
ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (0)
ps.pathLength(1)
ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
pp(ps.numberPaths()-1)
r(ps.configAtParam(0,2))
ps.getWaypoints (0)
# Add light to scene
lightName = "li4"
r.client.gui.addLight (lightName, r.windowId, 0.01, [0.4,0.4,0.4,0.5])
r.client.gui.addToGroup (lightName, r.sceneName)
r.client.gui.applyConfiguration (lightName, [1,0,0,1,0,0,0])
r.client.gui.refresh ()
## Video recording
pp.dt = 0.02
r.startCapture ("capture","png")
#pp(1)
ps.pathLength(nInitialPath)
#ps.addPathOptimizer('RandomShortcut') #9
#ps.optimizePath (nInitialPath)
#ps.pathLength(ps.numberPaths()-1)
#ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
ps.optimizePath (nInitialPath)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
pp(ps.numberPaths()-1)
ps.configAtParam(2,0.5)
r(ps.configAtParam(0,2))
ps.getWaypoints (0)
ps.getWaypoints (ps.numberPaths()-1)
# plot paths
import numpy as np
dt = 0.1
nPath = ps.numberPaths()-1
lineNamePrefix = "optimized_path"
for t in np.arange(0., cl.problem.pathLength(nPath), dt):
lineName = lineNamePrefix+str(t)
r.client.gui.addLine(lineName,[cl.problem.configAtParam(nPath, t)[0],cl.problem.configAtParam(nPath, t)[1],0],[cl.problem.configAtParam(nPath, t+dt)[0],cl.problem.configAtParam(nPath, t+dt)[1],0],[0,1,0.3,1])
r.client.gui.addToGroup (lineName, r.sceneName)
nPath = nInitialPath
r(q2)
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
# verif orientation:
r(ps.getWaypoints (0)[0]) # ref
r(ps.getWaypoints (1)[0]) # should be good...
index = cl.robot.getConfigSize () - 4
q = q1[::]
plotStraightLine ([q[index], q[index+1], q[index+2]], q, r, "normale")
plotCone (q1, cl, r, 0.5, 0.2, "cone1")
plotCone (q2, cl, r, 0.5, 0.2, "cone2")
r( ps.configAtParam(0,2) )
ps.pathLength(0)
ps.getWaypoints (0)
ps.getWaypoints (1)
plotPath (cl, 0, r, "pathy", 0.1) # time-step should depend on sub-path length ?
plotFrame (r, "framy", [0,0,0], 0.5)
plotThetaPlane (q1, q2, r, "ThetaPlane")
## DEBUG commands
cl.obstacle.getObstaclePosition('decor_base')
robot.getJointOuterObjects('base_joint_xyz')
robot.isConfigValid(q1)
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
ps.solve ()
# PROBLEM !! not finding solution for environment_3d_window with mu=0.5 V0max=6.5 Projectionshooter .... V0 or Vimp too much limiting ?? 'cause V0max=7 gives a too "easy" solution ...
samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.8,0.2,1])
plotConeWaypoints (cl, 0, r, "wp", "friction_cone")
#ps.saveRoadmap ('/local/mcampana/devel/hpp/data/PARAB_envir3d_with_window.rdm')
r.client.gui.setVisibility('robot/l_bounding_sphere',"OFF")
samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.8,0.2,1])
r(ps.configAtParam(0,0.001))
ps.pathLength(0)
ps.getWaypoints (0)
## 3D Plot tools ##
q0 = [0, 0, 5, 0, 0, 0, 1, 0, 0, 1, 0];
r(q0)
plotFrame (r, "_", [0,0,4], 0.5)
plotPath (cl, 0, r, "pathy", 0.1)
plotThetaPlane (q1, q2, r, "ThetaPlane2")
plt = addPathPlot (cl, x3Path, 'y', 1, plt)
plt = addPathPlot (cl, x4Path, 'c', 1, plt)
plt = addPathPlot (cl, x5Path, '0.75', 1, plt)
plt = addNodePlot (collConstrNodes, 'bo', 'qConstr', plt)
plt = addNodePlot (collNodes, 'ro', 'qCol', plt)
plt = addNodePlot ([ps.getWaypoints (4)[1], ps.getWaypoints (4)[2]], 'go', 'qCol', plt)
plt.show() # will reset plt
#####################################################################
## DEBUG commands
robot.isConfigValid(q1)
cl.robot.distancesToCollision()
from numpy import *
argmin(cl.robot.distancesToCollision()[0])
r( ps.configAtParam(0,5) )
ps.optimizePath (0)
ps.clearRoadmap ()
ps.resetGoalConfigs ()
r.client.gui.removeFromGroup (elementName, r.sceneName)
# Compute manually path length with waypoints
import math
wps = ps.getWaypoints(4)
dist = 0
for i in range(0,len(wps)-1):
wpi = wps [i]
wpii = wps [i+1]
dist += math.sqrt( (wpii [0] - wpi [0])**2 + (wpii [1] - wpi [1])**2 )
print dist
class Agent(Client):
robot = None
platform = None
index = 0
ps = None
# we load other agents as ghosts to reduce the computation time while planning
ghosts = []
ghost_urdf = ''
ghost_package = ''
# to avoid confusion, we use start and end instead of init and goal
start_config = []
end_config = []
current_config = [] # this is not used for now
permitted_plan = [] # this is the plan generated
repeat = 0 # to help the selectProblem function
# an agent should have a robot and the start and end configuration
# to avoid confusion, we use start_config instead of init_config and
# end_confi instead of goal_config
def __init__(self, robot, start, end):
Client.__init__(self)
self.repeat = 0
# print 'creating an agent of type ', robotType
self.robot = robot
self.start_config = start
self.end_config = end
self.current_config = self.start_config
self.__plan_proposed = []
# once all agents are generated, we may register the agents to a platform
def registerPlatform(self, platform, index):
self.platform = platform
self.index = index
# this function gives some information about the agent and robot it is managing
def printInformation(self):
print '-------------------------------------------'
print 'name of the robot:\t', self.robot.name
print 'configuration size:\t', self.robot.getConfigSize()
print 'degree of freedom:\t', self.robot.getNumberDof()
print 'mass of the robot:\t', self.robot.getMass()
print 'the center of mass:\t', self.robot.getCenterOfMass()
config = self.robot.getCurrentConfig()
nm = self.robot.getJointNames()
print 'there are ', len(nm), 'joint names in total. They are:'
for i in range(len(nm)):
lower = self.robot.getJointBounds(nm[i])[0]
upper = self.robot.getJointBounds(nm[i])[1]
print 'joint name: ', nm[
i], '\trank in configuration:', self.robot.rankInConfiguration[
nm[i]],
print '\tlower bound: {0:.3f}'.format(
lower), '\tupper bound: {0:.3f}'.format(upper)
# set up the environment
def setEnvironment(self):
if self.platform.env != None:
self.ps.loadObstacleFromUrdf(self.platform.env.packageName,
self.platform.env.urdfName,
self.platform.env.name)
# self.ps.moveObstacle('airbase_link_0', [0,0, -3, 1,0,0,0])
# load the other agents to the problem solver
def loadOtherAgents(self):
# print 'There are ', len(self.platform.agents), 'agents'
#load ghost agents
for a in self.platform.agents:
if (a.index != self.index):
# if it is not itself then load a ghost agent
g = Ghost()
self.ps.loadObstacleFromUrdf(
g.packageName, g.urdfName,
a.robot.name) # it's the robot's name!!!
# and then place it at the initial location of the agent
# print self.robot.name, ' is now loading ', a.robot.name, ' as a ghost'
config = a.current_config
spec = self.getMoveSpecification(config)
spec[2] = 0.3
self.obstacle.moveObstacle(a.robot.name + 'base_link_0', spec)
# load agents from the node
def loadOtherAgentsFromNode(self, node):
print 'There are ', len(self.platform.agents), 'agents'
#load ghost agents
for a in self.platform.agents:
if (a.index != self.index):
# if it is not itself then load a ghost agent
g = Ghost()
self.ps.loadObstacleFromUrdf(
g.packageName, g.urdfName,
a.robot.name) # it's the robot's name!!!
# and then place it at the initial location of the agent
config = node.getAgentCurrentConfig(a.index)
spec = self.getMoveSpecification(config)
self.obstacle.moveObstacle(a.robot.name + 'base_link_0', spec)
print self.robot.name, ' is now loading ', a.robot.name, ' as a ghost', 'it is at ', spec[
0], spec[1]
# note that the default solver does not consider the position of other agents
def startDefaultSolver(self):
self.repeat += 1
name = self.robot.name
self.problem.selectProblem(str(self.index) + ' ' + str(self.repeat))
self.robot = HyQ(name)
self.ps = ProblemSolver(self.robot)
self.ps.setInitialConfig(self.start_config)
self.ps.addGoalConfig(self.end_config)
self.ps.selectPathPlanner("VisibilityPrmPlanner")
self.ps.addPathOptimizer("RandomShortcut")
# initialise a solver from a node, the node contains information about other agents and itself
# this method is used when proposing plans while interacting with platform for MAS path planning
def startNodeSolver(self, node):
self.repeat += 1
name = self.robot.name
self.problem.selectProblem(str(self.index) + ' ' + str(self.repeat))
self.robot = HyQ(name)
self.ps = ProblemSolver(self.robot)
cfg = node.getAgentCurrentConfig(self.index)
print 'this iteration, the agent', name, 'starts from ', cfg[0], cfg[1]
self.ps.setInitialConfig(cfg)
self.ps.addGoalConfig(self.end_config)
self.ps.selectPathPlanner("VisibilityPrmPlanner")
self.ps.addPathOptimizer("RandomShortcut")
# this is only used when the agent takes too long (30 seconds) while planning
def terminate_solving(self):
self.problem.interruptPathPlanning()
# the solve method for problem solver but with a time bound
def solve(self):
# try catch -------------------
try:
t = Timer(30.0, self.terminate_solving)
t.start()
print 'solved: ', self.ps.solve()
t.cancel()
except Error as e:
print e.msg
print '***************\nfailed to plan within limited time\n**************'
return -1
# self.repeat += 1
# store the path for two reasons:
# 1. store as a default plan
# 2. to continue the path
def storePath(self, choice=0, segments=8):
# always store the first one for now
self.__plan_proposed = []
for p in range(int(round(segments * self.ps.pathLength(choice)))):
self.__plan_proposed.append(
self.ps.configAtParam(choice, p * 1.0 / segments))
# the last configuration is the goal configuration
if self.ps.configAtParam(
choice, self.ps.pathLength(choice)) == self.end_config:
self.__plan_proposed.append(self.end_config)
print 'stored; plan length: ', len(self.__plan_proposed)
# this is hard colded for now for the airplane example, we should introduce an entry for the environment
# class about it.
def setBounds(self):
self.robot.setJointBounds("base_joint_xy", [-35, 10, -2.6, 4.3])
#the rest are just some helping functions
def getConfigOfProposedPlanAtTime(self, index):
return self.__plan_proposed[index]
def getConfigOfPermittedPlanAtTime(self, index):
return self.permitted_plan[index]
def getProposedPlanLength(self):
return len(self.__plan_proposed)
def setPermittedPlan(self, plan):
self.permitted_plan = plan
def getPermittedPlanLength(self):
return len(self.permitted_plan)
# export the permitted plan to a specific file in the format
# agent X
# config 1
# config 2
# etc
def exportPermittedPlan(self, filename):
f = open(filename, 'a+')
f.write('agent ' + str(self.index) + '\n')
for p in self.permitted_plan:
f.write(str(p)[1:-1] + '\n')
f.close()
# for the sake of manipulation, we return a copy of it
def obtainPermittedPlan(self):
return copy.copy(self.permitted_plan)
# we will get only a copy of it, not the original one
# to remind the difference, we use 'obtain' instead of 'get'
def obtainProposedPlan(self):
return copy.copy(
self.__plan_proposed
) #for some reason, sometimes the value would maybe changed???
# to transfer the specification from 2D to 3D
def getMoveSpecification(self, config):
x = config[0]
y = config[1]
th = atan2(config[3], config[2])
# print 'sin = ', self.init_config[3], ' cos = ', self.init_config[2], ' th = ', th
return [x, y, 0, cos(th / 2), 0, 0, sin(th / 2)]
# the function to compute a plan, exceptions are not handled in this simple demo
def computePlan(self, node):
self.startNodeSolver(node)
self.setBounds()
self.setEnvironment()
self.loadOtherAgentsFromNode(node)
if self.solve() != -1:
self.storePath()
else:
self.__plan_proposed = self.__plan_proposed[node.progress_time::]
[node.getAgentCurrentConfig(self.index)]
print 'take the previous one and continue the searching'
return -1
robot.setJointBounds('base_joint_xy', [-kRange, kRange, -kRange, kRange])
ps = ProblemSolver (robot)
cl = robot.client
# q = [x, y, theta] # (z not considered since planar)
q1 = [-2, 0, 1, 0]; q2 = [2, 0, 1, 0]
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
cl.obstacle.loadObstacleModel('potential_description','cylinder_obstacle','')
ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [1,0,0,0], [0,0,1]) # OK
ps.setNumericalConstraints ("constraints", ["orConstraint"])
ps.solve ()
ps.configAtParam(0,2)
ps.configAtParam(0,6)
ps.configAtParam(0,13)
ps.getWaypoints (0)[1]
ps.optimizePath(0)
ps.pathLength(0)
ps.pathLength(1)
ps.configAtParam(1,2)
ps.configAtParam(1,6)
ps.configAtParam(1,13)
ps.getWaypoints (1)[1]
len(ps.getWaypoints (0))
cl.problem.getIterationNumber()
pp (0)
pp (1)
## Video recording
r.startCapture ("capture","png")
pp(1)
r.stopCapture ()
r.startCapture ("capture","png")
pp(2)
r.stopCapture ()
#ffmpeg -r 50 -i capture_0_%d.png -r 25 -vcodec libx264 video.mp4
robot.getJointNames ()
robot.getJointPosition ("torso_lift_joint")
r(ps.configAtParam(0,5))
ps.configAtParam(0,2)[0]
ps.configAtParam(0,6)[0]
ps.configAtParam(0,13)[0]
ps.getWaypoints (0)[1][0]
ps.configAtParam(0,2)[1]
ps.configAtParam(0,6)[1]
ps.configAtParam(0,13)[1]
ps.getWaypoints (0)[1][1]
ps.configAtParam(1,2)[0]
ps.configAtParam(1,6)[0]
ps.configAtParam(1,13)[0]
ps.getWaypoints (1)[1][0]
# with sub-paths
q1 = [-1.3, 0.2, 0, 1]; q2 = [0.2, 0.2, 0, 1]; q3 = [5, -2.8, 0, 1]; q4 = [9, 2.2, 0, 1];
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve () # pp(0)
ps.resetGoalConfigs ()
ps.setInitialConfig (q2); ps.addGoalConfig (q3); ps.solve () # pp(1)
ps.resetGoalConfigs ()
ps.setInitialConfig (q3); ps.addGoalConfig (q4); ps.solve () # pp(2)
ps.resetGoalConfigs ()
ps.setInitialConfig (q4); ps.addGoalConfig (q5); ps.solve () # pp(3)
ps.resetGoalConfigs ()
ps.setInitialConfig (q1); ps.addGoalConfig (q5); ps.solve () # pp(4)
ps.resetGoalConfigs ()
ps.pathLength(0)
cl.problem.getWaypoints (0)
ps.configAtParam(0,0.2)
ps.pathLength(4)
cl.problem.getWaypoints (4)
## Plot whole path in viewer ##
import numpy as np
nPath = 0
dt = 0.1
for t in np.arange(0., cl.problem.pathLength(nPath), dt):
lineName = "g"+str(t)
r.client.gui.addLine(lineName,[cl.problem.configAtParam(nPath, t)[0],cl.problem.configAtParam(nPath, t)[1],0.2],[cl.problem.configAtParam(nPath, t+dt)[0],cl.problem.configAtParam(nPath, t+dt)[1],0.2],[0,0.3,1,1])
r.client.gui.addToGroup (lineName, r.sceneName)
