Skip to content
GitLab
Commit e32ac15a authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

First step toward thread safety.

parent 8349b722
Hide whitespace changes
Inline Side-by-side
plugins/hppwidgetsplugin/hppwidgetsplugin.cc
View file @ e32ac15a
......@@ -205,6 +205,12 @@ namespace hpp {
return itj->prefix + itj->bodyNames[0];
}
void HppWidgetsPlugin::fetchConfiguration ()
{
hpp::floatSeq_var c = client()->robot ()->getCurrentConfig ();
config_ = c.in();
}
bool HppWidgetsPlugin::corbaException(int jobId, const CORBA::Exception &excep) const
{
try {
......@@ -258,6 +264,8 @@ namespace hpp {
void HppWidgetsPlugin::prepareApplyConfiguration()
{
bodyNames_.clear();
config_ .length (client()->robot()->getConfigSize());
velocity_.length (client()->robot()->getNumberDof ());
gepetto::gui::MainWindow * main = gepetto::gui::MainWindow::instance ();
CORBA::ULong size = 0; const CORBA::ULong sall = 100;
linkNames_.length(sall);
......@@ -299,33 +307,40 @@ namespace hpp {
}
// Something smarter could be done here.
// For instance, the joint tree item could know the NodePtr_t of their bodies.
hpp::TransformSeq_var Ts = client()->robot ()->getLinksPosition (linkNames_);
hpp::TransformSeq_var Ts = client()->robot ()->getLinksPosition (config_, linkNames_);
fromHPP (Ts, bodyConfs_);
main->osg()->applyConfigurations (bodyNames_, bodyConfs_);
hpp::floatSeq_var c = client()->robot ()->getCurrentConfig ();
for (JointMap::iterator ite = jointMap_.begin ();
ite != jointMap_.end (); ite++) {
if (!ite->item) continue;
if (ite->item->config().length() > 0) {
ite->item->updateFromRobotConfig (c.in());
ite->item->updateFromRobotConfig (config_);
}
}
OsgConfiguration_t T;
for (std::list<std::string>::const_iterator it = jointFrames_.begin ();
it != jointFrames_.end (); ++it) {
std::string n = escapeJointName(*it);
hpp::Transform__var t = client()->robot()->getJointPosition(it->c_str());
fromHPP(t, T);
main->osg()->applyConfiguration (n, T);
}
T.quat.set(0,0,0,1);
for (std::list<std::string>::const_iterator it = comFrames_.begin ();
it != comFrames_.end (); ++it) {
std::string n = "com_" + escapeJointName(*it);
hpp::floatSeq_var t = client()->robot()->getPartialCom(it->c_str());
fromHPP (t, T.position);
main->osg()->applyConfiguration (n, T);
hpp::floatSeq_var c = client()->robot ()->getCurrentConfig ();
// for (std::list<std::string>::const_iterator it = jointFrames_.begin ();
// it != jointFrames_.end (); ++it) {
Ts = client()->robot()->getJointsPosition(config_, jointFrames_);
fromHPP (Ts, bodyConfs_);
main->osg()->applyConfigurations (jointGroupNames_, bodyConfs_);
if (comFrames_.size() > 0) {
static bool firstTime = true;
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);
client()->robot()->setCurrentConfig (config_);
for (std::list<std::string>::const_iterator it = comFrames_.begin ();
it != comFrames_.end (); ++it) {
std::string n = "com_" + escapeJointName(*it);
hpp::floatSeq_var t = client()->robot()->getPartialCom(it->c_str());
fromHPP (t, T.position);
main->osg()->applyConfiguration (n, T);
}
}
main->osg()->refresh();
}
......@@ -568,7 +583,9 @@ namespace hpp {
hpp::Transform__var t = client()->robot()->getJointPosition (jn.c_str());
OsgConfiguration_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()->refresh();
}
......
plugins/hppwidgetsplugin/hppwidgetsplugin.hh
View file @ e32ac15a
......@@ -88,6 +88,29 @@ namespace hpp {
/// \todo this should be changed because there can be several body per
/// joints now.
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_;
}
void fetchConfiguration ();
signals:
void configurationValidationStatus (bool valid);
void configurationValidationStatus (QStringList collision);
......@@ -196,13 +219,16 @@ signals:
JointTreeWidget* jointTreeWidget_;
ConstraintWidget* constraintWidget_;
JointMap jointMap_;
std::list <std::string> jointFrames_;
hpp::Names_t jointFrames_;
std::list <std::string> comFrames_;
hpp::floatSeq config_, velocity_;
// Cache variables
hpp::Names_t linkNames_;
std::vector<std::string> bodyNames_;
std::vector<graphics::Configuration> bodyConfs_;
std::vector<std::string> jointGroupNames_;
};
} // namespace gui
} // namespace hpp
......
plugins/hppwidgetsplugin/joint-tree-item.cc
View file @ e32ac15a
......@@ -26,23 +26,27 @@ using CORBA::ULong;
namespace hpp {
namespace gui {
using CORBA::ULong;
const int JointTreeItem::IndexRole = Qt::UserRole + 1;
const int JointTreeItem::LowerBoundRole = Qt::UserRole + 2;
const int JointTreeItem::UpperBoundRole = Qt::UserRole + 3;
const int JointTreeItem::NumberDofRole = Qt::UserRole + 4;
const int JointTreeItem::TypeRole = Qt::UserRole + 10;
JointTreeItem::JointTreeItem(const char* name,
const std::size_t& idxQ,
const ULong& idxQ,
const ULong& idxV,
const hpp::floatSeq &q,
const hpp::floatSeq &b,
const unsigned int nbDof, const NodesPtr_t& nodes)
: QStandardItem (QString (name)), name_ (name), idxQ_ (idxQ), nodes_ (nodes), value_ ()
const ULong& nbDof, const NodesPtr_t& nodes)
: QStandardItem (QString (name)), name_ (name)
, idxQ_ (idxQ), idxV_ (idxV)
, nq_ (q.length()), nv_ (nbDof)
, nodes_ (nodes), value_ ()
{
setData((int)-1, IndexRole);
setData(nbDof, NumberDofRole);
setData(SkipType, TypeRole);
for (size_t i = 0; i < q.length(); ++i) {
for (size_t i = 0; i < nq_; ++i) {
QStandardItem *joint = new QStandardItem;
QStandardItem *upper = new QStandardItem;
QStandardItem *lower = new QStandardItem;
......@@ -72,7 +76,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(), idxQ_, q, b, data (NumberDofRole).toInt(), nodes_);
return new JointTreeItem (name_.c_str(), idxQ_, idxV_, q, b, nv_, nodes_);
}
hpp::floatSeq JointTreeItem::config() const
......@@ -128,9 +132,9 @@ namespace hpp {
// planar and freeflyer joints
int threshold = value_.size();
// TODO SO3 joint fall in that case too whereas threshold should be 0 for them.
if (data(NumberDofRole).toInt() == 3 && value_.size() == 4 )
if (nv_ == 3 && nq_ == 4 )
threshold = 2;
else if (data(NumberDofRole).toInt() == 6 && value_.size() == 7 )
else if (nv_ == 6 && nq_ == 7 )
threshold = 3;
for (int i = 0; i < value_.size(); ++i) {
float lo = value_[i][1]->data (Qt::EditRole).toFloat();
......@@ -205,33 +209,16 @@ namespace hpp {
switch (type) {
case JointTreeItem::SkipType:
return 0;
case JointTreeItem::IntegratorType: {
assert (ji);
IntegratorWheel* wheel =
new IntegratorWheel (Qt::Horizontal, plugin_, parent, main_,
ji->name(),
ji->data(JointTreeItem::NumberDofRole).toInt(),
std::min (
index.data(JointTreeItem::IndexRole).toInt(),
ji->data(JointTreeItem::NumberDofRole).toInt()-1));
return wheel;
}
case JointTreeItem::BoundedValueType: {
assert (ji);
hpp::floatSeq* q = new hpp::floatSeq;
hpp::floatSeq cfg = ji->config();
q->length(cfg.length());
*q = cfg;
const QModelIndex& lower = index.sibling(index.row(), 1);
const QModelIndex& upper = index.sibling(index.row(), 2);
float lo = lower.data(Qt::EditRole).toFloat(),
up = upper.data(Qt::EditRole).toFloat();
SliderBoundedJoint* slider =
new SliderBoundedJoint (Qt::Horizontal, plugin_, parent,
main_, ji->name(), q, index.data(JointTreeItem::IndexRole).toInt(),
lo, up);
return slider;
}
case JointTreeItem::IntegratorType:
assert (ji);
return new IntegratorWheel (Qt::Horizontal, plugin_, parent, main_,
ji, std::min ( (ULong)index.data(JointTreeItem::IndexRole).toInt(),
ji->numberDof()-1)
);
case JointTreeItem::BoundedValueType:
assert (ji);
return new SliderBoundedJoint (Qt::Horizontal, plugin_, parent,
main_, ji, index.data(JointTreeItem::IndexRole).toInt());
case JointTreeItem::UnboundedValueType:
case JointTreeItem::BoundType: {
QDoubleSpinBox* spinbox = new QDoubleSpinBox (parent);
......@@ -323,17 +310,19 @@ namespace hpp {
}
IntegratorWheel::IntegratorWheel(Qt::Orientation o, HppWidgetsPlugin *plugin, QWidget *parent,
gepetto::gui::MainWindow *main, std::string jointName,
int nbDof, int index)
: QSlider (o, parent), rate_ (100), main_ (main), plugin_ (plugin), jointName_ (jointName),
bound_ (100), maxVelocity_ (0.1),
currentValue_ (0), dq_ (new hpp::floatSeq), nbDof_ (nbDof), index_ (index)
gepetto::gui::MainWindow *main, JointTreeItem const* item, int index)
: QSlider (o, parent), rate_ (100), main_ (main), plugin_ (plugin),
item_ (item), bound_ (100), maxVelocity_ (0.1),
index_ (index)
{
setMinimum(-bound_);
setMaximum(bound_);
q_.length (item_->configSize());
dq_.length (item_->numberDof());
for (ULong i = 0; i < q_.length(); ++i)
q_ [ i] = plugin_->currentConfig()[item_->rankInConfig()+i];
for (ULong i = 0; i < dq_.length(); ++i) dq_[ i] = 0;
setValue (0);
dq_->length (nbDof_);
for (size_t i = 0; i < dq_->length(); ++i) dq_[(ULong) i] = 0;
connect(this, SIGNAL (sliderReleased()), this, SLOT (reset()));
connect(this, SIGNAL (sliderMoved(int)), this, SLOT (updateIntegrator(int)));
timerId_ = startTimer(rate_);
......@@ -342,9 +331,11 @@ namespace hpp {
void IntegratorWheel::timerEvent(QTimerEvent *)
{
killTimer(timerId_);
if (currentValue_ != 0) {
dq_[index_] = currentValue_;
plugin_->client()->robot ()->jointIntegrate (jointName_.c_str(), dq_.in());
if (dq_[index_] != 0) {
hpp::floatSeq_var q = plugin_->client()->robot ()->jointIntegrate (q_, item_->name().c_str(), dq_, true);
q_ = q.in();
for (ULong i = 0; i < q_.length(); ++i)
plugin_->currentConfig()[item_->rankInConfig()+i] = q_ [ i];
main_->requestApplyCurrentConfiguration();
}
timerId_ = startTimer(rate_);
......@@ -352,36 +343,40 @@ namespace hpp {
void IntegratorWheel::reset()
{
currentValue_ = 0;
dq_[index_] = 0;
setValue(0);
}
void IntegratorWheel::updateIntegrator(int value)
{
currentValue_ = maxVelocity_ * (double)value / (double)bound_;
dq_[index_] = maxVelocity_ * (double)value / (double)bound_;
}
SliderBoundedJoint::SliderBoundedJoint(Qt::Orientation orientation, HppWidgetsPlugin *plugin, QWidget *parent,
gepetto::gui::MainWindow *main, std::string jointName,
hpp::floatSeq *q, int index, double min, double max)
: QSlider (orientation, parent), main_ (main), plugin_ (plugin), jointName_ (jointName),
q_ (q), index_ (index), m_ (min), M_ (max)
gepetto::gui::MainWindow *main, JointTreeItem const* item, int index)
: QSlider (orientation, parent), main_ (main), plugin_ (plugin),
item_ (item), index_ (index)
{
value_ = item_->config()[index];
hpp::floatSeq bounds = item_->bounds();
m_ = bounds[2*index ];
M_ = bounds[2*index+1];
setMinimum(0);
setMaximum(100);
setValue ((int)(100*(q_[index_] - m_)/(M_ - m_)));
setValue ((int)(100*(value_ - m_)/(M_ - m_)));
connect (this, SIGNAL (sliderMoved(int)), this, SLOT (updateConfig(int)));
}
double SliderBoundedJoint::getValue()
{
return q_[index_];
return value_;
}
void SliderBoundedJoint::updateConfig(int value)
{
q_[index_] = m_ + (double)(value - 0) * (M_ - m_) / (double)100;
plugin_->client()->robot()->setJointConfig (jointName_.c_str(), q_.in());
value_ = m_ + (double)(value - 0) * (M_ - m_) / (double)100;
plugin_->currentConfig()[item_->rankInConfig()+index_] = value_;
main_->requestApplyCurrentConfiguration();
}
} // namespace gui
......
plugins/hppwidgetsplugin/joint-tree-item.hh
View file @ e32ac15a
......@@ -22,67 +22,14 @@ class QPushButton;
namespace hpp {
namespace gui {
class IntegratorWheel : public QSlider
{
Q_OBJECT
public:
IntegratorWheel (Qt::Orientation o, HppWidgetsPlugin* plugin, QWidget *parent,
gepetto::gui::MainWindow *main, std::string jointName,
int nbDof, int index);
protected:
void timerEvent(QTimerEvent *);
protected slots:
void reset ();
void updateIntegrator (int value);
private:
int rate_; // in millisecond
int timerId_;
gepetto::gui::MainWindow* main_;
HppWidgetsPlugin* plugin_;
std::string jointName_;
const int bound_;
const double maxVelocity_;
double currentValue_;
hpp::floatSeq_var dq_;
int nbDof_, index_;
};
class SliderBoundedJoint : public QSlider
{
Q_OBJECT
public:
SliderBoundedJoint (Qt::Orientation orientation, HppWidgetsPlugin* plugin, QWidget* parent,
gepetto::gui::MainWindow *main, std::string jointName,
hpp::floatSeq* q, int index, double min, double max);
double getValue ();
private slots:
void updateConfig (int value);
private:
gepetto::gui::MainWindow* main_;
HppWidgetsPlugin* plugin_;
std::string jointName_;
hpp::floatSeq_var q_;
int index_;
double m_, M_;
};
class JointTreeItem : public QStandardItem
{
public:
typedef CORBA::ULong ULong;
typedef graphics::NodePtr_t NodePtr_t;
typedef std::vector<NodePtr_t> NodesPtr_t;
static const int IndexRole ;
static const int NumberDofRole ;
static const int LowerBoundRole;
static const int UpperBoundRole;
static const int TypeRole ;
......@@ -96,10 +43,11 @@ namespace hpp {
};
JointTreeItem (const char* name,
const std::size_t& idxQ,
const ULong& idxQ,
const ULong& idxV,
const hpp::floatSeq& q,
const hpp::floatSeq& b,
const unsigned int nbDof,
const ULong& nbDof,
const NodesPtr_t& node);
~JointTreeItem ();
......@@ -118,6 +66,14 @@ namespace hpp {
hpp::floatSeq bounds () const;
ULong rankInConfig () const { return idxQ_; }
ULong rankInVelocity () const { return idxV_; }
ULong configSize () const { return nq_; }
ULong numberDof () const { return nv_; }
void updateBounds (const hpp::floatSeq &b);
void updateConfig (const hpp::floatSeq &c);
......@@ -134,12 +90,62 @@ namespace hpp {
typedef QList<QStandardItem*> StandardItemList;
std::string name_;
std::size_t idxQ_;
ULong idxQ_, idxV_, nq_, nv_;
NodesPtr_t nodes_;
QVector<StandardItemList> value_;
QList<QAction*> actions_;
};
class IntegratorWheel : public QSlider
{
Q_OBJECT
public:
IntegratorWheel (Qt::Orientation o, HppWidgetsPlugin* plugin, QWidget *parent,
gepetto::gui::MainWindow *main, JointTreeItem const* item, int index);
protected:
void timerEvent(QTimerEvent *);
protected slots:
void reset ();
void updateIntegrator (int value);
private:
int rate_; // in millisecond
int timerId_;
gepetto::gui::MainWindow* main_;
HppWidgetsPlugin* plugin_;
JointTreeItem const* item_;
const int bound_;
const double maxVelocity_;
hpp::floatSeq q_, dq_;
int index_;
};
class SliderBoundedJoint : public QSlider
{
Q_OBJECT
public:
SliderBoundedJoint (Qt::Orientation orientation, HppWidgetsPlugin* plugin, QWidget* parent,
gepetto::gui::MainWindow *main, JointTreeItem const* item, int index);
double getValue ();
private slots:
void updateConfig (int value);
private:
gepetto::gui::MainWindow* main_;
HppWidgetsPlugin* plugin_;
JointTreeItem const* item_;
double value_;
int index_;
double m_, M_;
};
class JointItemDelegate : public QItemDelegate
{
Q_OBJECT
......
plugins/hppwidgetsplugin/jointtreewidget.cc
View file @ e32ac15a
......@@ -134,7 +134,7 @@ namespace hpp {
}
}
JointTreeItem* JointTreeWidget::buildJointTreeItem(const char* name, std::size_t& rkConfig)
JointTreeItem* JointTreeWidget::buildJointTreeItem(const char* name, ULong& rkConfig, ULong& rkVel)
{
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
HppWidgetsPlugin::JointElement& je = plugin_->jointMap() [name];
......@@ -150,13 +150,14 @@ namespace hpp {
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, rkConfig, c.in(), b.in(), nbDof, nodes);
j = new JointTreeItem (name, rkConfig, rkVel, c.in(), b.in(), nbDof, nodes);
} else {
j = new JointTreeItem (name, rkConfig, hpp::floatSeq(), hpp::floatSeq(), nbDof, nodes);
j = new JointTreeItem (name, rkConfig, rkVel, hpp::floatSeq(), hpp::floatSeq(), nbDof, nodes);
}
je.item = j;
j->setupActions(plugin_);
rkConfig += nbCfg;
rkVel += nbDof;
return j;
}
......@@ -224,11 +225,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;
ULong rkCfg = 0, rkVel = 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, rk);
items[jname] = buildJointTreeItem(jname, rkCfg, rkVel);
}
for (JointTreeItemMap_t::const_iterator _jti = items.begin();
_jti != items.end(); ++_jti) {
......
plugins/hppwidgetsplugin/jointtreewidget.hh
View file @ e32ac15a
......@@ -75,7 +75,7 @@ signals:
/// Reset the tree.
void reset ();
JointTreeItem* buildJointTreeItem (const char* name, std::size_t& rkConfig);
JointTreeItem* buildJointTreeItem (const char* name, CORBA::ULong& rkConfig, CORBA::ULong& rkVel);
HppWidgetsPlugin* plugin_;
::Ui::JointTreeWidget* ui_;
......
plugins/hppwidgetsplugin/pathplayer.cc
View file @ e32ac15a
......@@ -268,11 +268,11 @@ namespace hpp {
{
hpp::floatSeq_var config =
plugin_->client()->problem()->configAtParam ((CORBA::ULong)pathIndex()->value(),currentParam_);
plugin_->client()->robot()->setCurrentConfig (config.in());
plugin_->currentConfig() = config.in();
if (velocity_) {
config =
plugin_->client()->problem()->derivativeAtParam ((CORBA::ULong)pathIndex()->value(),1,currentParam_);
plugin_->client()->robot()->setCurrentVelocity (config.in());
plugin_->currentVelocity() = config.in();
}
gepetto::gui::MainWindow::instance()->requestApplyCurrentConfiguration();
emit appliedConfigAtParam (getCurrentPath(), currentParam_);
......
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