First step toward thread safety.

plugins/hppwidgetsplugin/hppwidgetsplugin.cc

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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_);