change baseff controlValue rotatoin from matrix to quaternion

src/sot-hrp2-device.cpp

@@ -55,7 +55,7 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
   dg::Vector data (3); data.setZero ();
   accelerometerSOUT_.setConstant (data);
   gyrometerSOUT_.setConstant (data);
-  baseff_.resize(12);
+  baseff_.resize(7);
 
   using namespace dynamicgraph::command;
   std::string docstring;
@@ -244,11 +244,17 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
   controlOut["zmp"].setValues(ZMPRef);
 
   // Update position of freeflyer in global frame
-  for (int i = 0;i < 3; ++i)
-    baseff_[i*4+3] = freeFlyerPose () (i, 3);
-  for(unsigned i = 0;i < 3; ++i)
-    for(unsigned j = 0; j < 3; ++j)
-      baseff_[i * 4 + j] = freeFlyerPose () (i, j);
+  Eigen::Vector3d transq_(freeFlyerPose().translation());
+  dg::sot::VectorQuaternion qt_(freeFlyerPose().linear());
+
+  //translation
+  for(int i=0; i<3; i++) baseff_[i] = transq_(i);
+  
+  //rotation: quaternion
+  baseff_[3] = qt_.w();
+  baseff_[4] = qt_.x();
+  baseff_[5] = qt_.y();
+  baseff_[6] = qt_.z();
 
   controlOut["baseff"].setValues(baseff_);
   sotDEBUGOUT(25) ;