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) ...@@ -98,8 +98,10 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
} }
it = SensorsIn.find("attitude"); it = SensorsIn.find("attitude");
bool attitudeSensorFound_ = false;
if (it!=SensorsIn.end()) if (it!=SensorsIn.end())
{ {
attitudeSensorFound_ = true;
const vector<double>& attitude = it->second.getValues (); const vector<double>& attitude = it->second.getValues ();
for (unsigned int i = 0; i < 3; ++i) for (unsigned int i = 0; i < 3; ++i)
for (unsigned int j = 0; j < 3; ++j) for (unsigned int j = 0; j < 3; ++j)
...@@ -108,13 +110,28 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn) ...@@ -108,13 +110,28 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
attitudeSOUT.setTime (t); 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"); it = SensorsIn.find("joints");
if (it!=SensorsIn.end()) if (it!=SensorsIn.end())
{ {
const vector<double>& anglesIn = it->second.getValues(); const vector<double>& anglesIn = it->second.getValues();
mlRobotState.resize (anglesIn.size () + 6); mlRobotState.resize (anglesIn.size () + 6);
for (unsigned i = 0; i < 6; ++i) if (ffPositionSensorFound_ == true && attitudeSensorFound_ == true) {
mlRobotState (i) = 0.; 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) for (unsigned i = 0; i < anglesIn.size(); ++i)
mlRobotState (i + 6) = anglesIn[i]; mlRobotState (i + 6) = anglesIn[i];
robotState_.setConstant(mlRobotState); 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