Exemple #1
0
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;
}