Unverified Commit 76f9e3a7 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub

Merge pull request #3 from jmirabel/multithread

Use HPP thread safe API
parents 10d019bc 940a9d32
[plugins] [plugins]
hppcorbaserverplugin.so=false
hppmanipulationplugin.so=false # Plugin for hpp-manipulation-server
hppmanipulationwidgetsplugin.so=true hppmanipulationwidgetsplugin.so=true
hppmonitoringplugin.so=true hppmonitoringplugin.so=true
# Plugin for hppcorbaserver
hppwidgetsplugin.so=false hppwidgetsplugin.so=false
libhppmanipulationplugin.so=false
libhppmonitoringplugin.so=false
...@@ -67,7 +67,6 @@ namespace hpp { ...@@ -67,7 +67,6 @@ namespace hpp {
toolBar_->addAction (autoBuildGraph); toolBar_->addAction (autoBuildGraph);
connect(autoBuildGraph, SIGNAL(triggered()), SLOT(autoBuildGraph())); connect(autoBuildGraph, SIGNAL(triggered()), SLOT(autoBuildGraph()));
connect(&main->worker(), SIGNAL(done(int)), SLOT(handleWorkerDone(int)));
main->registerSlot("autoBuildGraph", this); main->registerSlot("autoBuildGraph", this);
main->registerSlot("drawRobotContacts", this); main->registerSlot("drawRobotContacts", this);
main->registerSlot("drawEnvironmentsContacts", this); main->registerSlot("drawEnvironmentsContacts", this);
...@@ -81,16 +80,17 @@ namespace hpp { ...@@ -81,16 +80,17 @@ namespace hpp {
void HppManipulationWidgetsPlugin::loadRobotModel(gepetto::gui::DialogLoadRobot::RobotDefinition rd) void HppManipulationWidgetsPlugin::loadRobotModel(gepetto::gui::DialogLoadRobot::RobotDefinition rd)
{ {
try { try {
client ()->robot ()->getCurrentConfig(); hpp::floatSeq_var q = client ()->robot ()->getCurrentConfig();
(void)q;
} catch (hpp::Error const& e) { } catch (hpp::Error const& e) {
client ()->robot ()->createRobot (gepetto::gui::Traits<QString>::to_corba("composite").in()); client ()->robot ()->createRobot (to_corba("composite").in());
} }
hpp_->robot ()->insertRobotModel (gepetto::gui::Traits<QString>::to_corba(rd.robotName_).in(), hpp_->robot ()->insertRobotModel (to_corba(rd.robotName_).in(),
gepetto::gui::Traits<QString>::to_corba(rd.rootJointType_).in(), to_corba(rd.rootJointType_).in(),
gepetto::gui::Traits<QString>::to_corba(rd.package_).in(), to_corba(rd.package_).in(),
gepetto::gui::Traits<QString>::to_corba(rd.modelName_).in(), to_corba(rd.modelName_).in(),
gepetto::gui::Traits<QString>::to_corba(rd.urdfSuf_).in(), to_corba(rd.urdfSuf_).in(),
gepetto::gui::Traits<QString>::to_corba(rd.srdfSuf_).in()); to_corba(rd.srdfSuf_).in());
// This is already done in requestRefresh // This is already done in requestRefresh
// jointTreeWidget_->reload(); // jointTreeWidget_->reload();
gepetto::gui::MainWindow::instance()->requestRefresh(); gepetto::gui::MainWindow::instance()->requestRefresh();
...@@ -101,15 +101,16 @@ namespace hpp { ...@@ -101,15 +101,16 @@ namespace hpp {
void HppManipulationWidgetsPlugin::loadEnvironmentModel(gepetto::gui::DialogLoadEnvironment::EnvironmentDefinition ed) void HppManipulationWidgetsPlugin::loadEnvironmentModel(gepetto::gui::DialogLoadEnvironment::EnvironmentDefinition ed)
{ {
try { try {
client ()->robot ()->getCurrentConfig(); hpp::floatSeq_var q = client ()->robot ()->getCurrentConfig();
(void)q;
} catch (hpp::Error const& e) { } catch (hpp::Error const& e) {
client ()->robot ()->createRobot (gepetto::gui::Traits<QString>::to_corba("composite").in()); client ()->robot ()->createRobot (to_corba("composite").in());
} }
hpp_->robot ()-> loadEnvironmentModel(gepetto::gui::Traits<QString>::to_corba(ed.package_).in(), hpp_->robot ()-> loadEnvironmentModel(to_corba(ed.package_).in(),
gepetto::gui::Traits<QString>::to_corba(ed.urdfFilename_).in(), to_corba(ed.urdfFilename_).in(),
gepetto::gui::Traits<QString>::to_corba(ed.urdfSuf_).in(), to_corba(ed.urdfSuf_).in(),
gepetto::gui::Traits<QString>::to_corba(ed.srdfSuf_).in(), to_corba(ed.srdfSuf_).in(),
gepetto::gui::Traits<QString>::to_corba(ed.name_ + "/").in()); to_corba(ed.name_ + "/").in());
HppWidgetsPlugin::computeObjectPosition(); HppWidgetsPlugin::computeObjectPosition();
gepetto::gui::MainWindow::instance()->requestRefresh(); gepetto::gui::MainWindow::instance()->requestRefresh();
emit logSuccess ("Environment " + ed.name_ + " loaded"); emit logSuccess ("Environment " + ed.name_ + " loaded");
...@@ -506,13 +507,6 @@ namespace hpp { ...@@ -506,13 +507,6 @@ namespace hpp {
graphBuilder_->show(); graphBuilder_->show();
} }
void HppManipulationWidgetsPlugin::handleWorkerDone(int id)
{
if (id == lastId_) {
gepetto::gui::MainWindow::instance()->logJobDone(id, "Graph is build");
}
}
void HppManipulationWidgetsPlugin::loadConstraintWidget() void HppManipulationWidgetsPlugin::loadConstraintWidget()
{ {
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance(); gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
......
...@@ -99,7 +99,6 @@ namespace hpp { ...@@ -99,7 +99,6 @@ namespace hpp {
private slots: private slots:
/// Construct all the corba vars and create the graph. /// Construct all the corba vars and create the graph.
void buildGraph(); void buildGraph();
void handleWorkerDone(int id);
protected: protected:
virtual void loadConstraintWidget(); virtual void loadConstraintWidget();
...@@ -135,8 +134,6 @@ namespace hpp { ...@@ -135,8 +134,6 @@ namespace hpp {
QToolBar *toolBar_; QToolBar *toolBar_;
QTabWidget *tw_; QTabWidget *tw_;
QDialog* graphBuilder_; QDialog* graphBuilder_;
int lastId_;
}; };
} // namespace gui } // namespace gui
} // namespace hpp } // namespace hpp
......
...@@ -75,8 +75,8 @@ namespace hpp { ...@@ -75,8 +75,8 @@ namespace hpp {
void ConfigurationListWidget::onSaveClicked () void ConfigurationListWidget::onSaveClicked ()
{ {
hpp::floatSeq_var c = plugin_->client()->robot()->getCurrentConfig (); hpp::floatSeq const* c = plugin_->getCurrentConfig ();
list()->addItem(makeItem(name()->text(), c.in())); list()->addItem(makeItem(name()->text(), *c));
name()->setText(basename_ + QString::number(count_)); name()->setText(basename_ + QString::number(count_));
count_++; count_++;
} }
...@@ -85,8 +85,7 @@ namespace hpp { ...@@ -85,8 +85,7 @@ namespace hpp {
{ {
if (current != 0) { if (current != 0) {
const hpp::floatSeq& c = getConfig(current); const hpp::floatSeq& c = getConfig(current);
plugin_->client()->robot()->setCurrentConfig (c); plugin_->setCurrentConfig (c);
main_->requestApplyCurrentConfiguration();
if (previous_ && if (previous_ &&
previous_ != current->listWidget()) { previous_ != current->listWidget()) {
previous_->clearSelection(); previous_->clearSelection();
......
...@@ -9,7 +9,6 @@ ...@@ -9,7 +9,6 @@
#include <QDockWidget> #include <QDockWidget>
#include <omniORB4/CORBA.h> #include <omniORB4/CORBA.h>
#include "gepetto/gui/meta.hh"
#include "gepetto/gui/mainwindow.hh" #include "gepetto/gui/mainwindow.hh"
#include "hppwidgetsplugin/hppwidgetsplugin.hh" #include "hppwidgetsplugin/hppwidgetsplugin.hh"
......
...@@ -30,7 +30,6 @@ ...@@ -30,7 +30,6 @@
#include "hppwidgetsplugin/joint-action.hh" #include "hppwidgetsplugin/joint-action.hh"
#include "hppwidgetsplugin/roadmap.hh" #include "hppwidgetsplugin/roadmap.hh"
#include <gepetto/gui/meta.hh>
using CORBA::ULong; using CORBA::ULong;
...@@ -151,8 +150,16 @@ namespace hpp { ...@@ -151,8 +150,16 @@ namespace hpp {
main->registerSlot("getCurrentPath", pathPlayer_); main->registerSlot("getCurrentPath", pathPlayer_);
main->registerSlot("getHppIIOPurl", this); main->registerSlot("getHppIIOPurl", this);
main->registerSlot("getHppContext", this); main->registerSlot("getHppContext", this);
main->registerSlot("getCurrentConfig", this);
main->registerSlot("setCurrentConfig", this);
main->registerSlot("getSelectedJoint", jointTreeWidget_); main->registerSlot("getSelectedJoint", jointTreeWidget_);
main->registerSignal(SIGNAL(appliedConfigAtParam(int,double)), pathPlayer_); main->registerSignal(SIGNAL(appliedConfigAtParam(int,double)), pathPlayer_);
QAction* action = main->findChild<QAction*>("actionFetch_configuration");
if (action != NULL) connect (action, SIGNAL(triggered()), SLOT(fetchConfiguration()));
else qDebug () << "Action actionFetch_configuration not found";
action = main->findChild<QAction*>("actionSend_configuration");
if (action != NULL) connect (action, SIGNAL(triggered()), SLOT(sendConfiguration()));
else qDebug () << "Action actionSend_configuration not found";
ActionSearchBar* asb = main->actionSearchBar(); ActionSearchBar* asb = main->actionSearchBar();
JointAction* a; JointAction* a;
...@@ -174,12 +181,12 @@ namespace hpp { ...@@ -174,12 +181,12 @@ namespace hpp {
void HppWidgetsPlugin::loadRobotModel(gepetto::gui::DialogLoadRobot::RobotDefinition rd) void HppWidgetsPlugin::loadRobotModel(gepetto::gui::DialogLoadRobot::RobotDefinition rd)
{ {
client()->robot()->loadRobotModel( client()->robot()->loadRobotModel(
gepetto::gui::Traits<QString>::to_corba(rd.robotName_ ).in(), to_corba(rd.robotName_ ).in(),
gepetto::gui::Traits<QString>::to_corba(rd.rootJointType_).in(), to_corba(rd.rootJointType_).in(),
gepetto::gui::Traits<QString>::to_corba(rd.package_ ).in(), to_corba(rd.package_ ).in(),
gepetto::gui::Traits<QString>::to_corba(rd.modelName_ ).in(), to_corba(rd.modelName_ ).in(),
gepetto::gui::Traits<QString>::to_corba(rd.urdfSuf_ ).in(), to_corba(rd.urdfSuf_ ).in(),
gepetto::gui::Traits<QString>::to_corba(rd.srdfSuf_ ).in()); to_corba(rd.srdfSuf_ ).in());
// This is already done in requestRefresh // This is already done in requestRefresh
// jointTreeWidget_->reload(); // jointTreeWidget_->reload();
gepetto::gui::MainWindow::instance()->requestRefresh(); gepetto::gui::MainWindow::instance()->requestRefresh();
...@@ -191,9 +198,9 @@ namespace hpp { ...@@ -191,9 +198,9 @@ namespace hpp {
{ {
QString prefix = ed.envName_ + "/"; QString prefix = ed.envName_ + "/";
client()->obstacle()->loadObstacleModel( client()->obstacle()->loadObstacleModel(
gepetto::gui::Traits<QString>::to_corba(ed.package_ ).in(), to_corba(ed.package_ ).in(),
gepetto::gui::Traits<QString>::to_corba(ed.urdfFilename_).in(), to_corba(ed.urdfFilename_).in(),
gepetto::gui::Traits<QString>::to_corba(prefix ).in()); to_corba(prefix ).in());
computeObjectPosition (); computeObjectPosition ();
gepetto::gui::MainWindow::instance()->requestRefresh(); gepetto::gui::MainWindow::instance()->requestRefresh();
emit logSuccess ("Environment " + ed.name_ + " loaded"); emit logSuccess ("Environment " + ed.name_ + " loaded");
...@@ -206,14 +213,36 @@ namespace hpp { ...@@ -206,14 +213,36 @@ namespace hpp {
return itj->prefix + itj->bodyNames[0]; return itj->prefix + itj->bodyNames[0];
} }
void HppWidgetsPlugin::fetchConfiguration ()
{
hpp::floatSeq_var c = client()->robot ()->getCurrentConfig ();
setCurrentConfig (c.in());
}
void HppWidgetsPlugin::sendConfiguration ()
{
client()->robot ()->setCurrentConfig (config_);
}
void HppWidgetsPlugin::setCurrentConfig (const hpp::floatSeq& q)
{
config_ = q;
MainWindow::instance()->requestApplyCurrentConfiguration();
}
hpp::floatSeq const* HppWidgetsPlugin::getCurrentConfig () const
{
return &config_;
}
bool HppWidgetsPlugin::corbaException(int jobId, const CORBA::Exception &excep) const bool HppWidgetsPlugin::corbaException(int jobId, const CORBA::Exception &excep) const
{ {
try { try {
const hpp::Error& error = dynamic_cast <const hpp::Error&> (excep); const hpp::Error& error = dynamic_cast <const hpp::Error&> (excep);
emit logJobFailed(jobId, QString (error.msg)); emit logJobFailed(jobId, QString (error.msg));
return true; return true;
} catch (const std::exception& exp) { } catch (const std::bad_cast&) {
qDebug () << exp.what(); // dynamic_cast failed.
} }
return false; return false;
} }
...@@ -259,6 +288,8 @@ namespace hpp { ...@@ -259,6 +288,8 @@ namespace hpp {
void HppWidgetsPlugin::prepareApplyConfiguration() void HppWidgetsPlugin::prepareApplyConfiguration()
{ {
bodyNames_.clear(); bodyNames_.clear();
config_ .length (client()->robot()->getConfigSize());
velocity_.length (client()->robot()->getNumberDof ());
gepetto::gui::MainWindow * main = gepetto::gui::MainWindow::instance (); gepetto::gui::MainWindow * main = gepetto::gui::MainWindow::instance ();
CORBA::ULong size = 0; const CORBA::ULong sall = 100; CORBA::ULong size = 0; const CORBA::ULong sall = 100;
linkNames_.length(sall); linkNames_.length(sall);
...@@ -298,29 +329,30 @@ namespace hpp { ...@@ -298,29 +329,30 @@ namespace hpp {
"interface) and you did not refresh this GUI. " "interface) and you did not refresh this GUI. "
"Use the refresh button \"Tools\" menu."); "Use the refresh button \"Tools\" menu.");
} }
// Something smarter could be done here. hpp::TransformSeq_var Ts = client()->robot ()->getLinksPosition (config_, linkNames_);
// For instance, the joint tree item could know the NodePtr_t of their bodies.
hpp::TransformSeq_var Ts = client()->robot ()->getLinksPosition (linkNames_);
fromHPP (Ts, bodyConfs_); fromHPP (Ts, bodyConfs_);
main->osg()->applyConfigurations (bodyNames_, bodyConfs_); main->osg()->applyConfigurations (bodyNames_, bodyConfs_);
hpp::floatSeq_var c = client()->robot ()->getCurrentConfig ();
for (JointMap::iterator ite = jointMap_.begin (); for (JointMap::iterator ite = jointMap_.begin ();
ite != jointMap_.end (); ite++) { ite != jointMap_.end (); ite++) {
if (!ite->item) continue; if (!ite->item) continue;
if (ite->item->config().length() > 0) { if (ite->item->config().length() > 0) {
ite->item->updateFromRobotConfig (c.in()); ite->item->updateFromRobotConfig (config_);
} }
} }
OsgConfiguration_t T; Ts = client()->robot()->getJointsPosition(config_, jointFrames_);
for (std::list<std::string>::const_iterator it = jointFrames_.begin (); fromHPP (Ts, bodyConfs_);
it != jointFrames_.end (); ++it) { main->osg()->applyConfigurations (jointGroupNames_, bodyConfs_);
std::string n = escapeJointName(*it);
hpp::Transform__var t = client()->robot()->getJointPosition(it->c_str()); if (comFrames_.size() > 0) {
fromHPP(t, T); static bool firstTime = true;
main->osg()->applyConfiguration (n, T); if (firstTime) {
main->log ("COM frames is not thread safe. Use with care.");
firstTime = false;
} }
OsgConfiguration_t T;
T.quat.set(0,0,0,1); T.quat.set(0,0,0,1);
client()->robot()->setCurrentConfig (config_);
for (std::list<std::string>::const_iterator it = comFrames_.begin (); for (std::list<std::string>::const_iterator it = comFrames_.begin ();
it != comFrames_.end (); ++it) { it != comFrames_.end (); ++it) {
std::string n = "com_" + escapeJointName(*it); std::string n = "com_" + escapeJointName(*it);
...@@ -328,24 +360,23 @@ namespace hpp { ...@@ -328,24 +360,23 @@ namespace hpp {
fromHPP (t, T.position); fromHPP (t, T.position);
main->osg()->applyConfiguration (n, T); main->osg()->applyConfiguration (n, T);
} }
}
main->osg()->refresh(); main->osg()->refresh();
} }
void HppWidgetsPlugin::configurationValidation() void HppWidgetsPlugin::configurationValidation()
{ {
hpp::floatSeq_var q = client()->robot()->getCurrentConfig (); bool valid = false;
bool bb = false;
CORBA::Boolean_out b = bb;
CORBA::String_var report; CORBA::String_var report;
try { try {
client()->robot()->isConfigValid (q.in(), b, report); client()->robot()->isConfigValid (config_, valid, report);
} catch (const hpp::Error& e) { } catch (const hpp::Error& e) {
emit logFailure(QString (e.msg)); emit logFailure(QString (e.msg));
return; return;
} }
static QRegExp collision ("Collision between object (.*) and (.*)"); static QRegExp collision ("Collision between object (.*) and (.*)");
QStringList col; QStringList col;
if (!bb) { if (!valid) {
if (collision.exactMatch(QString::fromLocal8Bit(report))) { if (collision.exactMatch(QString::fromLocal8Bit(report))) {
CORBA::String_var robotName = client ()->robot()->getRobotName(); CORBA::String_var robotName = client ()->robot()->getRobotName();
size_t pos = strlen(robotName) + 1; size_t pos = strlen(robotName) + 1;
...@@ -391,8 +422,7 @@ namespace hpp { ...@@ -391,8 +422,7 @@ namespace hpp {
if (type == "node") { if (type == "node") {
try { try {
hpp::floatSeq_var q = hpp_->problem()->node(n); hpp::floatSeq_var q = hpp_->problem()->node(n);
hpp_->robot()->setCurrentConfig(q.in()); setCurrentConfig(q.in());
gepetto::gui::MainWindow::instance()->requestApplyCurrentConfiguration();
} catch (const hpp::Error& e) { } catch (const hpp::Error& e) {
emit logFailure(QString::fromLocal8Bit(e.msg)); emit logFailure(QString::fromLocal8Bit(e.msg));
} }
...@@ -406,8 +436,7 @@ namespace hpp { ...@@ -406,8 +436,7 @@ namespace hpp {
hpp::floatSeq_var times; hpp::floatSeq_var times;
hpp::floatSeqSeq_var waypoints = hpp_->problem()->getWaypoints((CORBA::UShort)pid, times.out()); hpp::floatSeqSeq_var waypoints = hpp_->problem()->getWaypoints((CORBA::UShort)pid, times.out());
if (n < waypoints->length()) { if (n < waypoints->length()) {
hpp_->robot()->setCurrentConfig(waypoints[n]); setCurrentConfig(waypoints[n]);
MainWindow::instance()->requestApplyCurrentConfiguration();
} }
} }
} }
...@@ -569,7 +598,9 @@ namespace hpp { ...@@ -569,7 +598,9 @@ namespace hpp {
hpp::Transform__var t = client()->robot()->getJointPosition (jn.c_str()); hpp::Transform__var t = client()->robot()->getJointPosition (jn.c_str());
OsgConfiguration_t p; OsgConfiguration_t p;
fromHPP(t, p); fromHPP(t, p);
jointFrames_.push_back(jn); jointFrames_.length(jointFrames_.length()+1);
jointFrames_[jointFrames_.length() -1] = jn.c_str();
jointGroupNames_.push_back(target);
main->osg()->applyConfiguration (target, p); main->osg()->applyConfiguration (target, p);
main->osg()->refresh(); main->osg()->refresh();
} }
......
...@@ -22,6 +22,9 @@ namespace hpp { ...@@ -22,6 +22,9 @@ namespace hpp {
class Roadmap; class Roadmap;
class ConstraintWidget; class ConstraintWidget;
inline CORBA::String_var to_corba(const QString& s)
{ return (const char*)s.toLocal8Bit().data(); }
/// Plugin that add a lot of features to work with hpp. /// Plugin that add a lot of features to work with hpp.
class HppWidgetsPlugin : public QObject, public gepetto::gui::PluginInterface, class HppWidgetsPlugin : public QObject, public gepetto::gui::PluginInterface,
public gepetto::gui::ModelInterface, public gepetto::gui::CorbaInterface public gepetto::gui::ModelInterface, public gepetto::gui::CorbaInterface
...@@ -85,6 +88,27 @@ namespace hpp { ...@@ -85,6 +88,27 @@ namespace hpp {
/// \todo this should be changed because there can be several body per /// \todo this should be changed because there can be several body per
/// joints now. /// joints now.
std::string getBodyFromJoint (const std::string& jointName) const; std::string getBodyFromJoint (const std::string& jointName) const;
const hpp::floatSeq& currentConfig () const
{
return config_;
}
hpp::floatSeq& currentConfig ()
{
return config_;
}
const hpp::floatSeq& currentVelocity () const
{
return velocity_;
}
hpp::floatSeq& currentVelocity ()
{
return velocity_;
}
signals: signals:
void configurationValidationStatus (bool valid); void configurationValidationStatus (bool valid);
void configurationValidationStatus (QStringList collision); void configurationValidationStatus (QStringList collision);
...@@ -106,6 +130,16 @@ signals: ...@@ -106,6 +130,16 @@ signals:
/// Apply the current configuration of the robot. /// Apply the current configuration of the robot.
void applyCurrentConfiguration (); void applyCurrentConfiguration ();
void setCurrentConfig (const hpp::floatSeq& q);
hpp::floatSeq const* getCurrentConfig () const;
/// Set internal configuration from HPP current config.
void fetchConfiguration ();
/// Set HPP configuration to internal current configuration
void sendConfiguration ();
/// Build a list of bodies in collision. /// Build a list of bodies in collision.
void configurationValidation (); void configurationValidation ();
...@@ -193,13 +227,16 @@ signals: ...@@ -193,13 +227,16 @@ signals:
JointTreeWidget* jointTreeWidget_; JointTreeWidget* jointTreeWidget_;
ConstraintWidget* constraintWidget_; ConstraintWidget* constraintWidget_;
JointMap jointMap_; JointMap jointMap_;
std::list <std::string> jointFrames_; hpp::Names_t jointFrames_;
std::list <std::string> comFrames_; std::list <std::string> comFrames_;
hpp::floatSeq config_, velocity_;
// Cache variables // Cache variables
hpp::Names_t linkNames_; hpp::Names_t linkNames_;
std::vector<std::string> bodyNames_; std::vector<std::string> bodyNames_;
std::vector<graphics::Configuration> bodyConfs_; std::vector<graphics::Configuration> bodyConfs_;
std::vector<std::string> jointGroupNames_;
}; };
} // namespace gui } // namespace gui
} // namespace hpp } // namespace hpp
......
...@@ -26,23 +26,27 @@ using CORBA::ULong; ...@@ -26,23 +26,27 @@ using CORBA::ULong;
namespace hpp { namespace hpp {
namespace gui { namespace gui {
using CORBA::ULong;
const int JointTreeItem::IndexRole = Qt::UserRole + 1; const int JointTreeItem::IndexRole = Qt::UserRole + 1;
const int JointTreeItem::LowerBoundRole = Qt::UserRole + 2; const int JointTreeItem::LowerBoundRole = Qt::UserRole + 2;
const int JointTreeItem::UpperBoundRole = Qt::UserRole + 3; const int JointTreeItem::UpperBoundRole = Qt::UserRole + 3;
const int JointTreeItem::NumberDofRole = Qt::UserRole + 4;
const int JointTreeItem::TypeRole = Qt::UserRole + 10; const int JointTreeItem::TypeRole = Qt::UserRole + 10;
JointTreeItem::JointTreeItem(const char* name, JointTreeItem::JointTreeItem(const char* name,
const std::size_t& idxQ, const ULong& idxQ,
const ULong& idxV,
const hpp::floatSeq &q, const hpp::floatSeq &q,
const hpp::floatSeq &b, const hpp::floatSeq &b,
const unsigned int nbDof, const NodesPtr_t& nodes) const ULong& nbDof, const NodesPtr_t& nodes)
: QStandardItem (QString (name)), name_ (name), idxQ_ (idxQ), nodes_ (nodes), value_ () : QStandardItem (QString (name)), name_ (name)
, idxQ_ (idxQ), idxV_ (idxV)
, nq_ (q.length()), nv_ (nbDof)
, nodes_ (nodes), value_ ()
{ {
setData((int)-1, IndexRole); setData((int)-1, IndexRole);
setData(nbDof, NumberDofRole);
setData(SkipType, TypeRole); setData(SkipType, TypeRole);
for (size_t i = 0; i < q.length(); ++i)</