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

added api to plot sample data

parent b12a1324
......@@ -128,6 +128,18 @@ module hpp
/// and the selected sample
floatSeq getSamplePosition(in string sampleName, in unsigned short sampleId) raises (Error);
/// Get the end effector position of a given limb configuration
/// \param limbName name of the limb from which to retrieve a sample
/// \return number of samples generated for the limb
unsigned short getNumSamples(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
/// \param sampleId id of the considered sample
/// \return the value computed for the given sample and analytics
double getSampleValue(in string limbName, in string valueName, in unsigned short sampleId) raises (Error);
/// Generate all possible contact in a given configuration
/// \param dofArray initial configuration of the robot
/// \param direction desired direction of motion for the robot
......@@ -227,13 +239,15 @@ module hpp
short isConfigBalanced(in floatSeq config, in Names_t contacts, in double robustnessTreshold) raises (Error);
/// run and store an analysis on all limb databases
/// \param analysis name of the analysis existing
/// \param analysis name of the analysis existing if analysis ="all",
/// all tests are run.
/// \param isstatic 1 is becomes new static value of database, 0 otherwise
void runSampleAnalysis(in string analysis, in double isstatic) raises (Error);
/// run and store an analysis on a limb database
/// \param limbname name of the limb to perform the analysis to
/// \param analysis name of the analysis existing
/// \param analysis name of the analysis existing if analysis ="all",
/// all tests are run.
/// \param isstatic 1 is becomes new static value of database, 0 otherwise
void runLimbSampleAnalysis(in string limbname, in string analysis, in double isstatic) raises (Error);
......
......@@ -120,10 +120,16 @@ INSTALL(
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/client.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmbuilder.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmfullbody.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/problem_solver.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/problem_solver.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
)
INSTALL(
FILES
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/__init__.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools//generateROMs.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/plot_analytics.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/tools
)
# Stand alone corba server
ADD_EXECUTABLE (hpp-rbprm-server hpp-rbprm-corba.cc)
TARGET_LINK_LIBRARIES (hpp-rbprm-server ${LIBRARY_NAME} hpp-rbprm)
......
......@@ -154,7 +154,21 @@ class FullBody (object):
# \param direction a 3d vector specifying the desired direction of motion
def getContactSamplesIds(self, name, configuration, direction):
return self.client.rbprm.rbprm.getContactSamplesIds(name, configuration, direction)
## 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 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
# \param sampleId id of the considered sample
def getSampleValue(self, limbName, valueName, sampleId):
return self.client.rbprm.rbprm.getSampleValue(limbName, valueName, sampleId)
## Initialize the first configuration of the path discretization
# with a balanced configuration for the interpolation problem;
#
......@@ -162,7 +176,7 @@ class FullBody (object):
# \param contacts the array of limbs in contact
def setStartState(self, configuration, contacts):
return self.client.rbprm.rbprm.setStartState(configuration, contacts)
## Initialize the last configuration of the path discretization
# with a balanced configuration for the interpolation problem;
#
......
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()
## 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):
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)
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
......@@ -37,6 +37,7 @@ namespace hpp {
, romLoaded_(false)
, fullBodyLoaded_(false)
, bindShooter_()
, analysisFactory_(0)
{
// NOTHING
}
......@@ -129,7 +130,9 @@ namespace hpp {
hppDout (error, exc.what ());
throw hpp::Error (exc.what ());
}
fullBodyLoaded_ = true;
fullBodyLoaded_ = true;
if(!analysisFactory_)
analysisFactory_ = new sampling::AnalysisFactory(fullBody_);
}
hpp::floatSeq* RbprmBuilder::getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error)
......@@ -188,6 +191,43 @@ namespace hpp {
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 lit->second->sampleContainer_.samples_.size();
}
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)
{
......@@ -700,17 +740,34 @@ namespace hpp {
{
try
{
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
std::string eval(analysis);
sampling::T_evaluate::const_iterator analysisit = analysisFactory_.evaluate_.find(std::string(eval));
if(analysisit == analysisFactory_.evaluate_.end())
if (eval == "all")
{
std::string err("No analysis named " + eval + "was defined for analyzing database sample");
throw Error (err.c_str());
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);
}
}
}
for(T_Limb::const_iterator cit = fullBody_->GetLimbs().begin(); cit !=fullBody_->GetLimbs().end();++cit)
else
{
sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (cit->second->sampleContainer_);
sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
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)
......@@ -723,21 +780,35 @@ namespace hpp {
{
try
{
std::string eval(analysis);
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());
}
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());
}
sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_);
sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
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);
}
}
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);
}
}
catch(std::runtime_error& e)
{
......
......@@ -103,6 +103,8 @@ 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 double getSampleValue(const char* limb, const char* valueName, unsigned short sampleId) throw (hpp::Error);
virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
const hpp::floatSeq& direction) throw (hpp::Error);
......@@ -143,7 +145,7 @@ namespace hpp {
rbprm::State startState_;
rbprm::State endState_;
std::vector<rbprm::State> lastStatesComputed_;
sampling::AnalysisFactory analysisFactory_;
sampling::AnalysisFactory* analysisFactory_;
}; // class RobotBuilder
} // namespace impl
} // namespace manipulation
......
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