# 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
pathSamples = plotSampleSubPath (cl, r, pathId, 70, "path0", [0,0,1,1])
writePathSamples (pathSamples, 'path.txt')
## Write RM edge and node index associated to solution-path:
writeSkipList (cl, 'indexes.txt') #[0, 3, 12, 16, 24, 21, 10, 8] #[0, 0, 6, 15, 17, 18, 13, 14]
# Write nodes to file: PROBLEM when displaying them in Blender (like a small shift before rot...)
#from euler_quat_math import Quaternion, test_q2e
node_blender_k = []
f = open('nodes.txt','a')
for k in range (0,ps.numberNodes()):
print ("Node number: " + str(k))
node_k = ps.nodes() [k]
node_blender_k = node_k [0:7]
#euler = test_q2e (Quaternion(node_k [3],node_k [4],node_k [5],node_k [6])) # in degrees
#node_blender_k [3] = euler.x; node_blender_k [4] = euler.y; node_blender_k [5] = euler.z
f.write(str(node_blender_k).strip('[]')+'\n') # write node k
f.close()
q1 = [2, 1.5]; q2 = [1, 0.5]
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
ps.resetGoalConfigs ()
q1 = [1, 0.5]; q2 = [2, 0]
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
ps.resetGoalConfigs ()
q1 = [-2, 0]; q2 = [2, 0]
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
# pp(7) = p0 final
ps.optimizePath(7) # pp(8) = p1 final
ps.pathLength(7)
ps.pathLength(8)
"""
len(ps.nodes ())
len(ps.getWaypoints (0))
cl.problem.getIterationNumber()
"""
from hpp.gepetto import Viewer, PathPlayer
Viewer.withFloor = True
r = Viewer (ps)
pp = PathPlayer (cl, r)
r.loadObstacleModel ("robot_2d_description","cylinder_obstacle","cylinder_obstacle")
"""
git commit -m "Remove inverse Hessian computation from Cost"
## Debug Optimization Tools ##############
import matplotlib.pyplot as plt
# test
jp = [-4.016784835909555, -4.671737012514295, 0.9493205942691932, -0.19838422293030356, 0.17672477884262752, -0.35476407311266434, 0.8964120174695772]
ps.createPositionConstraint ("l_gripper", "l_gripper_tool_joint", "", [0,0,0], jp[:3], [1,1,1])
ps.client.problem.setGoalNumericalConstraints ("test",["l_gripper",], [0,])
# qq = [-3.2736323380281704, -4.919012557067956, -0.5645503720485425, 0.8253986172873397, 0.31, 2.4756472106318212, -0.10231121294734401, 0.5832544388135313, 0.9877368674476137, 0.351352194158052, -1.1240948972856866, 0.9832501749355624, -0.1822610586197337, -0.8650504112999698, 0.6602954608097523, -0.7510059283614456, 0.28134369878719734, 0.5218218893957427, 0.5020748972660279, 0.34837002692202573, 0.39307871732911037, 0.012744229982022303, 0.33261894418886817, -0.7484151440244099, -1.5567378515100605, -0.2601290957285226, -0.1203692551331449, -1.9576008301144934, 0.8124905337042794, -0.5829743842064892, -1.8222188044396315, -0.7752624281039475, -0.63163927013001, 0.5474106358659504, 0.11960478411037699, 0.2810869521336104, 0.45983350461154876, 0.3357266282624224, 0.026642845592760873, 0.34937864270125457]
# ps.addGoalConfig (qq)
time = list()
nbNode = list()
nbIter = 1
for i in range(nbIter):
ps.clearRoadmap ()
time.append (ps.solve ())
nbNode.append(len(ps.nodes()))
print i, time[-1], nbNode[-1]
i = -1
indexes=[]
while True:
try:
i = nbNode.index(2, i+1)
except ValueError:
break
indexes.append (i)
import numpy as np
m = np.mean(np.array(time)[indexes],0)
print (m[2]*1000 + m[3]) / 1000., "sec."
res = ps.applyConstraints (q2)
if res [0]:
q2proj = res [1]
else:
raise RuntimeError ("Failed to apply constraint.")
cl.obstacle.loadObstacleModel('room_description','room','')
ps.setInitialConfig (q1proj); ps.addGoalConfig (q2proj)
ps.solve ()
begin=time.time()
ps.optimizePath (0)
end=time.time()
print "Solving time: "+str(end-begin)
len(ps.nodes ())
ps.pathLength(0)
ps.pathLength(1)
pp (1)
## DEBUG commands
cl.obstacle.getObstaclePosition('obstacle_base')
cl.robot.getJointOuterObjects('CHEST_JOINT1')
cl.robot.getCurrentConfig()
cl.robot.setCurrentConfig(q1)
cl.robot.collisionTest()
res = cl.robot.distancesToCollision()
cl.problem.pathLength(1)
r( cl.problem.configAtDistance(1,5) )
