## 3D viewer tools ##
#gui.removeFromGroup("path0",r.sceneName)
#gui.getNodeList()
#gui.createGroup ('sphere_wp0_group')
#gui.getGroupNodeList ('sphere_wp0_group')
## IMPORT SCENE AND CONFIGS TO BLENDER ##
# I skip the "collada generation part" since I already have them from blender ...
blender/urdf_to_blender.py -p /local/mcampana/devel/hpp/videos/ -i /local/mcampana/devel/hpp/src/animals_description/urdf/sphere_mesh.urdf -o robot_blend.py # generate robot loadable by Blender
from viewer_display_library import pathToYamlFile, writeEdgeSamples, writePathSamples, writeSkipList
len(np.arange(0, ps.pathLength(pathId), 0.02))
pathToYamlFile (cl, r, "frames.yaml ", "robot/base_link", pathId, q2, 0.02)
ps.numberNodes()
# Plot cones and edges in viewer
plotConesRoadmap (cl, r, 'cone_rm_group', "friction_cone2")
plotEdgesRoadmap (cl, r, 'edgeGroup', 70, [0,1,0.2,1])
gui.writeNodeFile('cone_rm_group','cones_RM.dae')
## Write EDGES in a file, which will be parsed by a Blender-Python script
writeEdgeSamples (cl, 'edges.txt', 70)
## Write PATH samples in a file, which will be parsed by a Blender-Python script
nPointsPlot = 50 offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2 plotFrame (r, "_", [0,0,1.5], 0.5) # First parabolas: vmax = 8m/s, mu = 1.2 cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(4.5) plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2") ps.clearRoadmap(); solveTime = ps.solve () # 0.312 s pahtId = ps.numberPaths()-offsetOrientedPath # path without orientation stuff samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path0", [0,0,1,1]) plotConeWaypoints (cl, pahtId, r, "wp0", "friction_cone2") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp0", [0,0,1,1], 0.02) print "solve duration: " + str(solveTime) print "path length: " + str(ps.pathLength(pahtId)) print "number of waypoints: " + str(len(ps.getWaypoints (pahtId))) print "number of nodes: " + str(ps.numberNodes ()) cl.problem.getResultValues() # Second parabolas: vmax = 6.5m/s, mu = 0.5 plotCone (q1, cl, r, "cone1", "friction_cone"); plotCone (q2, cl, r, "cone12", "friction_cone") cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(4.5) ps.clearRoadmap(); solveTime = ps.solve () # 0.9689 s pahtId = ps.numberPaths()-offsetOrientedPath samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path2", [1,0,0,1]) plotConeWaypoints (cl, pahtId, r, "wp2", "friction_cone") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp2", [1,0,0,1], 0.02) print "solve duration: " + str(solveTime) print "path length: " + str(ps.pathLength(pahtId))
r(Q[0])
for i in range(0, len(Q)):
r(Q[i])
time.sleep (0.5)
#robot.isConfigValid(Q[0])
for i in range(0, len(Q)-1):
ps.setInitialConfig (Q[i]); ps.addGoalConfig (Q[i+1]); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (Q[0]); ps.addGoalConfig (Q[len(Q)-1]); ps.solve ();
nInitialPath = ps.numberPaths()-1 #8
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)
plotSphere (q2, cl, r, "sphere_q2", [0,1,0,1], 0.02) # same as robot nPointsPlot = 50 offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2 plotFrame (r, "_", [0,0,2.8], 0.5) # First parabola(s): vmax = 6.8m/s, mu = 1.2 cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(6.8) plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2") ps.clearRoadmap(); solveTime = ps.solve () # 0.085 s pahtId = ps.numberPaths()-offsetOrientedPath # path without orientation stuff samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path0", [0,0,1,1]) plotConeWaypoints (cl, pahtId, r, "wp0", "friction_cone2") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp0", [0,0,1,1], 0.02) print "solve duration: " + str(solveTime) print "path length: " + str(ps.pathLength(pahtId)) print "number of waypoints: " + str(len(ps.getWaypoints (pahtId))) print "number of nodes: " + str(ps.numberNodes ()) cl.problem.getResultValues () # Second parabola(s): vmax = 5.3m/s, mu = 0.5 plotCone (q1, cl, r, "cone1", "friction_cone"); plotCone (q2, cl, r, "cone12", "friction_cone") cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(5.3) ps.clearRoadmap(); solveTime = ps.solve () # 0.738 s pahtId = ps.numberPaths()-offsetOrientedPath samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path2", [0.2,0.8,0.2,1]) plotConeWaypoints (cl, pahtId, r, "wp2", "friction_cone") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp2", [0.2,0.8,0.2,1], 0.02) print "solve duration: " + str(solveTime)
q2 = [6.55, -2.91, 1.605, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]
q2hard = [7.60, -2.41, 0.545, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.8, 0.0, -0.4, -0.55, 0.0, -0.6, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -2.8, 0.0, 0.1, -0.2, -0.1, 0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.1, 1.2, -0.4, 0.2, -0.3, 0.0, -0.4, 0.2, 0.7, 0.0]
robot.isConfigValid(q1)
robot.isConfigValid(q2)
# qf should be invalid
qf = [1, -3, 3, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]
robot.isConfigValid(qf)
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
ps.solve ()
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))
q2 = [4, -4, 1, 0] # obstS 1
#q1 = [-4, 4, 0]; q2 = [4, -4, 0] # obstS 1
ps.setInitialConfig(q1)
ps.addGoalConfig(q2)
cl.obstacle.loadObstacleModel('potential_description', 'obstacles_concaves',
'obstacles_concaves')
#ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [1,0,0,0], [0,0,1])
#ps.setNumericalConstraints ("constraints", ["orConstraint"])
ps.selectPathPlanner("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
ps.solve()
ps.pathLength(0)
ps.addPathOptimizer("GradientBased")
ps.optimizePath(0)
ps.numberPaths()
ps.pathLength(ps.numberPaths() - 1)
import matplotlib.pyplot as plt
from mutable_trajectory_plot import planarPlot, addNodePlot
from parseLog import parseCollConstrPoints
num_log = 31891
contactPoints = parseCollConstrPoints(num_log, '77: contact point = (')
plt = planarPlot(cl, 0, ps.numberPaths() - 1, plt, 1.5, 5)
plt = addNodePlot(contactPoints, 'ko', '', 5.5, plt)
plt.show()
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")
plotCone (q1, cl, r, 0.5, 0.4, "c1")
"""
q1 = [-2, 0]; q2 = [-1, 1]; q3 = [-1.2, 1.8]; q4 = [-0.2, 1.2]; q5 = [0.5, 1.9]
q6 = [2, 1.5]; q7 = [1, 0.5]; q8 = [2, 0]
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q2); ps.addGoalConfig (q3); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q3); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q4); ps.addGoalConfig (q5); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q5); ps.addGoalConfig (q6); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q6); ps.addGoalConfig (q7); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q7); ps.addGoalConfig (q8); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q1); ps.addGoalConfig (q8); ps.solve (); # 7
nInitialPath = ps.numberPaths()-1 #8
ps.pathLength(nInitialPath)
ps.addPathOptimizer("GradientBased")
#ps.addPathOptimizer("Prune")
#ps.addPathOptimizer("PartialRandomShortcut")
ps.optimizePath(nInitialPath)
ps.pathLength(ps.numberPaths()-1)
ps.getWaypoints (nInitialPath)
ps.getWaypoints (ps.numberPaths()-1)
"""
from hpp.gepetto import Viewer, PathPlayer
#vmax = 6.5; mu = 0.5
#cl.problem.setFrictionCoef(mu); cl.problem.setMaxVelocityLim(vmax)
toSeconds = np.array ([60*60,60,1,1e-3])
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
imax=3;
f = open('results.txt','a')
# Assuming that seed in modified directly in HPP (e.g. in core::PathPlanner::solve or ProblemSolver constr)
for i in range(0, imax):
print i
ps.clearRoadmap ()
solveTimeVector = ps.solve ()
solveTime = np.array (solveTimeVector).dot (toSeconds)
pathId = ps.numberPaths()-offsetOrientedPath
pathLength = ps.pathLength (pathId)
pathNumberWaypoints = len(ps.getWaypoints (pathId))
roadmapNumberNodes = ps.numberNodes ()
#TODO: number collisions (checked ???)
#TODO: number parabola that has failed (because of which constraint ??)
#ps.addPathOptimizer("Prune")
#ps.optimizePath (pathId)
#prunePathId = ps.numberPaths()-1
# Write important results #
f.write('Try number: '+str(i)+'\n')
f.write('with parameters: vmax='+str(vmax)+' and mu='+str(mu)+'\n')
f.write('solve duration: '+str(solveTime)+'\n')
f.write('path length: '+str(pathLength)+'\n')
f.write('number of waypoints: '+str(pathNumberWaypoints)+'\n')
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
#vmax = 6.5; mu = 0.5
#cl.problem.setFrictionCoef(mu); cl.problem.setMaxVelocityLim(vmax)
toSeconds = np.array([60 * 60, 60, 1, 1e-3])
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
imax = 3
f = open('results.txt', 'a')
# Assuming that seed in modified directly in HPP (e.g. in core::PathPlanner::solve or ProblemSolver constr)
for i in range(0, imax):
print i
ps.clearRoadmap()
solveTimeVector = ps.solve()
solveTime = np.array(solveTimeVector).dot(toSeconds)
pathId = ps.numberPaths() - offsetOrientedPath
pathLength = ps.pathLength(pathId)
pathNumberWaypoints = len(ps.getWaypoints(pathId))
roadmapNumberNodes = ps.numberNodes()
#TODO: number collisions (checked ???)
#TODO: number parabola that has failed (because of which constraint ??)
#ps.addPathOptimizer("Prune")
#ps.optimizePath (pathId)
#prunePathId = ps.numberPaths()-1
# Write important results #
f.write('Try number: ' + str(i) + '\n')
f.write('with parameters: vmax=' + str(vmax) + ' and mu=' + str(mu) + '\n')
f.write('solve duration: ' + str(solveTime) + '\n')
f.write('path length: ' + str(pathLength) + '\n')
f.write('number of waypoints: ' + str(pathNumberWaypoints) + '\n')
ps.solve (); ps.resetGoalConfigs () q9 = [0, 0, 0, 1, 0, 0, 0] ps.setInitialConfig (q8); ps.addGoalConfig (q9) ps.solve (); ps.resetGoalConfigs () """ # Only path to do a useless turn ! (Not the case if solved separately) # r( ps.configAtParam(7,2.17) ) = [0.0, 0.0, 0.0, -0.9999559023922103, 0.0, 0.0, -0.00939112724262876] ps.setInitialConfig (q1); ps.addGoalConfig (q5) # do a turn as expected ps.solve () ps.optimizePath (4) # reduced to a point as expected len(ps.getWaypoints (0)) ps.pathLength(5) # = 0 ##############################" # TEST quaternions x0 HRP2 + optimisation = meme resultat (-1) (un ou 2 tours)? # Oui mais il faut imposer alpha = alpha_init et ne pas MàJ H1_ # les valeurs propres de la Hessienne sont bien >0 q1 = [0, 0, 0, 0.9999028455952614, -0.00643910090823595, -0.012362740316661774, 1.3620998722148461e-06] q2 = [0, 0, 0, 0.8258496711518952, -0.2042733013060623, -0.19724860126354768, -0.4871732015735299] ps.setInitialConfig (q1); ps.addGoalConfig (q2) ps.solve (); ps.resetGoalConfigs () q3 = [0, 0, 0, 0.6659085913630002, -0.04107471639409278, 0.06948325706470262, -0.7416540248726288] ps.setInitialConfig (q2); ps.addGoalConfig (q3) ps.solve (); ps.resetGoalConfigs ()
cl.obstacle.loadObstacleModel('robot_2d_description','cylinder_obstacle','')
# q = [x, y] # limits in URDF file
q1 = [-2, 0]; q2 = [-0.2, 2]; q3 = [0.2, 2]; q4 = [2, 0]
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q2); ps.addGoalConfig (q3); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q3); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q1); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs ()
# pp(3) = p0 final
#ps.addPathOptimizer("GradientBased")
#ps.addPathOptimizer("Prune")
ps.addPathOptimizer("PartialRandomShortcut")
ps.optimizePath(3) # pp(4) = p1 final
ps.pathLength(3)
ps.pathLength(4)
ps.getWaypoints (3)
ps.getWaypoints (4)
# should be [-0.07 0] [0.07 0] if alpha_init=1
"""
q1 = [-2, 0]; q2 = [-1, 1]
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
ps.resetGoalConfigs ()
q1 = [-1, 1]; q2 = [-1.2, 1.8]
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
ps.resetGoalConfigs ()
q1 = [-1.2, 1.8]; q2 = [-0.2, 1.2]
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
r.loadObstacleModel ("puzzle_description","decor_very_easy","decor_very_easy")
r(q1)
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
# Load box obstacle in HPP for collision avoidance
cl.obstacle.loadObstacleModel('puzzle_description','decor_very_easy','')
#cl.obstacle.loadObstacleModel('puzzle_description','decor_easy','')
#cl.obstacle.loadObstacleModel('puzzle_description','decor','')
ps.selectPathPlanner ("VisibilityPrmPlanner") # 26min solve time
begin=time.time()
ps.solve ()
end=time.time()
print "Solving time: "+str(end-begin)
ps.pathLength(0)
ps.addPathOptimizer("GradientBased")
begin=time.time()
ps.optimizePath (0)
end=time.time()
print "Optim time: "+str(end-begin)
cl.problem.getIterationNumber ()
ps.pathLength(1)
#vPRM: 24 iter, 25.3s->21s, weighted Cost + comp linear error
#RRTc: 30 iter, 33.4s->14.9s, weighted Cost + comp linear error
begin=time.time()
ps.optimizePath (1)
end=time.time()
print "Optim time: "+str(end-begin)
#ps.selectPathPlanner ("VisibilityPrmPlanner")
begin=time.time()
ps.solve ()
end=time.time()
print "Solving time: "+str(end-begin)
ps.addPathOptimizer("GradientBased")
begin=time.time()
ps.optimizePath(0)
end=time.time()
print "Optim time: "+str(end-begin)
cl = robot.client
cl.problem.getIterationNumber ()
cl.problem.getComputationTime ()
ps.pathLength(0)
ps.pathLength(1)
begin=time.time()
ps.optimizePath(1)
end=time.time()
print "Optim2 time: "+str(end-begin)
cl.problem.getIterationNumber ()
ps.pathLength(0)
ps.pathLength(1)
ps.pathLength(2)
len(ps.getWaypoints (0))
ps.optimizePath(3)
ps.setInitialConfig(q7)
ps.addGoalConfig(q8)
ps.solve()
ps.resetGoalConfigs()
ps.setInitialConfig(q8)
ps.addGoalConfig(q9)
ps.solve()
ps.resetGoalConfigs()
ps.setInitialConfig(q9)
ps.addGoalConfig(q10)
ps.solve()
ps.resetGoalConfigs()
ps.setInitialConfig(q1)
ps.addGoalConfig(q10)
ps.solve()
print ps.pathLength(9)
ps.addPathOptimizer("GradientBased")
ps.optimizePath(9)
ps.numberPaths()
print ps.pathLength(ps.numberPaths() - 1)
"""
# pp(9) = p0 final
ps.optimizePath(9) # pp(10) = p1 final
ps.pathLength(9)
ps.pathLength(10)
ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (9)
