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)
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)
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)
## ------------------------------------- # 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
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)
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)
## 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)