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