Update documentation of Device + minor changes

include/sot/core/device.hh

@@ -75,7 +75,6 @@ namespace dynamicgraph {
     protected:
       dg::Vector state_;
       dg::Vector velocity_;
-      bool vel_controlInit_;
       dg::Vector vel_control_;
       ControlInput controlInputType_;
       bool withForceSignals[4];
@@ -110,24 +109,33 @@ namespace dynamicgraph {
       dynamicgraph::SignalPtr<dg::Vector,int> attitudeSIN;
       dynamicgraph::SignalPtr<dg::Vector,int> zmpSIN;
 
+      /// \name Device current state.
+      /// \{
       dynamicgraph::Signal<dg::Vector,int> stateSOUT;
+      dynamicgraph::Signal<dg::Vector,int> velocitySOUT;
+      dynamicgraph::Signal<MatrixRotation,int> attitudeSOUT;
+      /*! \brief The current state of the robot from the command viewpoint. */
+      dynamicgraph::Signal<dg::Vector,int> motorcontrolSOUT;
+      dynamicgraph::Signal<dg::Vector,int> previousControlSOUT;
+      /*! \brief The ZMP reference send by the previous controller. */
+      dynamicgraph::Signal<dg::Vector,int> ZMPPreviousControllerSOUT;
+      /// \}
+
+      /// \name Real robot current state
       /// This corresponds to the real encoders values and take into
       /// account the stabilization step. Therefore, this usually
       /// does *not* match the state control input signal.
-      ///
+      /// \{
+      /// Motor positions
       dynamicgraph::Signal<dg::Vector, int> robotState_;
+      /// Motor velocities
       dynamicgraph::Signal<dg::Vector, int> robotVelocity_;
-      dynamicgraph::Signal<dg::Vector,int> velocitySOUT;
-      dynamicgraph::Signal<MatrixRotation,int> attitudeSOUT;
+      /// The force torque sensors
       dynamicgraph::Signal<dg::Vector,int>* forcesSOUT[4];
-
+      /// Motor torques
+      /// \todo why pseudo ?
       dynamicgraph::Signal<dg::Vector,int> pseudoTorqueSOUT;
-      dynamicgraph::Signal<dg::Vector,int> previousControlSOUT;
-
-      /*! \brief The current state of the robot from the command viewpoint. */
-      dynamicgraph::Signal<dg::Vector,int> motorcontrolSOUT;
-      /*! \brief The ZMP reference send by the previous controller. */
-      dynamicgraph::Signal<dg::Vector,int> ZMPPreviousControllerSOUT;
+      /// \}
 
     protected:
       /// Compute roll pitch yaw angles of freeflyer joint.

src/tools/device.cpp

@@ -99,11 +99,16 @@ Device( const std::string& n )
   //,attitudeSIN(NULL,"Device::input(matrixRot)::attitudeIN")
   ,velocitySOUT( "Device("+n+")::output(vector)::velocity"  )
   ,attitudeSOUT( "Device("+n+")::output(matrixRot)::attitude" )
-  ,pseudoTorqueSOUT( "Device("+n+")::output(vector)::ptorque" )
+  ,motorcontrolSOUT   ( "Device("+n+")::output(vector)::motorcontrol" )
   ,previousControlSOUT( "Device("+n+")::output(vector)::previousControl" )
-  ,motorcontrolSOUT( "Device("+n+")::output(vector)::motorcontrol" )
-  ,ZMPPreviousControllerSOUT( "Device("+n+")::output(vector)::zmppreviouscontroller" ), ffPose_(),
-    forceZero6 (6)
+  ,ZMPPreviousControllerSOUT( "Device("+n+")::output(vector)::zmppreviouscontroller" )
+
+  ,robotState_     ("Device("+n+")::output(vector)::robotState")
+  ,robotVelocity_  ("Device("+n+")::output(vector)::robotVelocity")
+  ,pseudoTorqueSOUT("Device("+n+")::output(vector)::ptorque" )
+
+  ,ffPose_()
+  ,forceZero6 (6)
 {
   forceZero6.fill (0);
   /* --- SIGNALS --- */
@@ -378,12 +383,8 @@ void Device::integrate( const double & dt )
     return;
   }
 
-  if( !vel_controlInit_ )
-  {
-    vel_control_ = Vector(controlIN.size());
-    vel_control_.setZero();
-    vel_controlInit_ = true;
-  }
+  if( vel_control_.size() == 0 )
+    vel_control_ = Vector::Zero(controlIN.size());
 
   // If control size is state size - 6, integrate joint angles,
   // if control and state are of same size, integrate 6 first degrees of