Commit c2927c08 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

early interface for comRRT

parent fa51596e
......@@ -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.
......
......@@ -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
......
......@@ -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
......
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()
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()
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()
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])