## 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()
Beispiel #7
0
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")
Beispiel #8
0
"""

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')
Beispiel #10
0
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 ()
Beispiel #13
0
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)
Beispiel #16
0
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)