diff --git a/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.BACKUP.4492.py b/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.BACKUP.4492.py
new file mode 100644
index 0000000000000000000000000000000000000000..840571b88fc4891fb2994085f0b84dfb1944f6cc
--- /dev/null
+++ b/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.BACKUP.4492.py
@@ -0,0 +1,139 @@
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plt
+from matplotlib import cm
+
+
+cma = cm.autumn
+
+#~ plt.ion()
+
+## Display a 3d plot of the values computed for a limb database
+# where all samples take the maximum value of the octree they belong to
+# \param robot FullBody object
+# \param valueName name of the plotted analytics
+# \param limb name of the considered limb
+def plotcube(plt, ax, c, pos):
+ x = pos[0]; y = pos[1]; z = pos[2]
+ r = (float)(pos[3])/2
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z -r, z - r]
+ X, Y = np.meshgrid(x1, y1)
+ ax.plot_surface(X,Y,z1, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z + r]
+ X, Y = np.meshgrid(x1, y1)
+ ax.plot_surface(X,Y,z1, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y + r, y + r]
+ z1 = [z + r, z - r]
+ X, Z = np.meshgrid(x1, z1)
+ ax.plot_surface(X,y1,Z, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y - r]
+ z1 = [z + r, z - r]
+ X, Z = np.meshgrid(x1, z1)
+ ax.plot_surface(X,y1,Z, color = cma(c))
+
+ x1 = [x - r, x - r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z - r]
+ Y, Z = np.meshgrid(y1, z1)
+ ax.plot_surface(x1,Y,Z, color = cma(c))
+
+ x1 = [x + r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z - r]
+ Y, Z = np.meshgrid(y1, z1)
+ ax.plot_surface(x1,Y,Z, color = cma(c))
+
+def getOctreeValues(robot, valueName, limb):
+ res = {}
+ res ['boxes'] = []
+ res ['values'] = []
+ octreeIds = robot.getOctreeNodeIds(limb)
+ for ocId in octreeIds:
+ sampleIds = robot.getSamplesIdsInOctreeNode(limb, ocId)
+ max_val = 0;
+ for sId in sampleIds:
+ i = int(sId)
+ g = robot.getSampleValue(limb, valueName, i)
+ max_val = max(max_val, g)
+ box = robot.getOctreeBox(limb, (int)(ocId))
+ res['boxes'].append(box)
+ res['values'].append(max_val)
+ return res
+
+
+def plotOctreeValues(robot, valueName, limb):
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection='3d')
+ boxesValues = getOctreeValues(robot, valueName, limb)
+ boxes = boxesValues['boxes']
+ values = boxesValues['values']
+ for i in range(0,len(boxes)):
+ plotcube(plt,ax,values[i], boxes[i])
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+ plt.title(valueName)
+ plt.show()
+
+def plotOctreeValuesCompare(ax, boxesValues):
+ boxes = boxesValues['boxes']
+ values = boxesValues['values']
+ for i in range(0,len(boxes)):
+ plotcube(plt,ax,values[i], boxes[i])
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+
+def compareOctreeValues(robotName1, robotName2, boxesValues1, boxesValues2, valueName):
+ fig = plt.figure()
+ fig.suptitle(valueName)
+ ax = fig.add_subplot(121, projection='3d')
+ ax.set_title(robotName1)
+ plotOctreeValuesCompare(ax, boxesValues1)
+ bx = fig.add_subplot(122, projection='3d')
+ bx.set_title(robotName2)
+ plotOctreeValuesCompare(bx, boxesValues2)
+ #~ plt.title(valueName)
+<<<<<<< HEAD
+=======
+ plt.savefig(valueName+'.png')
+>>>>>>> rb-rrt
+ plt.draw()
+
+## Display a 3d plot of the values computed for a limb database
+#
+# \param robot FullBody object
+# \param valueName name of the plotted analytics
+# \param limb name of the considered limb
+def plotValues(robot, valueName, limb):
+ xs = []
+ ys = []
+ zs = []
+ vals = []
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection='3d')
+ numSamples = robot.getNumSamples(limb)
+ for i in range(0,numSamples):
+ g = robot.getSampleValue(limb, valueName, i)
+ pos = robot.getSamplePosition(limb, i)
+ xs.append(pos[0])
+ ys.append(pos[1])
+ zs.append(pos[2])
+ g = robot.getSampleValue(limb, valueName, i)
+ vals.append(cma(g))
+ ax.scatter(xs, ys, zs, color=vals)
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+ plt.title(valueName)
+ plt.show()
diff --git a/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.BASE.4492.py b/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.BASE.4492.py
new file mode 100644
index 0000000000000000000000000000000000000000..a2783bc87a06aaa8f602ae7e6f8503d13b4a9b2a
--- /dev/null
+++ b/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.BASE.4492.py
@@ -0,0 +1,102 @@
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plt
+from matplotlib import cm
+
+
+cma = cm.autumn
+
+## Display a 3d plot of the values computed for a limb database
+# where all samples take the maximum value of the octree they belong to
+# \param robot FullBody object
+# \param valueName name of the plotted analytics
+# \param limb name of the considered limb
+def plotcube(plt, ax, c, pos):
+ x = pos[0]; y = pos[1]; z = pos[2]
+ r = (float)(pos[3])/2
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z -r, z - r]
+ X, Y = np.meshgrid(x1, y1)
+ ax.plot_surface(X,Y,z1, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z + r]
+ X, Y = np.meshgrid(x1, y1)
+ ax.plot_surface(X,Y,z1, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y + r, y + r]
+ z1 = [z + r, z - r]
+ X, Z = np.meshgrid(x1, z1)
+ ax.plot_surface(X,y1,Z, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y - r]
+ z1 = [z + r, z - r]
+ X, Z = np.meshgrid(x1, z1)
+ ax.plot_surface(X,y1,Z, color = cma(c))
+
+ x1 = [x - r, x - r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z - r]
+ Y, Z = np.meshgrid(y1, z1)
+ ax.plot_surface(x1,Y,Z, color = cma(c))
+
+ x1 = [x + r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z - r]
+ Y, Z = np.meshgrid(y1, z1)
+ ax.plot_surface(x1,Y,Z, color = cma(c))
+
+def plotOctreeValues(robot, valueName, limb):
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection='3d')
+ #first iterate over octree.
+ octreeIds = robot.getOctreeNodeIds(limb)
+ for ocId in octreeIds:
+ sampleIds = robot.getSamplesIdsInOctreeNode(limb, ocId)
+ max_val = 0;
+ for sId in sampleIds:
+ i = int(sId)
+ g = robot.getSampleValue(limb, valueName, i)
+ max_val = max(max_val, g)
+ box = robot.getOctreeBox(limb, (int)(ocId))
+ plotcube(plt,ax,max_val, box)
+ #~ if box[2] < -0.15 and box[2] > -0.25:
+ #~ plotcube(plt,ax,max_val, box)
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+ plt.title(valueName)
+ plt.show()
+
+## Display a 3d plot of the values computed for a limb database
+#
+# \param robot FullBody object
+# \param valueName name of the plotted analytics
+# \param limb name of the considered limb
+def plotValues(robot, valueName, limb):
+ xs = []
+ ys = []
+ zs = []
+ vals = []
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection='3d')
+ numSamples = robot.getNumSamples(limb)
+ for i in range(0,numSamples):
+ g = robot.getSampleValue(limb, valueName, i)
+ pos = robot.getSamplePosition(limb, i)
+ xs.append(pos[0])
+ ys.append(pos[1])
+ zs.append(pos[2])
+ g = robot.getSampleValue(limb, valueName, i)
+ vals.append(cma(g))
+ ax.scatter(xs, ys, zs, color=vals)
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+ plt.title(valueName)
+ plt.show()
diff --git a/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.LOCAL.4492.py b/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.LOCAL.4492.py
new file mode 100644
index 0000000000000000000000000000000000000000..eb8d49f799c4ec4964cc0f58e2f6660ea01ad042
--- /dev/null
+++ b/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.LOCAL.4492.py
@@ -0,0 +1,135 @@
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plt
+from matplotlib import cm
+
+
+cma = cm.autumn
+
+#~ plt.ion()
+
+## Display a 3d plot of the values computed for a limb database
+# where all samples take the maximum value of the octree they belong to
+# \param robot FullBody object
+# \param valueName name of the plotted analytics
+# \param limb name of the considered limb
+def plotcube(plt, ax, c, pos):
+ x = pos[0]; y = pos[1]; z = pos[2]
+ r = (float)(pos[3])/2
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z -r, z - r]
+ X, Y = np.meshgrid(x1, y1)
+ ax.plot_surface(X,Y,z1, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z + r]
+ X, Y = np.meshgrid(x1, y1)
+ ax.plot_surface(X,Y,z1, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y + r, y + r]
+ z1 = [z + r, z - r]
+ X, Z = np.meshgrid(x1, z1)
+ ax.plot_surface(X,y1,Z, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y - r]
+ z1 = [z + r, z - r]
+ X, Z = np.meshgrid(x1, z1)
+ ax.plot_surface(X,y1,Z, color = cma(c))
+
+ x1 = [x - r, x - r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z - r]
+ Y, Z = np.meshgrid(y1, z1)
+ ax.plot_surface(x1,Y,Z, color = cma(c))
+
+ x1 = [x + r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z - r]
+ Y, Z = np.meshgrid(y1, z1)
+ ax.plot_surface(x1,Y,Z, color = cma(c))
+
+def getOctreeValues(robot, valueName, limb):
+ res = {}
+ res ['boxes'] = []
+ res ['values'] = []
+ octreeIds = robot.getOctreeNodeIds(limb)
+ for ocId in octreeIds:
+ sampleIds = robot.getSamplesIdsInOctreeNode(limb, ocId)
+ max_val = 0;
+ for sId in sampleIds:
+ i = int(sId)
+ g = robot.getSampleValue(limb, valueName, i)
+ max_val = max(max_val, g)
+ box = robot.getOctreeBox(limb, (int)(ocId))
+ res['boxes'].append(box)
+ res['values'].append(max_val)
+ return res
+
+
+def plotOctreeValues(robot, valueName, limb):
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection='3d')
+ boxesValues = getOctreeValues(robot, valueName, limb)
+ boxes = boxesValues['boxes']
+ values = boxesValues['values']
+ for i in range(0,len(boxes)):
+ plotcube(plt,ax,values[i], boxes[i])
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+ plt.title(valueName)
+ plt.show()
+
+def plotOctreeValuesCompare(ax, boxesValues):
+ boxes = boxesValues['boxes']
+ values = boxesValues['values']
+ for i in range(0,len(boxes)):
+ plotcube(plt,ax,values[i], boxes[i])
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+
+def compareOctreeValues(robotName1, robotName2, boxesValues1, boxesValues2, valueName):
+ fig = plt.figure()
+ fig.suptitle(valueName)
+ ax = fig.add_subplot(121, projection='3d')
+ ax.set_title(robotName1)
+ plotOctreeValuesCompare(ax, boxesValues1)
+ bx = fig.add_subplot(122, projection='3d')
+ bx.set_title(robotName2)
+ plotOctreeValuesCompare(bx, boxesValues2)
+ #~ plt.title(valueName)
+ plt.draw()
+
+## Display a 3d plot of the values computed for a limb database
+#
+# \param robot FullBody object
+# \param valueName name of the plotted analytics
+# \param limb name of the considered limb
+def plotValues(robot, valueName, limb):
+ xs = []
+ ys = []
+ zs = []
+ vals = []
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection='3d')
+ numSamples = robot.getNumSamples(limb)
+ for i in range(0,numSamples):
+ g = robot.getSampleValue(limb, valueName, i)
+ pos = robot.getSamplePosition(limb, i)
+ xs.append(pos[0])
+ ys.append(pos[1])
+ zs.append(pos[2])
+ g = robot.getSampleValue(limb, valueName, i)
+ vals.append(cma(g))
+ ax.scatter(xs, ys, zs, color=vals)
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+ plt.title(valueName)
+ plt.show()
diff --git a/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.REMOTE.4492.py b/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.REMOTE.4492.py
new file mode 100644
index 0000000000000000000000000000000000000000..2965bdaadd6eeb974c4ffdcad3e94cee243ff092
--- /dev/null
+++ b/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.REMOTE.4492.py
@@ -0,0 +1,136 @@
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plt
+from matplotlib import cm
+
+
+cma = cm.autumn
+
+#~ plt.ion()
+
+## Display a 3d plot of the values computed for a limb database
+# where all samples take the maximum value of the octree they belong to
+# \param robot FullBody object
+# \param valueName name of the plotted analytics
+# \param limb name of the considered limb
+def plotcube(plt, ax, c, pos):
+ x = pos[0]; y = pos[1]; z = pos[2]
+ r = (float)(pos[3])/2
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z -r, z - r]
+ X, Y = np.meshgrid(x1, y1)
+ ax.plot_surface(X,Y,z1, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z + r]
+ X, Y = np.meshgrid(x1, y1)
+ ax.plot_surface(X,Y,z1, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y + r, y + r]
+ z1 = [z + r, z - r]
+ X, Z = np.meshgrid(x1, z1)
+ ax.plot_surface(X,y1,Z, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y - r]
+ z1 = [z + r, z - r]
+ X, Z = np.meshgrid(x1, z1)
+ ax.plot_surface(X,y1,Z, color = cma(c))
+
+ x1 = [x - r, x - r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z - r]
+ Y, Z = np.meshgrid(y1, z1)
+ ax.plot_surface(x1,Y,Z, color = cma(c))
+
+ x1 = [x + r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z - r]
+ Y, Z = np.meshgrid(y1, z1)
+ ax.plot_surface(x1,Y,Z, color = cma(c))
+
+def getOctreeValues(robot, valueName, limb):
+ res = {}
+ res ['boxes'] = []
+ res ['values'] = []
+ octreeIds = robot.getOctreeNodeIds(limb)
+ for ocId in octreeIds:
+ sampleIds = robot.getSamplesIdsInOctreeNode(limb, ocId)
+ max_val = 0;
+ for sId in sampleIds:
+ i = int(sId)
+ g = robot.getSampleValue(limb, valueName, i)
+ max_val = max(max_val, g)
+ box = robot.getOctreeBox(limb, (int)(ocId))
+ res['boxes'].append(box)
+ res['values'].append(max_val)
+ return res
+
+
+def plotOctreeValues(robot, valueName, limb):
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection='3d')
+ boxesValues = getOctreeValues(robot, valueName, limb)
+ boxes = boxesValues['boxes']
+ values = boxesValues['values']
+ for i in range(0,len(boxes)):
+ plotcube(plt,ax,values[i], boxes[i])
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+ plt.title(valueName)
+ plt.show()
+
+def plotOctreeValuesCompare(ax, boxesValues):
+ boxes = boxesValues['boxes']
+ values = boxesValues['values']
+ for i in range(0,len(boxes)):
+ plotcube(plt,ax,values[i], boxes[i])
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+
+def compareOctreeValues(robotName1, robotName2, boxesValues1, boxesValues2, valueName):
+ fig = plt.figure()
+ fig.suptitle(valueName)
+ ax = fig.add_subplot(121, projection='3d')
+ ax.set_title(robotName1)
+ plotOctreeValuesCompare(ax, boxesValues1)
+ bx = fig.add_subplot(122, projection='3d')
+ bx.set_title(robotName2)
+ plotOctreeValuesCompare(bx, boxesValues2)
+ #~ plt.title(valueName)
+ plt.savefig(valueName+'.png')
+ plt.draw()
+
+## Display a 3d plot of the values computed for a limb database
+#
+# \param robot FullBody object
+# \param valueName name of the plotted analytics
+# \param limb name of the considered limb
+def plotValues(robot, valueName, limb):
+ xs = []
+ ys = []
+ zs = []
+ vals = []
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection='3d')
+ numSamples = robot.getNumSamples(limb)
+ for i in range(0,numSamples):
+ g = robot.getSampleValue(limb, valueName, i)
+ pos = robot.getSamplePosition(limb, i)
+ xs.append(pos[0])
+ ys.append(pos[1])
+ zs.append(pos[2])
+ g = robot.getSampleValue(limb, valueName, i)
+ vals.append(cma(g))
+ ax.scatter(xs, ys, zs, color=vals)
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+ plt.title(valueName)
+ plt.show()
diff --git a/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.orig b/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.orig
new file mode 100644
index 0000000000000000000000000000000000000000..840571b88fc4891fb2994085f0b84dfb1944f6cc
--- /dev/null
+++ b/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.orig
@@ -0,0 +1,139 @@
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plt
+from matplotlib import cm
+
+
+cma = cm.autumn
+
+#~ plt.ion()
+
+## Display a 3d plot of the values computed for a limb database
+# where all samples take the maximum value of the octree they belong to
+# \param robot FullBody object
+# \param valueName name of the plotted analytics
+# \param limb name of the considered limb
+def plotcube(plt, ax, c, pos):
+ x = pos[0]; y = pos[1]; z = pos[2]
+ r = (float)(pos[3])/2
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z -r, z - r]
+ X, Y = np.meshgrid(x1, y1)
+ ax.plot_surface(X,Y,z1, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z + r]
+ X, Y = np.meshgrid(x1, y1)
+ ax.plot_surface(X,Y,z1, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y + r, y + r]
+ z1 = [z + r, z - r]
+ X, Z = np.meshgrid(x1, z1)
+ ax.plot_surface(X,y1,Z, color = cma(c))
+
+ x1 = [x - r, x + r]
+ y1 = [y - r, y - r]
+ z1 = [z + r, z - r]
+ X, Z = np.meshgrid(x1, z1)
+ ax.plot_surface(X,y1,Z, color = cma(c))
+
+ x1 = [x - r, x - r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z - r]
+ Y, Z = np.meshgrid(y1, z1)
+ ax.plot_surface(x1,Y,Z, color = cma(c))
+
+ x1 = [x + r, x + r]
+ y1 = [y - r, y + r]
+ z1 = [z + r, z - r]
+ Y, Z = np.meshgrid(y1, z1)
+ ax.plot_surface(x1,Y,Z, color = cma(c))
+
+def getOctreeValues(robot, valueName, limb):
+ res = {}
+ res ['boxes'] = []
+ res ['values'] = []
+ octreeIds = robot.getOctreeNodeIds(limb)
+ for ocId in octreeIds:
+ sampleIds = robot.getSamplesIdsInOctreeNode(limb, ocId)
+ max_val = 0;
+ for sId in sampleIds:
+ i = int(sId)
+ g = robot.getSampleValue(limb, valueName, i)
+ max_val = max(max_val, g)
+ box = robot.getOctreeBox(limb, (int)(ocId))
+ res['boxes'].append(box)
+ res['values'].append(max_val)
+ return res
+
+
+def plotOctreeValues(robot, valueName, limb):
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection='3d')
+ boxesValues = getOctreeValues(robot, valueName, limb)
+ boxes = boxesValues['boxes']
+ values = boxesValues['values']
+ for i in range(0,len(boxes)):
+ plotcube(plt,ax,values[i], boxes[i])
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+ plt.title(valueName)
+ plt.show()
+
+def plotOctreeValuesCompare(ax, boxesValues):
+ boxes = boxesValues['boxes']
+ values = boxesValues['values']
+ for i in range(0,len(boxes)):
+ plotcube(plt,ax,values[i], boxes[i])
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+
+def compareOctreeValues(robotName1, robotName2, boxesValues1, boxesValues2, valueName):
+ fig = plt.figure()
+ fig.suptitle(valueName)
+ ax = fig.add_subplot(121, projection='3d')
+ ax.set_title(robotName1)
+ plotOctreeValuesCompare(ax, boxesValues1)
+ bx = fig.add_subplot(122, projection='3d')
+ bx.set_title(robotName2)
+ plotOctreeValuesCompare(bx, boxesValues2)
+ #~ plt.title(valueName)
+<<<<<<< HEAD
+=======
+ plt.savefig(valueName+'.png')
+>>>>>>> rb-rrt
+ plt.draw()
+
+## Display a 3d plot of the values computed for a limb database
+#
+# \param robot FullBody object
+# \param valueName name of the plotted analytics
+# \param limb name of the considered limb
+def plotValues(robot, valueName, limb):
+ xs = []
+ ys = []
+ zs = []
+ vals = []
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection='3d')
+ numSamples = robot.getNumSamples(limb)
+ for i in range(0,numSamples):
+ g = robot.getSampleValue(limb, valueName, i)
+ pos = robot.getSamplePosition(limb, i)
+ xs.append(pos[0])
+ ys.append(pos[1])
+ zs.append(pos[2])
+ g = robot.getSampleValue(limb, valueName, i)
+ vals.append(cma(g))
+ ax.scatter(xs, ys, zs, color=vals)
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+ plt.title(valueName)
+ plt.show()
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index e21c6b4f5d21526e111a2b325684e8a046914900..b0c4bcb6c605decb54febfccf637c33bbd58b229 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -559,7 +559,7 @@ namespace hpp {
{
throw std::runtime_error ("End state not initialized, can not interpolate ");
}
- hpp::rbprm::RbPrmInterpolationPtr_t interpolator = rbprm::RbPrmInterpolation::create(fullBody_,startState_,endState_);
+ hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_);
std::vector<model::Configuration_t> configurations = doubleDofArrayToConfig(fullBody_->device_, configs);
lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),configurations,robustnessTreshold);
hpp::floatSeqSeq *res;
@@ -646,7 +646,7 @@ namespace hpp {
{
try
{
- std::size_t s1(state1), s2(state2);
+ std::size_t s1((std::size_t)state1), s2((std::size_t)state2);
if(lastStatesComputed_.size () < s1 || lastStatesComputed_.size () < s2 )
{
throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s1) + ", " + std::string(""+s2));
diff --git a/src/rbprmbuilder.impl.cc.orig b/src/rbprmbuilder.impl.cc.orig
new file mode 100755
index 0000000000000000000000000000000000000000..4fe99b90d90b3ac0a9a1e051adc910a75dae8b82
--- /dev/null
+++ b/src/rbprmbuilder.impl.cc.orig
@@ -0,0 +1,973 @@
+// Copyright (c) 2012 CNRS
+// Author: Florent Lamiraux, Joseph Mirabel
+//
+// This file is part of hpp-manipulation-corba.
+// hpp-manipulation-corba is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation-corba is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation-corba. If not, see
+// <http://www.gnu.org/licenses/>.
+
+//#include <hpp/fcl/math/transform.h>
+#include <hpp/util/debug.hh>
+#include "rbprmbuilder.impl.hh"
+#include "hpp/rbprm/rbprm-device.hh"
+#include "hpp/rbprm/rbprm-validation.hh"
+#include "hpp/rbprm/interpolation/rbprm-path-interpolation.hh"
+<<<<<<< HEAD
+=======
+#include "hpp/rbprm/interpolation/limb-rrt-helper.hh"
+>>>>>>> rb-rrt
+#include "hpp/rbprm/stability/stability.hh"
+#include "hpp/rbprm/sampling/sample-db.hh"
+#include "hpp/model/urdf/util.hh"
+#include <fstream>
+
+
+
+namespace hpp {
+ namespace rbprm {
+ namespace impl {
+
+ RbprmBuilder::RbprmBuilder ()
+ : POA_hpp::corbaserver::rbprm::RbprmBuilder()
+ , romLoaded_(false)
+ , fullBodyLoaded_(false)
+ , bindShooter_()
+ , analysisFactory_(0)
+ {
+ // NOTHING
+ }
+
+ void RbprmBuilder::loadRobotRomModel(const char* robotName,
+ const char* rootJointType,
+ const char* packageName,
+ const char* modelName,
+ const char* urdfSuffix,
+ const char* srdfSuffix) throw (hpp::Error)
+ {
+ try
+ {
+ hpp::model::DevicePtr_t romDevice = model::Device::create (robotName);
+std::cout << romDevice->currentConfiguration() << std::endl;
+ romDevices_.insert(std::make_pair(robotName, romDevice));
+ hpp::model::urdf::loadRobotModel (romDevice,
+ std::string (rootJointType),
+ std::string (packageName),
+ std::string (modelName),
+ std::string (urdfSuffix),
+ std::string (srdfSuffix));
+ }
+ catch (const std::exception& exc)
+ {
+ hppDout (error, exc.what ());
+ throw hpp::Error (exc.what ());
+ }
+ romLoaded_ = true;
+ }
+
+ void RbprmBuilder::loadRobotCompleteModel(const char* robotName,
+ const char* rootJointType,
+ const char* packageName,
+ const char* modelName,
+ const char* urdfSuffix,
+ const char* srdfSuffix) throw (hpp::Error)
+ {
+ if(!romLoaded_)
+ {
+ std::string err("Rom must be loaded before loading complete model") ;
+ hppDout (error, err );
+ throw hpp::Error(err.c_str());
+ }
+ try
+ {
+ hpp::model::RbPrmDevicePtr_t device = hpp::model::RbPrmDevice::create (robotName, romDevices_);
+ hpp::model::urdf::loadRobotModel (device,
+ std::string (rootJointType),
+ std::string (packageName),
+ std::string (modelName),
+ std::string (urdfSuffix),
+ std::string (srdfSuffix));
+ // Add device to the planner
+ problemSolver_->robot (device);
+ problemSolver_->robot ()->controlComputation
+ (model::Device::JOINT_POSITION);
+ }
+ catch (const std::exception& exc)
+ {
+ hppDout (error, exc.what ());
+ throw hpp::Error (exc.what ());
+ }
+ }
+
+ void RbprmBuilder::loadFullBodyRobot(const char* robotName,
+ const char* rootJointType,
+ const char* packageName,
+ const char* modelName,
+ const char* urdfSuffix,
+ const char* srdfSuffix) throw (hpp::Error)
+ {
+ try
+ {
+ model::DevicePtr_t device = model::Device::create (robotName);
+ hpp::model::urdf::loadRobotModel (device,
+ std::string (rootJointType),
+ std::string (packageName),
+ std::string (modelName),
+ std::string (urdfSuffix),
+ std::string (srdfSuffix));
+ fullBody_ = rbprm::RbPrmFullBody::create(device);
+ problemSolver_->pathValidationType ("Discretized",0.05); // reset to avoid conflict with rbprm path
+ problemSolver_->robot (fullBody_->device_);
+ problemSolver_->resetProblem();
+ problemSolver_->robot ()->controlComputation
+ (model::Device::JOINT_POSITION);
+ }
+ catch (const std::exception& exc)
+ {
+ hppDout (error, exc.what ());
+ throw hpp::Error (exc.what ());
+ }
+ fullBodyLoaded_ = true;
+ analysisFactory_ = new sampling::AnalysisFactory(fullBody_);
+ }
+
+ hpp::floatSeq* RbprmBuilder::getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error)
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ const T_Limb& limbs = fullBody_->GetLimbs();
+ T_Limb::const_iterator lit = limbs.find(std::string(limb));
+ if(lit == limbs.end())
+ {
+ std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody_->device_->name());
+ throw Error (err.c_str());
+ }
+ const RbPrmLimbPtr_t& limbPtr = lit->second;
+ hpp::floatSeq *dofArray;
+ Eigen::VectorXd config = fullBody_->device_->currentConfiguration ();
+ if(sampleId > limbPtr->sampleContainer_.samples_.size())
+ {
+ std::string err("Limb " + std::string(limb) + "does not have samples.");
+ throw Error (err.c_str());
+ }
+ const sampling::Sample& sample = limbPtr->sampleContainer_.samples_[sampleId];
+ config.segment(sample.startRank_, sample.length_) = sample.configuration_;
+ dofArray = new hpp::floatSeq();
+ dofArray->length(_CORBA_ULong(config.rows()));
+ for(std::size_t i=0; i< _CORBA_ULong(config.rows()); i++)
+ (*dofArray)[(_CORBA_ULong)i] = config [i];
+ return dofArray;
+ }
+
+
+ hpp::floatSeq* RbprmBuilder::getSamplePosition(const char* limb, unsigned short sampleId) throw (hpp::Error)
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ const T_Limb& limbs = fullBody_->GetLimbs();
+ T_Limb::const_iterator lit = limbs.find(std::string(limb));
+ if(lit == limbs.end())
+ {
+ std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody_->device_->name());
+ throw Error (err.c_str());
+ }
+ const RbPrmLimbPtr_t& limbPtr = lit->second;
+ hpp::floatSeq *dofArray;
+ if(sampleId > limbPtr->sampleContainer_.samples_.size())
+ {
+ std::string err("Limb " + std::string(limb) + "does not have samples.");
+ throw Error (err.c_str());
+ }
+ const sampling::Sample& sample = limbPtr->sampleContainer_.samples_[sampleId];
+ const fcl::Vec3f& position = sample.effectorPosition_;
+ dofArray = new hpp::floatSeq();
+ dofArray->length(_CORBA_ULong(3));
+ for(std::size_t i=0; i< 3; i++)
+ (*dofArray)[(_CORBA_ULong)i] = position [i];
+ return dofArray;
+ }
+
+
+ CORBA::UShort RbprmBuilder::getNumSamples(const char* limb) throw (hpp::Error)
+ {
+ const T_Limb& limbs = fullBody_->GetLimbs();
+ T_Limb::const_iterator lit = limbs.find(std::string(limb));
+ if(lit == limbs.end())
+ {
+ std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody_->device_->name());
+ throw Error (err.c_str());
+ }
+ return (CORBA::UShort)(lit->second->sampleContainer_.samples_.size());
+ }
+
+ floatSeq *RbprmBuilder::getOctreeNodeIds(const char* limb) throw (hpp::Error)
+ {
+ const T_Limb& limbs = fullBody_->GetLimbs();
+ T_Limb::const_iterator lit = limbs.find(std::string(limb));
+ if(lit == limbs.end())
+ {
+ std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody_->device_->name());
+ throw Error (err.c_str());
+ }
+ const sampling::T_VoxelSampleId& ids = lit->second->sampleContainer_.samplesInVoxels_;
+ hpp::floatSeq* dofArray = new hpp::floatSeq();
+ dofArray->length((_CORBA_ULong)ids.size());
+ sampling::T_VoxelSampleId::const_iterator it = ids.begin();
+ for(std::size_t i=0; i< _CORBA_ULong(ids.size()); ++i, ++it)
+ {
+ (*dofArray)[(_CORBA_ULong)i] = (double)it->first;
+ }
+ return dofArray;
+ }
+
+ double RbprmBuilder::getSampleValue(const char* limb, const char* valueName, unsigned short sampleId) throw (hpp::Error)
+ {
+ const T_Limb& limbs = fullBody_->GetLimbs();
+ T_Limb::const_iterator lit = limbs.find(std::string(limb));
+ if(lit == limbs.end())
+ {
+ std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody_->device_->name());
+ throw Error (err.c_str());
+ }
+ const sampling::SampleDB& database = lit->second->sampleContainer_;
+ if (database.samples_.size() <= sampleId)
+ {
+ std::string err("unexisting sample id " + sampleId);
+ throw Error (err.c_str());
+ }
+ sampling::T_Values::const_iterator cit = database.values_.find(std::string(valueName));
+ if(cit == database.values_.end())
+ {
+ std::string err("value not existing in database " + std::string(valueName));
+ throw Error (err.c_str());
+ }
+ return cit->second[sampleId];
+ }
+
+ model::Configuration_t dofArrayToConfig (const model::DevicePtr_t& robot,
+ const hpp::floatSeq& dofArray)
+ {
+ std::size_t configDim = (std::size_t)dofArray.length();
+ // Get robot
+ if (!robot) {
+ throw hpp::Error ("No robot in problem solver.");
+ }
+ std::size_t deviceDim = robot->configSize ();
+ // Fill dof vector with dof array.
+ model::Configuration_t config; config.resize (configDim);
+ for (std::size_t iDof = 0; iDof < configDim; iDof++) {
+ config [iDof] = (_CORBA_ULong)dofArray[(_CORBA_ULong)iDof];
+ }
+ // fill the vector by zero
+ hppDout (info, "config dimension: " <<configDim
+ <<", deviceDim "<<deviceDim);
+ if(configDim != deviceDim){
+ throw hpp::Error ("dofVector Does not match");
+ }
+ return config;
+ }
+
+ std::vector<model::Configuration_t> doubleDofArrayToConfig (const model::DevicePtr_t& robot,
+ const hpp::floatSeqSeq& doubleDofArray)
+ {
+ std::size_t configsDim = (std::size_t)doubleDofArray.length();
+ std::vector<model::Configuration_t> res;
+ for (_CORBA_ULong iConfig = 0; iConfig < configsDim; iConfig++)
+ {
+ res.push_back(dofArrayToConfig(robot, doubleDofArray[iConfig]));
+ }
+ return res;
+ }
+
+ std::vector<std::string> stringConversion(const hpp::Names_t& dofArray)
+ {
+ std::vector<std::string> res;
+ std::size_t dim = (std::size_t)dofArray.length();
+ for (_CORBA_ULong iDof = 0; iDof < dim; iDof++)
+ {
+ res.push_back(std::string(dofArray[iDof]));
+ }
+ return res;
+ }
+
+ std::vector<double> doubleConversion(const hpp::floatSeq& dofArray)
+ {
+ std::vector<double> res;
+ std::size_t dim = (std::size_t)dofArray.length();
+ for (_CORBA_ULong iDof = 0; iDof < dim; iDof++)
+ {
+ res.push_back(dofArray[iDof]);
+ }
+ return res;
+ }
+
+
+ void RbprmBuilder::setFilter(const hpp::Names_t& roms) throw (hpp::Error)
+ {
+ bindShooter_.romFilter_ = stringConversion(roms);
+ }
+
+
+ void RbprmBuilder::setNormalFilter(const char* romName, const hpp::floatSeq& normal, double range) throw (hpp::Error)
+ {
+ std::string name(romName);
+ bindShooter_.normalFilter_.erase(name);
+ fcl::Vec3f dir;
+ for(_CORBA_ULong i =0; i <3; ++i)
+ {
+ dir[i] = normal[i];
+ }
+ NormalFilter filter(dir,range);
+ bindShooter_.normalFilter_.insert(std::make_pair(name,filter));
+ }
+
+ void RbprmBuilder::boundSO3(const hpp::floatSeq& limitszyx) throw (hpp::Error)
+ {
+ std::vector<double> limits = doubleConversion(limitszyx);
+ if(limits.size() !=6)
+ {
+ throw Error ("Can not bound SO3, array of 6 double required");
+ }
+ bindShooter_.so3Bounds_ = limits;
+
+ }
+
+ hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration, const hpp::floatSeq& direction) throw (hpp::Error)
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ try
+ {
+ fcl::Vec3f dir;
+ for(std::size_t i =0; i <3; ++i)
+ {
+ dir[i] = direction[(_CORBA_ULong)i];
+ }
+ model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
+ rbprm::State state = rbprm::ComputeContacts(fullBody_,config,
+ problemSolver_->collisionObstacles(), dir);
+ hpp::floatSeq* dofArray = new hpp::floatSeq();
+ dofArray->length(_CORBA_ULong(state.configuration_.rows()));
+ for(std::size_t i=0; i< _CORBA_ULong(config.rows()); i++)
+ (*dofArray)[(_CORBA_ULong)i] = state.configuration_ [i];
+ return dofArray;
+ } catch (const std::exception& exc) {
+ throw hpp::Error (exc.what ());
+ }
+ }
+
+ hpp::floatSeq* RbprmBuilder::getContactSamplesIds(const char* limbname,
+ const hpp::floatSeq& configuration,
+ const hpp::floatSeq& direction) throw (hpp::Error)
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ try
+ {
+ fcl::Vec3f dir;
+ for(std::size_t i =0; i <3; ++i)
+ {
+ dir[i] = direction[(_CORBA_ULong)i];
+ }
+ model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
+ model::Configuration_t save = fullBody_->device_->currentConfiguration();
+ fullBody_->device_->currentConfiguration(config);
+
+ sampling::T_OctreeReport finalSet;
+ rbprm::T_Limb::const_iterator lit = fullBody_->GetLimbs().find(std::string(limbname));
+ if(lit == fullBody_->GetLimbs().end())
+ {
+ throw std::runtime_error ("Impossible to find limb for joint "
+ + std::string(limbname) + " to robot; limb not defined.");
+ }
+ const RbPrmLimbPtr_t& limb = lit->second;
+ fcl::Transform3f transform = limb->limb_->robot()->rootJoint()->childJoint(0)->currentTransformation (); // get root transform from configuration
+ std::vector<sampling::T_OctreeReport> reports(problemSolver_->collisionObstacles().size());
+ std::size_t i (0);
+ //#pragma omp parallel for
+ for(model::ObjectVector_t::const_iterator oit = problemSolver_->collisionObstacles().begin();
+ oit != problemSolver_->collisionObstacles().end(); ++oit, ++i)
+ {
+ sampling::GetCandidates(limb->sampleContainer_, transform, *oit, dir, reports[i]);
+ }
+ for(std::vector<sampling::T_OctreeReport>::const_iterator cit = reports.begin();
+ cit != reports.end(); ++cit)
+ {
+ finalSet.insert(cit->begin(), cit->end());
+ }
+ hpp::floatSeq* dofArray = new hpp::floatSeq();
+ dofArray->length((_CORBA_ULong)finalSet.size());
+ sampling::T_OctreeReport::const_iterator candCit = finalSet.begin();
+ for(std::size_t i=0; i< _CORBA_ULong(finalSet.size()); ++i, ++candCit)
+ {
+ (*dofArray)[(_CORBA_ULong)i] = (double)candCit->sample_->id_;
+ }
+ fullBody_->device_->currentConfiguration(save);
+ return dofArray;
+ } catch (const std::exception& exc) {
+ throw hpp::Error (exc.what ());
+ }
+ }
+
+ hpp::floatSeq* RbprmBuilder::getSamplesIdsInOctreeNode(const char* limb,
+ double octreeNodeId) throw (hpp::Error)
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ try
+ {
+ long ocId ((long)octreeNodeId);
+ const T_Limb& limbs = fullBody_->GetLimbs();
+ T_Limb::const_iterator lit = limbs.find(std::string(limb));
+ if(lit == limbs.end())
+ {
+ std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody_->device_->name());
+ throw Error (err.c_str());
+ }
+ const sampling::T_VoxelSampleId& sampleIds = lit->second->sampleContainer_.samplesInVoxels_;
+ sampling::T_VoxelSampleId::const_iterator cit = sampleIds.find(ocId);
+ if(cit == sampleIds.end())
+ {
+ std::stringstream ss; ss << ocId;
+ std::string err("No octree node with id " + ss.str() + "was defined for robot" + fullBody_->device_->name());
+ throw Error (err.c_str());
+ }
+ const sampling::VoxelSampleId& ids = cit->second;
+ hpp::floatSeq* dofArray = new hpp::floatSeq();
+ dofArray->length((_CORBA_ULong)ids.second);
+ std::size_t sampleId = ids.first;
+ for(std::size_t i=0; i< _CORBA_ULong(ids.second); ++i, ++sampleId)
+ {
+ (*dofArray)[(_CORBA_ULong)i] = (double)sampleId;
+ }
+ return dofArray;
+ } catch (const std::exception& exc) {
+ throw hpp::Error (exc.what ());
+ }
+ }
+
+ void RbprmBuilder::addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y,
+ unsigned short samples, const char* heuristicName, double resolution, const char *contactType) throw (hpp::Error)
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ try
+ {
+ fcl::Vec3f off, norm;
+ for(std::size_t i =0; i <3; ++i)
+ {
+ off[i] = offset[(_CORBA_ULong)i];
+ norm[i] = normal[(_CORBA_ULong)i];
+ }
+ ContactType cType = hpp::rbprm::_6_DOF;
+ if(std::string(contactType) == "_3_DOF")
+ {
+ cType = hpp::rbprm::_3_DOF;
+ }
+ fullBody_->AddLimb(std::string(id), std::string(limb), std::string(effector), off, norm, x, y, problemSolver_->collisionObstacles(), samples,heuristicName,resolution,cType);
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+
+ void RbprmBuilder::addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues) throw (hpp::Error)
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ try
+ {
+ std::string fileName(databasePath);
+ fullBody_->AddLimb(fileName, std::string(id), problemSolver_->collisionObstacles(), heuristicName, loadValues > 0.5);
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+ void SetPositionAndNormal(rbprm::State& state, hpp::rbprm::RbPrmFullBodyPtr_t fullBody, const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs)
+ {
+ core::Configuration_t old = fullBody->device_->currentConfiguration();
+ model::Configuration_t config = dofArrayToConfig (fullBody->device_, configuration);
+ fullBody->device_->currentConfiguration(config);
+ fullBody->device_->computeForwardKinematics();
+ std::vector<std::string> names = stringConversion(contactLimbs);
+ for(std::vector<std::string>::const_iterator cit = names.begin(); cit != names.end();++cit)
+ {
+ rbprm::T_Limb::const_iterator lit = fullBody->GetLimbs().find(*cit);
+ if(lit == fullBody->GetLimbs().end())
+ {
+ throw std::runtime_error ("Impossible to find limb for joint "
+ + (*cit) + " to robot; limb not defined");
+ }
+ const core::JointPtr_t joint = fullBody->device_->getJointByName(lit->second->effector_->name());
+ const fcl::Transform3f& transform = joint->currentTransformation ();
+ const fcl::Matrix3f& rot = transform.getRotation();
+ state.contactPositions_[*cit] = transform.getTranslation();
+ state.contactRotation_[*cit] = rot;
+ state.contactNormals_[*cit] = fcl::Vec3f(rot(0,2),rot(1,2), rot(2,2));
+ state.contacts_[*cit] = true;
+ state.contactOrder_.push(*cit);
+ }
+ state.nbContacts = state.contactNormals_.size() ;
+ state.configuration_ = config;
+ state.robustness = stability::IsStable(fullBody,state);
+ state.stable = state.robustness >= 0;
+ fullBody->device_->currentConfiguration(old);
+ }
+
+ void RbprmBuilder::setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error)
+ {
+ try
+ {
+ SetPositionAndNormal(startState_,fullBody_, configuration, contactLimbs);
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+ void RbprmBuilder::setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error)
+ {
+ try
+ {
+ SetPositionAndNormal(endState_,fullBody_, configuration, contactLimbs);
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+ floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error)
+ {
+ try
+ {
+ if(startState_.configuration_.rows() == 0)
+ {
+ throw std::runtime_error ("Start state not initialized, can not interpolate ");
+ }
+ if(endState_.configuration_.rows() == 0)
+ {
+ throw std::runtime_error ("End state not initialized, can not interpolate ");
+ }
+<<<<<<< HEAD
+ hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_);
+=======
+ hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = hpp::rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_);
+>>>>>>> rb-rrt
+ std::vector<model::Configuration_t> configurations = doubleDofArrayToConfig(fullBody_->device_, configs);
+ lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),configurations,robustnessTreshold);
+ hpp::floatSeqSeq *res;
+ res = new hpp::floatSeqSeq ();
+
+ res->length ((_CORBA_ULong)lastStatesComputed_.size ());
+ std::size_t i=0;
+ std::size_t id = 0;
+ for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin(); cit != lastStatesComputed_.end(); ++cit, ++id)
+ {
+ std::cout << "ID " << id;
+ cit->print();
+ const core::Configuration_t config = cit->configuration_;
+ _CORBA_ULong size = (_CORBA_ULong) config.size ();
+ double* dofArray = hpp::floatSeq::allocbuf(size);
+ hpp::floatSeq floats (size, size, dofArray, true);
+ //convert the config in dofseq
+ for (model::size_type j=0 ; j < config.size() ; ++j) {
+ dofArray[j] = config [j];
+ }
+ (*res) [(_CORBA_ULong)i] = floats;
+ ++i;
+ }
+ return res;
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+ floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error)
+ {
+ try
+ {
+ unsigned int pathId = int(path);
+ if(startState_.configuration_.rows() == 0)
+ {
+ throw std::runtime_error ("Start state not initialized, can not interpolate ");
+ }
+ if(endState_.configuration_.rows() == 0)
+ {
+ throw std::runtime_error ("End state not initialized, can not interpolate ");
+ }
+
+ if(problemSolver_->paths().size() <= pathId)
+ {
+ throw std::runtime_error ("No path computed, cannot interpolate ");
+ }
+
+<<<<<<< HEAD
+ hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_,problemSolver_->paths()[pathId]);
+=======
+ hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = hpp::rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_,problemSolver_->paths()[pathId]);
+>>>>>>> rb-rrt
+ lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),timestep,robustnessTreshold);
+
+ hpp::floatSeqSeq *res;
+ res = new hpp::floatSeqSeq ();
+
+ res->length ((_CORBA_ULong)lastStatesComputed_.size ());
+ std::size_t i=0;
+ std::size_t id = 0;
+ for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin(); cit != lastStatesComputed_.end(); ++cit, ++id)
+ {
+ std::cout << "ID " << id;
+ cit->print();
+ const core::Configuration_t config = cit->configuration_;
+ _CORBA_ULong size = (_CORBA_ULong) config.size ();
+ double* dofArray = hpp::floatSeq::allocbuf(size);
+ hpp::floatSeq floats (size, size, dofArray, true);
+ //convert the config in dofseq
+ for (model::size_type j=0 ; j < config.size() ; ++j) {
+ dofArray[j] = config [j];
+ }
+ (*res) [(_CORBA_ULong)i] = floats;
+ ++i;
+ }
+ return res;
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+ void RbprmBuilder::interpolateBetweenStates(double state1, double state2) throw (hpp::Error)
+ {
+ try
+ {
+ std::size_t s1(state1), s2(state2);
+ if(lastStatesComputed_.size () < s1 || lastStatesComputed_.size () < s2 )
+ {
+ throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s1) + ", " + std::string(""+s2));
+ }
+ //create helper
+// /interpolation::LimbRRTHelper helper(fullBody_, problemSolver_->problem());
+ core::PathVectorPtr_t path = interpolation::interpolateStates(fullBody_,problemSolver_->problem(),
+ lastStatesComputed_.begin()+s1,lastStatesComputed_.begin()+s2);
+ problemSolver_->addPath(path);
+ problemSolver_->robot()->setDimensionExtraConfigSpace(problemSolver_->robot()->extraConfigSpace().dimension()+1);
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+ void RbprmBuilder::saveComputedStates(const char* outfilename) throw (hpp::Error)
+ {
+ std::stringstream ss;
+ ss << lastStatesComputed_.size()-2 << "\n";
+ std::vector<rbprm::State>::iterator cit = lastStatesComputed_.begin()+1;
+ int i = 0;
+ ss << i++ << " ";
+ cit->print(ss);
+ for(std::vector<rbprm::State>::iterator cit2 = lastStatesComputed_.begin()+2;
+ cit2 != lastStatesComputed_.end()-1; ++cit2, ++cit, ++i)
+ {
+ cit2->robustness = stability::IsStable(this->fullBody_, *cit2);
+ ss << i<< " ";
+ cit2->print(ss,*cit);
+ }
+ std::ofstream outfile;
+ outfile.open(outfilename);
+ if (outfile.is_open())
+ {
+ outfile << ss.rdbuf();
+ outfile.close();
+ }
+ else
+ {
+ std::string error("Can not open outfile " + std::string(outfilename));
+ throw Error(error.c_str());
+ }
+ }
+
+ void RbprmBuilder::saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error)
+ {
+ try
+ {
+ std::string limbName(limbname);
+ std::ofstream fout;
+ fout.open(filepath, std::fstream::out | std::fstream::app);
+ rbprm::saveLimbInfoAndDatabase(fullBody_->GetLimbs().at(limbName),fout);
+ //sampling::saveLimbDatabase(fullBody_->GetLimbs().at(limbName)->sampleContainer_,fout);
+ fout.close();
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+ hpp::floatSeqSeq* RbprmBuilder::getOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error)
+ {
+ try
+ {
+ model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
+ model::Configuration_t save = fullBody_->device_->currentConfiguration();
+ fullBody_->device_->currentConfiguration(config);
+ fullBody_->device_->computeForwardKinematics();
+ const std::map<std::size_t, fcl::CollisionObject*>& boxes =
+ fullBody_->GetLimbs().at(std::string(limbName))->sampleContainer_.boxes_;
+ const double resolution = fullBody_->GetLimbs().at(std::string(limbName))->sampleContainer_.resolution_;
+ std::size_t i =0;
+ hpp::floatSeqSeq *res;
+ res = new hpp::floatSeqSeq ();
+ res->length ((_CORBA_ULong)boxes.size ());
+ for(std::map<std::size_t, fcl::CollisionObject*>::const_iterator cit = boxes.begin();
+ cit != boxes.end();++cit,++i)
+ {
+ fcl::Vec3f position = (*cit->second).getTranslation();
+ _CORBA_ULong size = (_CORBA_ULong) 4;
+ double* dofArray = hpp::floatSeq::allocbuf(size);
+ hpp::floatSeq floats (size, size, dofArray, true);
+ //convert the config in dofseq
+ for (model::size_type j=0 ; j < 3 ; ++j) {
+ dofArray[j] = position[j];
+ }
+ dofArray[3] = resolution;
+ (*res) [(_CORBA_ULong)i] = floats;
+ }
+ fullBody_->device_->currentConfiguration(save);
+ fullBody_->device_->computeForwardKinematics();
+ return res;
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+ hpp::floatSeq* RbprmBuilder::getOctreeBox(const char* limbName, double octreeNodeId) throw (hpp::Error)
+ {
+ try
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ long ocId ((long)octreeNodeId);
+ const T_Limb& limbs = fullBody_->GetLimbs();
+ T_Limb::const_iterator lit = limbs.find(std::string(limbName));
+ if(lit == limbs.end())
+ {
+ std::string err("No limb " + std::string(limbName) + "was defined for robot" + fullBody_->device_->name());
+ throw Error (err.c_str());
+ }
+ const std::map<std::size_t, fcl::CollisionObject*>& boxes =
+ fullBody_->GetLimbs().at(std::string(limbName))->sampleContainer_.boxes_;
+ std::map<std::size_t, fcl::CollisionObject*>::const_iterator cit = boxes.find(ocId);
+ if(cit == boxes.end())
+ {
+ std::stringstream ss; ss << ocId;
+ std::string err("No octree node with id " + ss.str() + "was defined for robot" + fullBody_->device_->name());
+ throw Error (err.c_str());
+ }
+ const fcl::CollisionObject* box = cit->second;
+ const fcl::Vec3f& pos = box->getTransform().getTranslation();
+ hpp::floatSeq* dofArray = new hpp::floatSeq();
+ dofArray->length(4);
+ for(std::size_t i=0; i< 3; ++i)
+ {
+ (*dofArray)[(_CORBA_ULong)i] = pos[i];
+ }
+ (*dofArray)[(_CORBA_ULong)3] = fullBody_->GetLimbs().at(std::string(limbName))->sampleContainer_.resolution_;
+ return dofArray;
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+ hpp::floatSeq* RbprmBuilder::getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error)
+ {
+ try{
+ model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
+ model::Configuration_t save = fullBody_->device_->currentConfiguration();
+ fullBody_->device_->currentConfiguration(config);
+ fullBody_->device_->computeForwardKinematics();
+ const hpp::rbprm::RbPrmLimbPtr_t limb =fullBody_->GetLimbs().at(std::string(limbName));
+ const fcl::Transform3f transform = limb->octreeRoot();
+ const fcl::Quaternion3f& quat = transform.getQuatRotation();
+ const fcl::Vec3f& position = transform.getTranslation();
+ hpp::floatSeq *dofArray;
+ dofArray = new hpp::floatSeq();
+ dofArray->length(_CORBA_ULong(7));
+ for(std::size_t i=0; i< 3; i++)
+ (*dofArray)[(_CORBA_ULong)i] = position [i];
+ for(std::size_t i=0; i< 4; i++)
+ (*dofArray)[(_CORBA_ULong)i+3] = quat [i];
+ fullBody_->device_->currentConfiguration(save);
+ fullBody_->device_->computeForwardKinematics();
+ return dofArray;
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+ CORBA::Short RbprmBuilder::isConfigBalanced(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs, double robustnessTreshold) throw (hpp::Error)
+ {
+ try{
+ rbprm::State testedState;
+ model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
+ model::Configuration_t save = fullBody_->device_->currentConfiguration();
+ std::vector<std::string> names = stringConversion(contactLimbs);
+ for(std::vector<std::string>::const_iterator cit = names.begin(); cit != names.end();++cit)
+ {
+ std::cout << "name " << * cit << std::endl;
+ const hpp::rbprm::RbPrmLimbPtr_t limb =fullBody_->GetLimbs().at(std::string(*cit));
+ testedState.contacts_[*cit] = true;
+ testedState.contactPositions_[*cit] = limb->effector_->currentTransformation().getTranslation();
+ testedState.contactRotation_[*cit] = limb->effector_->currentTransformation().getRotation();
+ // normal given by effector normal
+ const fcl::Vec3f normal = limb->effector_->currentTransformation().getRotation() * limb->normal_;
+ testedState.contactNormals_[*cit] = normal;
+ testedState.configuration_ = config;
+ ++testedState.nbContacts;
+ }
+ fullBody_->device_->currentConfiguration(save);
+ fullBody_->device_->computeForwardKinematics();
+ if (stability::IsStable(fullBody_, testedState) >= robustnessTreshold)
+ {
+ return (CORBA::Short)(1);
+ }
+ else
+ {
+ return (CORBA::Short)(0);
+ }
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+
+ void RbprmBuilder::runSampleAnalysis(const char* analysis, double isstatic) throw (hpp::Error)
+ {
+ try
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ std::string eval(analysis);
+ if (eval == "all")
+ {
+ for(sampling::T_evaluate::const_iterator analysisit = analysisFactory_->evaluate_.begin();
+ analysisit != analysisFactory_->evaluate_.end(); ++ analysisit)
+ {
+ for(T_Limb::const_iterator cit = fullBody_->GetLimbs().begin(); cit !=fullBody_->GetLimbs().end();++cit)
+ {
+ sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (cit->second->sampleContainer_);
+ sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
+ }
+ }
+ }
+ else
+ {
+ sampling::T_evaluate::const_iterator analysisit = analysisFactory_->evaluate_.find(std::string(eval));
+ if(analysisit == analysisFactory_->evaluate_.end())
+ {
+ std::string err("No analysis named " + eval + "was defined for analyzing database sample");
+ throw Error (err.c_str());
+ }
+ for(T_Limb::const_iterator cit = fullBody_->GetLimbs().begin(); cit !=fullBody_->GetLimbs().end();++cit)
+ {
+ sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (cit->second->sampleContainer_);
+ sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
+ }
+ }
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+ hpp::floatSeq* RbprmBuilder::runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) throw (hpp::Error)
+ {
+ try
+ {
+ rbprm::sampling::ValueBound bounds;
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ T_Limb::const_iterator lit = fullBody_->GetLimbs().find(std::string(limbname));
+ if(lit == fullBody_->GetLimbs().end())
+ {
+ std::string err("No limb " + std::string(limbname) + "was defined for robot" + fullBody_->device_->name());
+ throw Error (err.c_str());
+ }
+ std::string eval(analysis);
+ if (eval == "all")
+ {
+ for(sampling::T_evaluate::const_iterator analysisit = analysisFactory_->evaluate_.begin();
+ analysisit != analysisFactory_->evaluate_.end(); ++ analysisit)
+ {
+ sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_);
+ sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
+ bounds = sampleDB.valueBounds_[analysisit->first];
+ }
+ }
+ else
+ {
+ sampling::T_evaluate::const_iterator analysisit = analysisFactory_->evaluate_.find(std::string(eval));
+ if(analysisit == analysisFactory_->evaluate_.end())
+ {
+ std::string err("No analysis named " + eval + "was defined for analyzing database sample");
+ throw Error (err.c_str());
+ }
+ sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_);
+ sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
+ bounds = sampleDB.valueBounds_[analysisit->first];
+ }
+ hpp::floatSeq* dofArray = new hpp::floatSeq();
+ dofArray->length(2);
+ (*dofArray)[0] = bounds.first;
+ (*dofArray)[1] = bounds.second;
+ return dofArray;
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
+ void RbprmBuilder::SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver)
+ {
+ problemSolver_ = problemSolver;
+ bindShooter_.problemSolver_ = problemSolver;
+ //bind shooter creator to hide problem as a parameter and respect signature
+
+ // add rbprmshooter
+ problemSolver->add<core::ConfigurationShooterBuilder_t>("RbprmShooter",
+ boost::bind(&BindShooter::create, boost::ref(bindShooter_), _1));
+ problemSolver->add<core::PathValidationBuilder_t>("RbprmPathValidation",
+ boost::bind(&BindShooter::createPathValidation, boost::ref(bindShooter_), _1, _2));
+ }
+
+ } // namespace impl
+ } // namespace rbprm
+} // namespace hpp