vf = ViewerFactory (ps)
# vf.loadObstacleModel ("ur_description","obstacles","obstacles")
vf.loadObstacleModel ("ur_description","table","table")
# vf.loadObstacleModel ("ur_description","wall","wall")
vf(q1)
ps.lockJoint("elbow_joint", [q1[2]])
ps.selectPathValidation("Progressive", .05)
ps.setParameter("SplineGradientBased/alphaInit", CORBA.Any(CORBA.TC_double, 0.3))
ps.setParameter("SplineGradientBased/alwaysStopAtFirst", CORBA.Any(CORBA.TC_boolean, True))
ps.setParameter("SplineGradientBased/linearizeAtEachStep", CORBA.Any(CORBA.TC_boolean, False))
print "Optimizer parameters are:"
print "alphaInit:", ps.getParameter("SplineGradientBased/alphaInit").value()
print "alwaysStopAtFirst:", ps.getParameter("SplineGradientBased/alwaysStopAtFirst").value()
print "linearizeAtEachStep:", ps.getParameter("SplineGradientBased/linearizeAtEachStep").value()
print ""
import datetime as dt
totalSolveTime = dt.timedelta (0)
totalOptimTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
ps.clearPathOptimizers()
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
