Merge pull request #25 from mehdi-benallegue/master

Make the velocity signal available even when the robot is not controlled...

Author: florent-lamiraux

src/tools/device.cpp

@@ -141,12 +141,15 @@ Device( const std::string& n )
   forcesSOUT[3] =
     new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceLARM");
 
-  signalRegistration( controlSIN<<stateSOUT<<attitudeSOUT<<attitudeSIN<<zmpSIN
-		      <<*forcesSOUT[0]<<*forcesSOUT[1]<<*forcesSOUT[2]<<*forcesSOUT[3]
-		      <<previousControlSOUT <<pseudoTorqueSOUT
-		      << motorcontrolSOUT << ZMPPreviousControllerSOUT );
+  signalRegistration( controlSIN<<stateSOUT<<velocitySOUT<<attitudeSOUT
+              <<attitudeSIN<<zmpSIN <<*forcesSOUT[0]<<*forcesSOUT[1]
+              <<*forcesSOUT[2]<<*forcesSOUT[3] <<previousControlSOUT
+              <<pseudoTorqueSOUT << motorcontrolSOUT << ZMPPreviousControllerSOUT );
   state_.fill(.0); stateSOUT.setConstant( state_ );
 
+  velocity_.resize(state_.size()); velocity_.setZero();
+  velocitySOUT.setConstant( velocity_ );
+
   /* --- Commands --- */
   {
     std::string docstring;
@@ -258,7 +261,6 @@ void Device::
 setSecondOrderIntegration()
 {
   secondOrderIntegration_ = true;
-  signalRegistration( velocitySOUT );
   velocity_.resize(state_.size());
   velocity_.setZero();
   velocitySOUT.setConstant( velocity_ );
@@ -306,11 +308,17 @@ increment( const double & dt )
 
   /* Position the signals corresponding to sensors. */
   stateSOUT .setConstant( state_ ); stateSOUT.setTime( time+1 );
+   //computation of the velocity signal
   if( secondOrderIntegration_  )
     {
       velocitySOUT.setConstant( velocity_ );
       velocitySOUT.setTime( time+1 );
     }
+  else
+    {
+      velocitySOUT.setConstant( controlSIN.accessCopy() );
+      velocitySOUT.setTime( time+1 );
+    }
   for( int i=0;i<4;++i ){
     if(  !withForceSignals[i] ) forcesSOUT[i]->setConstant(forceZero6);
   }