Commit ef2180cc authored by Rohan Budhiraja's avatar Rohan Budhiraja

change baseff controlValue rotatoin from matrix to quaternion

parent b9cf2f3f
......@@ -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) ;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment