Commit f0c2869c authored by Rohan Budhiraja's avatar Rohan Budhiraja

[sot-hrp2-device] Accept base position and orientation into robotState.

If base position and attitude exist, set as first 6 dof of robotState
else, set as zero
parent 58ca58d5
......@@ -98,8 +98,10 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
}
it = SensorsIn.find("attitude");
bool attitudeSensorFound_ = false;
if (it!=SensorsIn.end())
{
attitudeSensorFound_ = true;
const vector<double>& attitude = it->second.getValues ();
for (unsigned int i = 0; i < 3; ++i)
for (unsigned int j = 0; j < 3; ++j)
......@@ -108,13 +110,28 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
attitudeSOUT.setTime (t);
}
it = SensorsIn.find("base_position");
bool ffPositionSensorFound_ = false;
Eigen::Vector3d base_p;
if (it!=SensorsIn.end())
{
ffPositionSensorFound_ = true;
const vector<double>& baseposition = it->second.getValues ();
base_p << baseposition[0], baseposition[1], baseposition[2];
}
it = SensorsIn.find("joints");
if (it!=SensorsIn.end())
{
const vector<double>& anglesIn = it->second.getValues();
mlRobotState.resize (anglesIn.size () + 6);
for (unsigned i = 0; i < 6; ++i)
mlRobotState (i) = 0.;
if (ffPositionSensorFound_ == true && attitudeSensorFound_ == true) {
mlRobotState.head<3>() = base_p;
mlRobotState.segment<3>(3) = pose.eulerAngles(2,1,0).reverse();
}
else
mlRobotState.head<6>().setZero();
for (unsigned i = 0; i < anglesIn.size(); ++i)
mlRobotState (i + 6) = anglesIn[i];
robotState_.setConstant(mlRobotState);
......
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