Commit 8824abb9 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

added plot values for octree nodes

parent fadece05
......@@ -133,6 +133,11 @@ module hpp
/// \return number of samples generated for the limb
unsigned short getNumSamples(in string limbName) raises (Error);
/// Get the number of octree nodes for a limb database
/// \param limbName name of the limb from which to retrieve octree number
/// \return ids of the nodes in the octree
floatSeq getOctreeNodeIds(in string limbName) raises (Error);
/// Get the sample value for a given analysis
/// \param limbName name of the limb from which to retrieve a sample
/// \param valueName name of the analytic measure desired
......@@ -154,7 +159,12 @@ module hpp
/// \return transformed configuration for which all possible contacts have been created
floatSeq getContactSamplesIds(in string name, in floatSeq dofArray, in floatSeq direction) raises (Error);
/// Specifies a subchain of the robot as a limb, which can be used to create contacts.
/// get Ids of limb in an octree cell
/// \param name name of the considered limb
/// \param octreeNodeId considered configuration of the robot
/// \return list of ids in the cell
floatSeq getSamplesIdsInOctreeNode(in string name, in double octreeNodeId) raises (Error);
/// A limb must consist in a simple kinematic chain, where every node has only one child
/// \param id user given name of the new limb
/// \param limb robot joint corresponding to the root of the limb (ex a shoulder or ankle joint)
......@@ -222,8 +232,13 @@ module hpp
/// \param limbname name of the considered limb
/// \param dofArray considered configuration of the robot
/// \return transformed configuration for which all possible contacts have been created
floatSeqSeq GetOctreeBoxes(in string limbname, in floatSeq dofArray) raises (Error);
floatSeqSeq getOctreeBoxes(in string limbname, in floatSeq dofArray) raises (Error);
/// returns the size and transforms of all boxes of the octree for a limb
/// \param limbname name of the considered limb
/// \param dofArray considered configuration of the robot
/// \return transformed configuration for which all possible contacts have been created
floatSeq getOctreeBox(in string limbname, in double sampleId) raises (Error);
/// returns octree transform for a given robot configuration
/// \param limbname name of the considered limb
......
......@@ -155,12 +155,26 @@ class FullBody (object):
def getContactSamplesIds(self, name, configuration, direction):
return self.client.rbprm.rbprm.getContactSamplesIds(name, configuration, direction)
## Retrieves the samples IDs In a given octree cell
#
# \param name id of the limb considered
# \param configuration the considered robot configuration
# \param direction a 3d vector specifying the desired direction of motion
def getSamplesIdsInOctreeNode(self, limbName, octreeNodeId):
return self.client.rbprm.rbprm.getSamplesIdsInOctreeNode(limbName, octreeNodeId)
## Get the number of samples generated for a limb
#
# \param limbName name of the limb from which to retrieve a sample
def getNumSamples(self, limbName):
return self.client.rbprm.rbprm.getNumSamples(limbName)
## Get the number of octreeNodes
#
# \param limbName name of the limb from which to retrieve a sample
def getOctreeNodeIds(self, limbName):
return self.client.rbprm.rbprm.getOctreeNodeIds(limbName)
## Get the sample value for a given analysis
#
# \param limbName name of the limb from which to retrieve a sample
......@@ -256,7 +270,7 @@ class FullBody (object):
# \param config initial configuration of the robot, used when created octree
# \param color of the octree boxes
def createOctreeBoxes(self, gui, winId, limbName, config, color = [1,1,1,0.3]):
boxes = self.client.rbprm.rbprm.GetOctreeBoxes(limbName, config)
boxes = self.client.rbprm.rbprm.getOctreeBoxes(limbName, config)
scene = "oct"+limbName
gui.createScene(scene)
resolution = boxes[0][3]
......@@ -269,6 +283,13 @@ class FullBody (object):
self.octrees[limbName] = scene
gui.addSceneToWindow(scene,winId)
gui.refresh()
## Create octree nodes representation for a given limb
#
# \param limbName name of the limb considered
# \param ocId of the octree box
def getOctreeBox(self, limbName, ocId):
return self.client.rbprm.rbprm.getOctreeBox(limbName, ocId)
## Draws robot configuration, along with octrees associated
#
......
......@@ -2,24 +2,76 @@ import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from matplotlib import cm
#~
#~ def randrange(n, vmin, vmax):
#~ return (vmax - vmin)*np.random.rand(n) + vmin
#~
#~ fig = plt.figure()
#~ ax = fig.add_subplot(111, projection='3d')
#~ n = 100
#~ for c, m, zl, zh in [('r', 'o', -50, -25), ('b', '^', -30, -5)]:
#~ xs = randrange(n, 23, 32)
#~ ys = randrange(n, 0, 100)
#~ zs = randrange(n, zl, zh)
#~ ax.scatter(xs, ys, zs, c=c, marker=m)
#~
#~ ax.set_xlabel('X Label')
#~ ax.set_ylabel('Y Label')
#~ ax.set_zlabel('Z Label')
#~
#~ plt.show()
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
#
......@@ -34,17 +86,17 @@ def plotValues(robot, valueName, limb):
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
numSamples = robot.getNumSamples(limb)
for i in range(0,numSamples):
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)
print g
vals.append([1-g,g,0])
#~ ax.scatter(pos[0], pos[1], pos[2], c=robot.getSampleValue(limb, valueName, i))
ax.scatter(xs, ys, zs, c=vals)
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')
ax.set_zlabel('Z Label')
plt.title(valueName)
plt.show()
......@@ -204,6 +204,26 @@ namespace hpp {
return 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(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] = 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();
......@@ -393,6 +413,43 @@ namespace hpp {
}
}
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 (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(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] = 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)
{
......@@ -631,7 +688,7 @@ namespace hpp {
}
}
hpp::floatSeqSeq* RbprmBuilder::GetOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error)
hpp::floatSeqSeq* RbprmBuilder::getOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error)
{
try
{
......@@ -670,6 +727,46 @@ namespace hpp {
}
}
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 (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{
......
......@@ -104,6 +104,7 @@ namespace hpp {
virtual hpp::floatSeq* getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error);
virtual hpp::floatSeq* getSamplePosition(const char* limb, unsigned short sampleId) throw (hpp::Error);
virtual CORBA::UShort getNumSamples(const char* limb) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeNodeIds(const char* limb) throw (hpp::Error);
virtual double getSampleValue(const char* limb, const char* valueName, unsigned short sampleId) throw (hpp::Error);
virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
......@@ -113,6 +114,9 @@ namespace hpp {
const hpp::floatSeq& configuration,
const hpp::floatSeq& direction) throw (hpp::Error);
virtual hpp::floatSeq* getSamplesIdsInOctreeNode(const char* limb,
double octreeNodeId) throw (hpp::Error);
virtual void 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);
virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName) throw (hpp::Error);
......@@ -123,7 +127,8 @@ namespace hpp {
virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) 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::floatSeqSeq* GetOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);
virtual hpp::floatSeqSeq* getOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold) throw (hpp::Error);
virtual void runSampleAnalysis(const char* analysis, double isstatic) throw (hpp::Error);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment