Esempi in Python per parseNodes

Esempio n. 1

File: puzzle_test.py Progetto: mylene-campana/puzzle_description

robot.getJointOuterObjects('j_object_one')
robot.isConfigValid(q1)
robot.distancesToCollision()
r( ps.configAtDistance(0,5) )
ps.optimizePath (0)
ps.clearRoadmap ()
ps.resetGoalConfigs ()
from numpy import *
argmin(robot.distancesToCollision()[0])


## Debug Optimization Tools ##############
num_log = 26123
from parseLog import parseCollConstrPoints, parseNodes

collConstrNodes = parseNodes (num_log, '189: qFree_ = ')
collNodes = parseNodes (num_log, '182: qColl = ')

contactPoints = parseCollConstrPoints (num_log, '77: contact point = (')
x1_J1 = parseCollConstrPoints (num_log, '96: x1 in R0 = (')
x2_J1 = parseCollConstrPoints (num_log, '97: x2 in R0 = (')
x1_J2 = parseCollConstrPoints (num_log, '116: x1 in J2 = (')
x2_J2 = parseCollConstrPoints (num_log, '117: x2 in J2 = (') #x2_J2 <=> contactPoints


## same with viewer !
from viewer_display_library_OPTIM import transformInConfig, plotPoints, plotPointsAndLines, plot2DBaseCurvPath, plotDofCurvPath, plotPointBodyCurvPath, plotBodyCurvPath
contactPointsViewer = transformInConfig (contactPoints)
x1_J1Viewer = transformInConfig (x1_J1)
x2_J1Viewer = transformInConfig (x2_J1)
x1_J2Viewer = transformInConfig (x1_J2)

Esempio n. 2

File: potential_test_GB_optim.py Progetto: mylene-campana/robot_2d_description

r.client.gui.addToGroup("optimCurvPathc", r.sceneName)

r.client.gui.removeFromGroup("optimCurvPath", r.sceneName)

#q1 = [2.4, -4.6, 1.0, 0.0]; q2 = [-0.4, 4.6, 1.0, 0.0] # obstS 2
#cl.obstacle.loadObstacleModel('potential_description','cylinder_obstacle','')

## Debug Optimization Tools ##############

import matplotlib.pyplot as plt
num_log = 12903
from parseLog import parseNodes, parsePathVector
from mutable_trajectory_plot import planarPlot, addNodePlot, addPathPlot

collConstrNodes = parseNodes(
    num_log,
    'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:318: qCollConstr = '
)
collNodes = parseNodes(
    num_log,
    'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:308: qColl = '
)

x1initLine = 'INFO:/local/mcampana/devel/hpp/src/hpp-core/include/hpp/core/path-optimization/gradient-based.hh:86: x0+alpha*p -> x1='
x1finishLine = 'INFO:/local/mcampana/devel/hpp/src/hpp-core/include/hpp/core/path-optimization/gradient-based.hh:89: finish path parsing'
skipLines = 1  # skip useless lines while parsing path, usually skip rotation parts
x0Path = parsePathVector(num_log, x1initLine, x1finishLine, 1, skipLines)
x1Path = parsePathVector(num_log, x1initLine, x1finishLine, 2, skipLines)
x2Path = parsePathVector(num_log, x1initLine, x1finishLine, 3, skipLines)
x3Path = parsePathVector(num_log, x1initLine, x1finishLine, 4, skipLines)
x4Path = parsePathVector(num_log, x1initLine, x1finishLine, 5, skipLines)
x5Path = parsePathVector(num_log, x1initLine, x1finishLine, 6, skipLines)

Esempio n. 3

File: robot_2d_test_spirale.py Progetto: mylene-campana/robot_2d_description

cl.problem.optimizePath(8) # pp(9) = p1 final
end=time.time()
print "Solving time: "+str(end-begin)

len(cl.problem.nodes ())
cl.problem.pathLength(0)
cl.problem.pathLength(1)

## Debug Optimization Tools ##############

import matplotlib.pyplot as plt
num_log = 15297
from parseLog import parseNodes, parsePathVector
from mutable_trajectory_plot import planarPlot, addNodePlot, addPathPlot

collConstrNodes = parseNodes (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:178: qCollConstr = ')
collNodes = parseNodes (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:172: qColl = ')

x0Path = parsePathVector (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:136: x0=','INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:138: finish path parsing',2,0)
x1Path = parsePathVector (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:141: x0+alpha*p -> x1=','INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:143: finish path parsing',2,0)
x2Path = parsePathVector (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:141: x0+alpha*p -> x1=','INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:143: finish path parsing',3,0)
x3Path = parsePathVector (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:141: x0+alpha*p -> x1=','INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:143: finish path parsing',4,0)
x4Path = parsePathVector (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:141: x0+alpha*p -> x1=','INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:143: finish path parsing',5,0)
x5Path = parsePathVector (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:141: x0+alpha*p -> x1=','INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:143: finish path parsing',6,0)

plt = planarPlot (cl, 8, 9, plt) # initialize 2D plot with obstacles and path
plt = addNodePlot (collConstrNodes, 'bo', 'qConstr', plt)
plt = addNodePlot (collNodes, 'ro', 'qCol', plt)
plt = addPathPlot (cl, x0Path, 'm', 1, plt)
plt = addPathPlot (cl, x1Path, 'g', 1, plt)
plt = addPathPlot (cl, x2Path, 'b', 1, plt)

Esempio n. 4

File: potential_test_GB_optim.py Progetto: mylene-campana/potential_description

## -------------------------------------
# Simple 2D trajectory plot (in Matplotlib)
import matplotlib.pyplot as plt
from mutable_trajectory_plot import planarPlot
#plt = planarPlot (cl, 0, ps.numberPaths()-1, plt) # if launched from potential_description
plt = planarPlot (cl, 0, ps.numberPaths()-1, plt, 1.2, 5) # if launched from robot_2d_description
plt.show()

## Debug Optimization Tools ##############

import matplotlib.pyplot as plt
num_log = 12903
from parseLog import parseNodes, parsePathVector
from mutable_trajectory_plot import planarPlot, addNodePlot, addPathPlot

collConstrNodes = parseNodes (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:318: qCollConstr = ')
collNodes = parseNodes (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:308: qColl = ')

x1initLine = 'INFO:/local/mcampana/devel/hpp/src/hpp-core/include/hpp/core/path-optimization/gradient-based.hh:86: x0+alpha*p -> x1='
x1finishLine = 'INFO:/local/mcampana/devel/hpp/src/hpp-core/include/hpp/core/path-optimization/gradient-based.hh:89: finish path parsing'
skipLines = 1 # skip useless lines while parsing path, usually skip rotation parts
x0Path = parsePathVector (num_log, x1initLine, x1finishLine, 1, skipLines)
x1Path = parsePathVector (num_log, x1initLine, x1finishLine, 2, skipLines)
x2Path = parsePathVector (num_log, x1initLine, x1finishLine, 3, skipLines)
x3Path = parsePathVector (num_log, x1initLine, x1finishLine, 4, skipLines)
x4Path = parsePathVector (num_log, x1initLine, x1finishLine, 5, skipLines)
x5Path = parsePathVector (num_log, x1initLine, x1finishLine, 6, skipLines)
x6Path = parsePathVector (num_log, x1initLine, x1finishLine, 7, skipLines)


plt = planarPlot (cl, 0, ps.numberPaths()-1, plt) # initialize 2D plot with obstacles and path

Esempio n. 5

File: potential_test.py Progetto: mylene-campana/test-hpp

cl.problem.optimizePath(0)
end=time.time()
print "Solving time: "+str(end-begin)

len(cl.problem.nodes ())
cl.problem.pathLength(0)
cl.problem.pathLength(1)

## Debug Optimization Tools ##############

import matplotlib.pyplot as plt
num_log = 32234
from parseLog import parseNodes, parsePathVector
from mutable_trajectory_plot import planarPlot, addNodePlot, addPathPlot

collConstrNodes = parseNodes (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:334: qCollConstr = ')
collNodes = parseNodes (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:328: qColl = ')

x0Path = parsePathVector (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:287: x0=','INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:289: finish path parsing',2,2)
x1Path = parsePathVector (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:292: x0+alpha*p -> x1=','INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:294: finish path parsing',2,2)
x2Path = parsePathVector (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:292: x0+alpha*p -> x1=','INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:294: finish path parsing',3,2)
x3Path = parsePathVector (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:292: x0+alpha*p -> x1=','INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:294: finish path parsing',4,2)
x4Path = parsePathVector (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:292: x0+alpha*p -> x1=','INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:294: finish path parsing',5,2)
x5Path = parsePathVector (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:292: x0+alpha*p -> x1=','INFO:/local/mcampana/devel/hpp/src/hpp-core/src/path-optimization/gradient-based.cc:294: finish path parsing',6,2)

plt = planarPlot (cl, 1, plt) # initialize 2D plot with obstacles and path
plt = addNodePlot (collConstrNodes, 'bo', 'qConstr', plt)
plt = addNodePlot (collNodes, 'ro', 1, 'qCol', plt)
plt = addPathPlot (cl, x0Path, 'm', 1, plt)
plt = addPathPlot (cl, x1Path, 'g', 1, plt)
plt = addPathPlot (cl, x2Path, 'b', 1, plt)

Esempio n. 6

cl.robot.distancesToCollision()[2][argmin(cl.robot.distancesToCollision()[0])]
r( cl.problem.configAtDistance(0,5) )
cl.problem.optimizePath (0)
cl.problem.clearRoadmap ()
cl.problem.resetGoalConfigs ()
robot.getJointNames ()
robot.getConfigSize ()
cl.obstacle.getObstaclePosition('decor_base')
robot.getJointOuterObjects('shoulder_pan_joint')
cl.robot.getRobotRadiuses ()

## Debug Optimization Tools ##############
num_log = 32174
from parseLog import parseCollConstrPoints, parseNodes

collConstrNodes = parseNodes (num_log, '189: qFree_ = ')
collNodes = parseNodes (num_log, '182: qColl = ')

contactPoints = parseCollConstrPoints (num_log, '77: contact point = (')
x1_J1 = parseCollConstrPoints (num_log, '96: x1 in R0 = (')
x2_J1 = parseCollConstrPoints (num_log, '97: x2 in R0 = (')
x1_J2 = parseCollConstrPoints (num_log, '116: x1 in J2 = (')
x2_J2 = parseCollConstrPoints (num_log, '117: x2 in J2 = (') #x2_J2 <=> contactPoints


## same with viewer !
from viewer_display_library_OPTIM import transformInConfig, plotPoints, plotPointsAndLines, plot2DBaseCurvPath, plotDofCurvPath, plotPointBodyCurvPath, plotBodyCurvPath
contactPointsViewer = transformInConfig (contactPoints)
x1_J1Viewer = transformInConfig (x1_J1)
x2_J1Viewer = transformInConfig (x2_J1)
x1_J2Viewer = transformInConfig (x1_J2)

Esempio n. 7

## Video capture ##
r.startCapture ("capture","png")
pp(0)
r.stopCapture ()
#ffmpeg -r 50 -i capture_0_%d.png -r 25 -vcodec libx264 video.mp4


## Path drawing ##
import matplotlib.pyplot as plt
num_log = 23374
from parseLog import parseNodes, parseParabola
from parabola_plot_tools import parabPlot, addNodePlot, plotParsedParab
nPath = 0
mu = 0.5

RandConfig = parseNodes (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/parabola/parabola-planner.cc:187: qrand: ')
ProjNodes = parseNodes (num_log, 'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/parabola/parabola-planner.cc:203: q_proj: ')

plt = addNodePlot (RandConfig, 'bo', 'qrand', plt)
plt = addNodePlot (ProjNodes, 'ro', 'qproj', plt)

plt = parabPlot (cl, nPath, mu, plt)


plt.show() # will reset plt


parab = parseParabola (num_log, 1)
plt = plotParsedParab (cl, parab, 'b', 1, plt)