Commit c7cb9691 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

JointTreeItem uses the rank in configuration.

parent 88ac6fc3
...@@ -272,8 +272,7 @@ namespace hpp { ...@@ -272,8 +272,7 @@ namespace hpp {
} }
if (!ite->item) continue; if (!ite->item) continue;
if (ite->item->config().length() > 0) { if (ite->item->config().length() > 0) {
hpp::floatSeq_var c = client()->robot ()->getJointConfig (ite->name.c_str()); ite->item->updateFromRobotConfig (c.in());
ite->item->updateConfig (c.in());
} }
} }
for (std::list<std::string>::const_iterator it = jointFrames_.begin (); for (std::list<std::string>::const_iterator it = jointFrames_.begin ();
......
...@@ -32,10 +32,12 @@ namespace hpp { ...@@ -32,10 +32,12 @@ namespace hpp {
const int JointTreeItem::NumberDofRole = Qt::UserRole + 4; 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, const hpp::floatSeq &q, JointTreeItem::JointTreeItem(const char* name,
const std::size_t& idxQ,
const hpp::floatSeq &q,
const hpp::floatSeq &b, const hpp::floatSeq &b,
const unsigned int nbDof, const NodesPtr_t& nodes) const unsigned int nbDof, const NodesPtr_t& nodes)
: QStandardItem (QString (name)), name_ (name), nodes_ (nodes), value_ () : QStandardItem (QString (name)), name_ (name), idxQ_ (idxQ), nodes_ (nodes), value_ ()
{ {
setData((int)-1, IndexRole); setData((int)-1, IndexRole);
setData(nbDof, NumberDofRole); setData(nbDof, NumberDofRole);
...@@ -70,7 +72,7 @@ namespace hpp { ...@@ -70,7 +72,7 @@ namespace hpp {
b[2*(ULong) i] = value_[(ULong) i][0]->data(LowerBoundRole).toFloat(); b[2*(ULong) i] = value_[(ULong) i][0]->data(LowerBoundRole).toFloat();
b[2*(ULong) i+1] = value_[(ULong) i][0]->data(UpperBoundRole).toFloat(); b[2*(ULong) i+1] = value_[(ULong) i][0]->data(UpperBoundRole).toFloat();
} }
return new JointTreeItem (name_.c_str(), q, b, data (NumberDofRole).toInt(), nodes_); return new JointTreeItem (name_.c_str(), idxQ_, q, b, data (NumberDofRole).toInt(), nodes_);
} }
hpp::floatSeq JointTreeItem::config() const hpp::floatSeq JointTreeItem::config() const
...@@ -100,6 +102,13 @@ namespace hpp { ...@@ -100,6 +102,13 @@ namespace hpp {
value_[i][0]->setData(c[i], Qt::EditRole); value_[i][0]->setData(c[i], Qt::EditRole);
} }
void JointTreeItem::updateFromRobotConfig (const hpp::floatSeq& rc)
{
assert (idxQ_ + value_.size() <= rc.length());
for (int i = 0; i < value_.size(); ++i)
value_[i][0]->setData(rc[(int)idxQ_ + i], Qt::EditRole);
}
void JointTreeItem::updateBounds(const hpp::floatSeq& b) void JointTreeItem::updateBounds(const hpp::floatSeq& b)
{ {
assert ((int)b.length() == 2*value_.size()); assert ((int)b.length() == 2*value_.size());
......
...@@ -95,7 +95,9 @@ namespace hpp { ...@@ -95,7 +95,9 @@ namespace hpp {
BoundType BoundType
}; };
JointTreeItem (const char* name, const hpp::floatSeq& q, JointTreeItem (const char* name,
const std::size_t& idxQ,
const hpp::floatSeq& q,
const hpp::floatSeq& b, const hpp::floatSeq& b,
const unsigned int nbDof, const unsigned int nbDof,
const NodesPtr_t& node); const NodesPtr_t& node);
...@@ -120,6 +122,8 @@ namespace hpp { ...@@ -120,6 +122,8 @@ namespace hpp {
void updateConfig (const hpp::floatSeq &c); void updateConfig (const hpp::floatSeq &c);
void updateFromRobotConfig (const hpp::floatSeq &c);
void updateTypeRole (); void updateTypeRole ();
void setupActions(HppWidgetsPlugin* plugin); void setupActions(HppWidgetsPlugin* plugin);
...@@ -130,6 +134,7 @@ namespace hpp { ...@@ -130,6 +134,7 @@ namespace hpp {
typedef QList<QStandardItem*> StandardItemList; typedef QList<QStandardItem*> StandardItemList;
std::string name_; std::string name_;
std::size_t idxQ_;
NodesPtr_t nodes_; NodesPtr_t nodes_;
QVector<StandardItemList> value_; QVector<StandardItemList> value_;
QList<QAction*> actions_; QList<QAction*> actions_;
......
...@@ -131,7 +131,7 @@ namespace hpp { ...@@ -131,7 +131,7 @@ namespace hpp {
} }
} }
JointTreeItem* JointTreeWidget::buildJointTreeItem(const char* name) JointTreeItem* JointTreeWidget::buildJointTreeItem(const char* name, std::size_t& rkConfig)
{ {
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance(); gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
HppWidgetsPlugin::JointElement& je = plugin_->jointMap() [name]; HppWidgetsPlugin::JointElement& je = plugin_->jointMap() [name];
...@@ -141,17 +141,19 @@ namespace hpp { ...@@ -141,17 +141,19 @@ namespace hpp {
// TODO I do not remember why this is important... // TODO I do not remember why this is important...
if (!nodes[i]) nodes[i] = main->osg ()->getGroup(je.bodyNames[i]); if (!nodes[i]) nodes[i] = main->osg ()->getGroup(je.bodyNames[i]);
} }
CORBA::Long nbDof = plugin_->client()->robot ()->getJointNumberDof (name); CORBA::Long nbCfg = plugin_->client()->robot ()->getJointConfigSize (name);
CORBA::Long nbDof = plugin_->client()->robot ()->getJointNumberDof (name);
JointTreeItem* j; JointTreeItem* j;
if (nbDof > 0) { if (nbDof > 0) {
hpp::floatSeq_var c = plugin_->client()->robot ()->getJointConfig (name); hpp::floatSeq_var c = plugin_->client()->robot ()->getJointConfig (name);
hpp::floatSeq_var b = plugin_->client()->robot ()->getJointBounds (name); hpp::floatSeq_var b = plugin_->client()->robot ()->getJointBounds (name);
j = new JointTreeItem (name, c.in(), b.in(), nbDof, nodes); j = new JointTreeItem (name, rkConfig, c.in(), b.in(), nbDof, nodes);
} else { } else {
j = new JointTreeItem (name, hpp::floatSeq(), hpp::floatSeq(), nbDof, nodes); j = new JointTreeItem (name, rkConfig, hpp::floatSeq(), hpp::floatSeq(), nbDof, nodes);
} }
je.item = j; je.item = j;
j->setupActions(plugin_); j->setupActions(plugin_);
rkConfig += nbCfg;
return j; return j;
} }
...@@ -219,10 +221,11 @@ namespace hpp { ...@@ -219,10 +221,11 @@ namespace hpp {
hpp::Names_t_var joints = plugin_->client()->robot()->getAllJointNames (); hpp::Names_t_var joints = plugin_->client()->robot()->getAllJointNames ();
typedef std::map<std::string, JointTreeItem*> JointTreeItemMap_t; typedef std::map<std::string, JointTreeItem*> JointTreeItemMap_t;
JointTreeItemMap_t items; JointTreeItemMap_t items;
std::size_t rk = 0;
for (size_t i = 0; i < joints->length (); ++i) { for (size_t i = 0; i < joints->length (); ++i) {
const char* jname = joints[(ULong) i]; const char* jname = joints[(ULong) i];
std::string bjn (joints[0]); std::string bjn (joints[0]);
items[jname] = buildJointTreeItem(jname); items[jname] = buildJointTreeItem(jname, rk);
} }
for (JointTreeItemMap_t::const_iterator _jti = items.begin(); for (JointTreeItemMap_t::const_iterator _jti = items.begin();
_jti != items.end(); ++_jti) { _jti != items.end(); ++_jti) {
......
...@@ -71,7 +71,7 @@ signals: ...@@ -71,7 +71,7 @@ signals:
/// Reset the tree. /// Reset the tree.
void reset (); void reset ();
JointTreeItem* buildJointTreeItem (const char* name); JointTreeItem* buildJointTreeItem (const char* name, std::size_t& rkConfig);
HppWidgetsPlugin* plugin_; HppWidgetsPlugin* plugin_;
::Ui::JointTreeWidget* ui_; ::Ui::JointTreeWidget* ui_;
......
Markdown is supported
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