diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 99b06058135bbd180f0c4e564250ca5077c855f8..8401ff703c32f2a25d60552d14395526e84af7dc 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -260,7 +260,7 @@ module hpp
/// \param state1 index of first state.
/// \param state2 index of second state.
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
- void interpolateBetweenStates(in double state1, in double state2, in unsigned short numOptimizations) raises (Error);
+ void limbRRT(in double state1, in double state2, in unsigned short numOptimizations) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
@@ -272,7 +272,19 @@ module hpp
/// \param state2 index of second state.
/// \param path index of the path considered for the generation
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
- void interpolateBetweenStatesFromPath(in double state1, in double state2, in unsigned short path, in unsigned short numOptimizations) raises (Error);
+ void limbRRTFromRootPath(in double state1, in double state2, in unsigned short path, in unsigned short numOptimizations) raises (Error);
+
+ /// Provided a path has already been computed and interpolated, generate a continuous path
+ /// between two indicated states. The states do not need to be consecutive, but increasing in Id.
+ /// Will fail if the index of the states do not exist
+ /// The path of the COM of thr robot and limbs not considered by the contact transitions between
+ /// two states is assumed to be already computed, and registered in the solver under the id specified by the user.
+ /// It must be valid in the sense of the active PathValidation.
+ /// \param state1 index of first state.
+ /// \param state2 index of second state.
+ /// \param comPath index of the path considered of the com path
+ /// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
+ void comRRT(in double state1, in double state2, in unsigned short comPath, in unsigned short numOptimizations) raises (Error);
/// Saves the last computed states by the function interpolate in a filename.
/// Raises an error if interpolate has not been called, or the file could not be opened.
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index cf26ea64e5158657dbb0d0d55c6abd11dd6e4e9a..1add5f7a9cea14807e8b673197cca8f8038cb2f5 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -305,8 +305,8 @@ class FullBody (object):
# \param index of first state.
# \param index of second state.
# \param numOptim Number of iterations of the shortcut algorithm to apply between each states
- def interpolateBetweenStates(self, state1, state2, numOptim = 10):
- return self.client.rbprm.rbprm.interpolateBetweenStates(state1, state2, numOptim)
+ def limbRRT(self, state1, state2, numOptim = 10):
+ return self.client.rbprm.rbprm.limbRRT(state1, state2, numOptim)
## Provided a path has already been computed and interpolated, generate a continuous path
# between two indicated states. The states do not need to be consecutive, but increasing in Id.
@@ -318,8 +318,22 @@ class FullBody (object):
# \param state2 index of second state.
# \param path index of the path considered for the generation
# \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
- def interpolateBetweenStatesFromPath(self, state1, state2, path, numOptim = 10):
- return self.client.rbprm.rbprm.interpolateBetweenStatesFromPath(state1, state2, path, numOptim)
+ def limbRRTFromRootPath(self, state1, state2, path, numOptim = 10):
+ return self.client.rbprm.rbprm.limbRRTFromRootPath(state1, state2, path, numOptim)
+
+
+ ## Provided a center of mass trajectory has already been computed and interpolated, generate a continuous full body path
+ # between two indicated states. The states do not need to be consecutive, but increasing in Id.
+ # Will fail if the index of the states do not exist
+ # The path of the COM between
+ # two states is assumed to be already computed, and registered in the solver under the id specified by the user.
+ # It must be valid in the sense of the active PathValidation.
+ # \param state1 index of first state.
+ # \param state2 index of second state.
+ # \param path index of the com path considered for the generation
+ # \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
+ def comRRT(self, state1, state2, path, numOptim = 10):
+ return self.client.rbprm.rbprm.comRRT(state1, state2, path, numOptim)
## Given start and goal states
diff --git a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
index d35b4ae825f7b1c3c6b8b44db17a40d304c8416d..a4c3b9751dd1f60bc3e761ffefb6f51de6525712 100644
--- a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
+++ b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
@@ -5,7 +5,10 @@ def __get_com(robot, config):
save = robot.getCurrentConfig()
robot.setCurrentConfig(config)
com = robot.getCenterOfMass()
- com = config[0:3] #assimilate root and com
+ print 'debut', com
+ print 'debut c ', config[0:3]
+ print 'z_diff', config[2] - com[2]
+ #~ com = config[0:3] #assimilate root and com
robot.setCurrentConfig(save)
return com
@@ -20,7 +23,7 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, red
cones = None
if(computeCones):
cones = [fullBody.getContactCone(state_id)[0]]
- if(len(p) > 1):
+ if(len(p) > 2):
cones.append(fullBody.getContactIntermediateCone(state_id)[0])
if(len(p) > len(cones)):
cones.append(fullBody.getContactCone(state_id+1)[0])
@@ -29,7 +32,6 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, red
def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True):
var_final, params = gen_trajectory(fullBody, states, state_id, computeCones, mu , reduce_ineq)
p, N = fullBody.computeContactPoints(state_id)
- print var_final
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
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
deleted file mode 100644
index 840571b88fc4891fb2994085f0b84dfb1944f6cc..0000000000000000000000000000000000000000
--- a/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.BACKUP.4492.py
+++ /dev/null
@@ -1,139 +0,0 @@
-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
deleted file mode 100644
index a2783bc87a06aaa8f602ae7e6f8503d13b4a9b2a..0000000000000000000000000000000000000000
--- a/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.BASE.4492.py
+++ /dev/null
@@ -1,102 +0,0 @@
-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
deleted file mode 100644
index eb8d49f799c4ec4964cc0f58e2f6660ea01ad042..0000000000000000000000000000000000000000
--- a/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.LOCAL.4492.py
+++ /dev/null
@@ -1,135 +0,0 @@
-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
deleted file mode 100644
index 2965bdaadd6eeb974c4ffdcad3e94cee243ff092..0000000000000000000000000000000000000000
--- a/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.REMOTE.4492.py
+++ /dev/null
@@ -1,136 +0,0 @@
-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
deleted file mode 100644
index 840571b88fc4891fb2994085f0b84dfb1944f6cc..0000000000000000000000000000000000000000
--- a/src/hpp/corbaserver/rbprm/tools/plot_analytics.py.orig
+++ /dev/null
@@ -1,139 +0,0 @@
-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 2a466d1ab659e90246ec5c61ab1fe33fb3ed3cd1..4071a356d2a3b8c62d95c0fca02a1cf35b12d41f 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -22,8 +22,8 @@
#include "hpp/rbprm/rbprm-device.hh"
#include "hpp/rbprm/rbprm-validation.hh"
#include "hpp/rbprm/interpolation/rbprm-path-interpolation.hh"
-#include "hpp/rbprm/interpolation/limb-rrt-helper.hh"
-#include "hpp/rbprm/interpolation/limb-rrt-path.hh"
+#include "hpp/rbprm/interpolation/limb-rrt.hh"
+#include "hpp/rbprm/interpolation/com-rrt.hh"
#include "hpp/rbprm/stability/stability.hh"
#include "hpp/rbprm/sampling/sample-db.hh"
#include "hpp/model/urdf/util.hh"
@@ -709,7 +709,8 @@ namespace hpp {
model::ConfigurationIn_t q1,
model::ConfigurationIn_t q2)
{
- return StraightPath::create(device, q1, q2, (*problem->distance()) (q1, q2));
+ // TODO DT
+ return StraightPath::create(device, q1, q2, 0.1);
}
model::Configuration_t addRotation(CIT_Configuration& pit, const model::value_type& u,
@@ -735,6 +736,11 @@ namespace hpp {
++pit;
for(;pit != positions.end()-1; ++pit, u+=size_step)
{
+ std::cout << "position added \n" << ((*pit).head(3)) << std::endl;
+ if((*pit)(0) > 0.552368)
+ {
+ std::cout << "pos devant \n" << (*pit)(0) << std::endl;
+ }
current = addRotation(pit, u, q1, q2, ref);
res->appendPath(makePath(device,problem, previous, current));
previous = current;
@@ -919,7 +925,7 @@ namespace hpp {
ps->addPath(resPath);
}
- void RbprmBuilder::interpolateBetweenStates(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error)
+ void RbprmBuilder::limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error)
{
try
{
@@ -929,8 +935,8 @@ namespace hpp {
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::PathPtr_t path = interpolation::interpolateStates<rbprm::interpolation::LimbRRTPath, CIT_State>(fullBody_,problemSolver_->problem(),
+// /interpolation::TimeConstraintHelper helper(fullBody_, problemSolver_->problem());
+ core::PathPtr_t path = interpolation::limbRRT(fullBody_,problemSolver_->problem(),
lastStatesComputed_.begin()+s1,lastStatesComputed_.begin()+s2, numOptimizations);
AddPath(path,problemSolver_);
}
@@ -940,7 +946,7 @@ namespace hpp {
}
}
- void RbprmBuilder::interpolateBetweenStatesFromPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error)
+ void RbprmBuilder::limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error)
{
try
{
@@ -954,7 +960,7 @@ namespace hpp {
{
throw std::runtime_error ("No path computed, cannot interpolate ");
}
- core::PathPtr_t path = interpolation::interpolateStates<rbprm::interpolation::LimbRRTPath>(fullBody_,problemSolver_->problem(), problemSolver_->paths()[pathId],
+ core::PathPtr_t path = interpolation::limbRRTFromPath(fullBody_,problemSolver_->problem(), problemSolver_->paths()[pathId],
lastStatesComputedTime_.begin()+s1,lastStatesComputedTime_.begin()+s2, numOptimizations);
AddPath(path,problemSolver_);
}
@@ -964,6 +970,32 @@ namespace hpp {
}
}
+ void RbprmBuilder::comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error)
+ {
+ try
+ {
+ std::size_t s1((std::size_t)state1), s2((std::size_t)state2);
+// temp
+assert(s2 == s1 +1);
+ 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));
+ }
+ unsigned int pathId = (unsigned int)(path);
+ if(problemSolver_->paths().size() <= pathId)
+ {
+ throw std::runtime_error ("No path computed, cannot interpolate ");
+ }
+ core::PathPtr_t path = interpolation::comRRT(fullBody_,problemSolver_->problem(), problemSolver_->paths()[pathId],
+ *(lastStatesComputed_.begin()+s1),*(lastStatesComputed_.begin()+s2), numOptimizations);
+ AddPath(path,problemSolver_);
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
void RbprmBuilder::saveComputedStates(const char* outfilename) throw (hpp::Error)
{
std::stringstream ss;
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 27d601b27f817ba29cd02549475ce2a20c413c9d..2e07ede06ea59f54bcf367122c22f248bd8bbbe8 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -146,8 +146,9 @@ namespace hpp {
virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId) throw (hpp::Error);
virtual void generateRootPath(const hpp::floatSeqSeq& rootPositions,
const hpp::floatSeq& q1, const hpp::floatSeq& q2) throw (hpp::Error);
- virtual void interpolateBetweenStates(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
- virtual void interpolateBetweenStatesFromPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
+ virtual void limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
+ virtual void limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
+ virtual void comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);