SensorsMeasurements::SensorsMeasurements(const SensorsList &sensorsList)
{
this->pimpl = new SensorsMeasurementsPrivateAttributes;
this->pimpl->SixAxisFTSensorsMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE));
this->pimpl->AccelerometerMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::ACCELEROMETER));
this->pimpl->GyroscopeMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::GYROSCOPE));
}
bool SensorsMeasurements::resize(const SensorsList &sensorsList)
{
Wrench zeroWrench;zeroWrench.zero();
LinAcceleration zeroLinAcc;zeroLinAcc.zero();
AngVelocity zeroAngVel;zeroAngVel.zero();
this->pimpl->SixAxisFTSensorsMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE),zeroWrench);
this->pimpl->AccelerometerMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::ACCELEROMETER),zeroLinAcc);
this->pimpl->GyroscopeMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::GYROSCOPE),zeroAngVel);
return true;
}
void SensorsList::constructor(const SensorsList& other)
{
this->pimpl = new SensorsListPimpl();
this->pimpl->VecSensors.resize(NR_OF_SENSOR_TYPES,std::vector<Sensor *>(0));
this->pimpl->NamesSensors.resize(NR_OF_SENSOR_TYPES);
for(int sens_type = 0; sens_type < NR_OF_SENSOR_TYPES; sens_type++ )
{
for(unsigned int sens = 0; sens < other.getNrOfSensors((SensorType)sens_type); sens++ )
{
this->pimpl->VecSensors[sens_type].push_back(other.pimpl->VecSensors[sens_type][sens]->clone());
std::string sensor_name = other.getSensor((SensorType)sens_type,sens)->getName();
this->pimpl->NamesSensors[sens_type].insert(std::pair<std::string,int>(sensor_name,sens));
}
}
}
int main()
{
// Load data
std::string fileName = getAbsModelPath("icub_sensorised.urdf");
Model fullModel = getModel(fileName);
SensorsList fullSensors = getSensors(fileName);
// Store data for a given FT sensor
DataFT dataFT_l_leg;
dataFT_l_leg.jointName = "l_leg_ft_sensor";
// Links
dataFT_l_leg.parent_linkName = "l_hip_2";
dataFT_l_leg.child_linkName = "l_hip_3";
dataFT_l_leg.grandparent_linkName = "l_hip_1";
dataFT_l_leg.grandchild_linkName = "l_upper_leg";
// Joints
dataFT_l_leg.grandparentToParent_jointName = "l_hip_roll";
dataFT_l_leg.childToGranchild_jointName = "l_hip_yaw";
// Get all the iCub joints
vector<string> fullJoints = get_iCubJointsSensorised();
// Keep only one FT
vector<string> removedJoints;
removedJoints.push_back("r_leg_ft_sensor");
removedJoints.push_back("l_foot_ft_sensor");
removedJoints.push_back("r_foot_ft_sensor");
removedJoints.push_back("l_arm_ft_sensor");
removedJoints.push_back("r_arm_ft_sensor");
vector<string> fullJoints_1FT = removeJoints(fullJoints, removedJoints);
// Setup the experiments
// ---------------------
// 1) The reduced model doesn't lump links which the FT is connected
ExperimentFT test_defaultModel;
test_defaultModel.dataFT = dataFT_l_leg;
test_defaultModel.expectedFirstLinkName = dataFT_l_leg.parent_linkName;
test_defaultModel.expectedSecondLinkName = dataFT_l_leg.child_linkName;
// 2) The reduced model doesn't contain the firstLink
ExperimentFT test_removeFirstLink;
test_removeFirstLink.dataFT = dataFT_l_leg;
test_removeFirstLink.removedJoints.push_back(test_removeFirstLink.dataFT.grandparentToParent_jointName);
test_removeFirstLink.expectedFirstLinkName = dataFT_l_leg.grandparent_linkName;
test_removeFirstLink.expectedSecondLinkName = dataFT_l_leg.child_linkName;
// 3) The reduced model doesn't contain the secondLink
ExperimentFT test_removeSecondLink;
test_removeSecondLink.dataFT = dataFT_l_leg;
test_removeSecondLink.removedJoints.push_back(test_removeSecondLink.dataFT.childToGranchild_jointName);
test_removeSecondLink.expectedFirstLinkName = dataFT_l_leg.parent_linkName;
test_removeSecondLink.expectedSecondLinkName = dataFT_l_leg.child_linkName;
// 3) The reduced model doesn't contain both firstLink and secondLink
ExperimentFT test_removeFirstAndSecondLink;
test_removeFirstAndSecondLink.dataFT = dataFT_l_leg;
test_removeFirstAndSecondLink.removedJoints.push_back(test_removeFirstAndSecondLink.dataFT.grandparentToParent_jointName);
test_removeFirstAndSecondLink.removedJoints.push_back(test_removeFirstAndSecondLink.dataFT.childToGranchild_jointName);
test_removeFirstAndSecondLink.expectedFirstLinkName = dataFT_l_leg.grandparent_linkName;
test_removeFirstAndSecondLink.expectedSecondLinkName = dataFT_l_leg.child_linkName;
// Group all the experiments together
vector<ExperimentFT> experiments;
experiments.push_back(test_defaultModel);
experiments.push_back(test_removeFirstLink);
experiments.push_back(test_removeSecondLink);
experiments.push_back(test_removeFirstAndSecondLink);
for (auto experiment : experiments)
{
bool ok;
Model reducedModel;
SensorsList reducedSensors;
ok = createReducedModelAndSensors(fullModel,
fullSensors,
removeJoints(fullJoints_1FT, experiment.removedJoints),
reducedModel,
reducedSensors);
ASSERT_IS_TRUE(ok);
ASSERT_EQUAL_DOUBLE(reducedSensors.getNrOfSensors(SIX_AXIS_FORCE_TORQUE), 1);
Sensor* s = reducedSensors.getSensor(SIX_AXIS_FORCE_TORQUE, 0);
ASSERT_IS_TRUE(s != nullptr);
ASSERT_IS_TRUE(s->isValid());
JointSensor* jointSens = dynamic_cast<JointSensor*>(s);
ASSERT_IS_TRUE(jointSens != nullptr);
unique_ptr<SixAxisForceTorqueSensor> sensorCopy;
sensorCopy.reset(static_cast<SixAxisForceTorqueSensor*>(jointSens->clone()));
ASSERT_IS_TRUE(sensorCopy != nullptr);
printFT(*sensorCopy);
cout << endl;
ASSERT_EQUAL_STRING(sensorCopy->getFirstLinkName(), experiment.expectedFirstLinkName);
ASSERT_EQUAL_STRING(sensorCopy->getSecondLinkName(), experiment.expectedSecondLinkName);
// Test transform
// --------------
// Get the transform from the fixed joint.
// It uses the createReducedModel() logic. It is used as ground truth.
Transform parent_H_child;
auto jointFT = reducedModel.getJoint(reducedModel.getJointIndex(experiment.dataFT.jointName));
parent_H_child = jointFT->getRestTransform(jointFT->getFirstAttachedLink(),
jointFT->getSecondAttachedLink());
// Get the transform from the sensor.
// This is calculated by createReducedModelAndSensors() and it is the method under test.
LinkIndex firstLinkIndex = sensorCopy->getFirstLinkIndex();
LinkIndex secondLinkIndex = sensorCopy->getSecondLinkIndex();
Transform firstLink_H_sensorFrame;
Transform secondLink_H_sensorFrame;
ok = true;
ok = ok && sensorCopy->getLinkSensorTransform(firstLinkIndex, firstLink_H_sensorFrame);
ok = ok && sensorCopy->getLinkSensorTransform(secondLinkIndex, secondLink_H_sensorFrame);
ASSERT_IS_TRUE(ok);
ASSERT_EQUAL_TRANSFORM(parent_H_child,
firstLink_H_sensorFrame*secondLink_H_sensorFrame.inverse());
}
return EXIT_SUCCESS;
}
