Add COM in HppWidgetsPlugin

plugins/hppwidgetsplugin/hppwidgetsplugin.cc

@@ -135,6 +135,7 @@ namespace hpp {
       main->osg()->refresh();
 
       main->registerSlot("requestCreateJointGroup", this);
+      main->registerSlot("requestCreateComGroup", this);
       main->registerSlot("getHppIIOPurl", this);
       main->registerSlot("getSelectedJoint", jointTreeWidget_);
       main->registerSignal(SIGNAL(appliedConfigAtParam(int,double)), pathPlayer_);
@@ -269,6 +270,14 @@ namespace hpp {
         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);
+      }
       main->osg()->refresh();
     }
 
@@ -386,6 +395,11 @@ namespace hpp {
       return createJointGroup(jn.toStdString()).c_str();
     }
 
+    QString HppWidgetsPlugin::requestCreateComGroup(const QString com)
+    {
+      return QString::fromStdString(createComGroup(com.toStdString()));
+    }
+
     HppWidgetsPlugin::HppClient *HppWidgetsPlugin::client() const
     {
       return hpp_;
@@ -510,6 +524,27 @@ namespace hpp {
       return target;
     }
 
+    std::string HppWidgetsPlugin::createComGroup(const std::string com)
+    {
+      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
+      std::string target = "com_" + escapeJointName(com);
+      graphics::GroupNodePtr_t group = main->osg()->getGroup (target.c_str(), false);
+      if (group) return target;
+      if (!main->osg()->getGroup(target)) {
+        main->osg()->createGroup(target);
+        main->osg()->addToGroup(target, "joints");
+
+        hpp::floatSeq_var p = client()->robot()->getPartialCom (com.c_str());
+        OsgConfiguration_t t;
+        t.quat.set(0,0,0,1);
+        fromHPP (p, t.position);
+        comFrames_.push_back(com);
+        main->osg()->applyConfiguration (target, t);
+        main->osg()->refresh();
+      }
+      return target;
+    }
+
     Q_EXPORT_PLUGIN2 (hppwidgetsplugin, HppWidgetsPlugin)
   } // namespace gui
 } // namespace hpp

plugins/hppwidgetsplugin/hppwidgetsplugin.hh

@@ -106,6 +106,9 @@ signals:
         /// See createJointGroup
         QString requestCreateJointGroup(const QString jn);
 
+        /// See createComGroup
+        QString requestCreateComGroup(const QString com);
+
         QString getHppIIOPurl () const;
 
       public:
@@ -162,6 +165,10 @@ signals:
         /// \param jn joint name
         std::string createJointGroup (const std::string jn);
 
+        /// Create a group from the given COM.
+        /// \param com COM name
+        std::string createComGroup (const std::string com);
+
         /// Replace all the bodies according to their position in hpp.
         void computeObjectPosition();
 
@@ -172,6 +179,7 @@ signals:
         ConstraintWidget* constraintWidget_;
         JointMap jointMap_;
         std::list <std::string> jointFrames_;
+        std::list <std::string> comFrames_;
     };
   } // namespace gui
 } // namespace hpp