CustomSensorMotor

Prev: Writing tests for evolutionary experiments	Up: Home	Next: Implementing a new world object

Table of contents

• Creating Custom Sensors or Motors
  • A Simple Sensor
  • A Robot-Specific Sensor
  • A Simple Motor

Creating Custom Sensors or Motors

FARSA provide many ready to use sensors and motors for each supported robot type as well as additional sensors and motors that can be used with different kinds of robots. Some sensors also perform a pre-processing of the information that is extracted from the physical sensors. These sensors and motors are istantiated and used by simply listing the required ones in the configuration file. In some cases, however, you might want to implement a new type of sensor or motor for your experiment, or you might want to implement a new version of an existing sensor that preprocesses the sensory information in a different way with respect to existing sensors.

Each sensor updates the state of a certain number of sensory units or sensory neurons. For example the infrared sensor of the ePuck operates by updating a block of 8 sensory units of the robot controller that encode the state of the 8 infrared sensors distributed around the circular body of the robot. Each motor updates the state of one or more robot actuator on the basis of the state of one or more motor units or motor neurons of the robot controller. For example the EpuckWheelVelocityMotor updates the desired speed of the two motors controlling the two corresponding wheels on the basis of the state of two motor units or motor neurons of the robot controller. The interface between the sensors and motors and the robots' controllers is handled by the NeuronsIterator class. The sensory and motor units are divided in blocks that have the same name of the sensor that is used to update them or of the motor that use them to set the state of the corresponding robot actuators. The NeuronsIterator class include the following functions:

• setCurrentBlock() - selects the block to be read or written;
• setInput() - sets the value of the current element;
• getOutput() - reads the value of the current element;
• setGraphicProperties() - sets the graphical properties of the element that are used by the graphic interface;
• nextNeuron() - sets the next unit of the current block;

Sensors and motors are components that can be implemented within an experimental plugin that is compiled indendently from FARSA. Like all components implemented in a plugin, they should make use of the FARSA_PLUGIN_* and the FARSA_REGISTER_CLASS macros (for more information see this page).

Sensors that can be used with different robots are declared as subclasses of the general Sensor class. Sensors that can be used only with a specific robotic platform are declared instead as subclasses of more specific classes (named KheperaSensor, EpuckSensor, MarXbotSensor and iCubSensor). Similarly, motors that can be used with differen robots are declared as subclasses of the general Motor class, while other motors are declared as subclasses of more specific motor classes (named KheperaMotor, EpuckMotor, MarXbotMotor, and iCubMotor).

A Simple Sensor

Below we illustrate the implementation of a simple sensor, named MinimalSensor, that creates 3 sensory units and simply sets their state to 0.5 each time step. The source code included in this example, therefore, can be used as a template for other sensors (with the exception of the instructions that assign the fixed value to the sensory units).

The declaration of the sensor class, included below, shows that the sensor is a subclass of the general Sensor class (i.e. that can be used on any type of robot). The class has two data members: a pointer to the neuronsIterator resource (called m_neuronsIterator) that contains the state of the sensory units to be updated and a string containing the name of the neuronsIterator resource.

We assume that this sensor will be placed inside an experimental plugin. Consequently the declaration includes the FARSA_PLUGIN_API and FARSA_REGISTER_CLASS macros.

:::C++
class FARSA_PLUGIN_API MinimalSensor : public Sensor
{
    FARSA_REGISTER_CLASS(Sensor)
public:
    MinimalSensor(ConfigurationParameters& params, QString prefix);
    ~MinimalSensor();
    virtual void save(ConfigurationParameters& params, QString prefix);
    static void describe(QString type);
    virtual void update();
    virtual int size();
protected:
    virtual void resourceChanged(QString resourceName, ResourceChangeType changeType);
    const QString m_neuronsIteratorResource;
    NeuronsIterator* m_neuronsIterator;
};

The Sensor class (as well as Motor) inherits from ParameterSettableInConstructor. This means that the constructor has to take two parameters and that describe() has to be present. The only configuration parameter that this class expects is the name of the resource of the neuronsIterator (in the majority of the cases this parameter does not need to be changed).

:::C++
void MinimalSensor::describe(QString type)
{
    // Calling parent function
    Sensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "It simply set 3 sensory units to 0.5", "This is an exemplificative sensor that create three sensory units and simply set them to a costant 0.5 value every time");
    d.describeString("neuronsIterator").def("neuronsIterator").help("The name of the resource associated with the neural network iterator (default is \"neuronsIterator\")");
}

The constructor of the MinimalSensor class reads the configuration parameter with the name of the resource of the neuronsIterator and declares which resources the class will use. In the case of sensors that are used in multirobots experiments, it is important to keep in mind that the sensor should refer to the right robot of the team. This means that, in case of resources that refer to a specific robot, the name of the resource must be mangled so that it refers to the correct robot in the team. This is done by actualResourceNameForMultirobot(), which takes the generic resource name and returns the name the sensor should use. In the example below the function is applied to the name of the neuronsIterator resource.

:::C++
MinimalSensor::MinimalSensor(ConfigurationParameters& params, QString prefix) :
    Sensor(params, prefix),
    m_neuronsIteratorResource(actualResourceNameForMultirobot(ConfigurationHelper::getString(params, prefix + "neuronsIterator", "neuronsIterator"))),
    m_neuronsIterator(NULL)
{
    addUsableResource(m_neuronsIteratorResource);
}

MinimalSensor::~MinimalSensor()
{
}

The size() function returns the number of sensory units created and updated by the sensor, 3 in our example. The value returned should be constant for the whole lifetime of the Sensor instance.

:::C++
int MinimalSensor::size()
{
    return 3;
}

The resourceChanged() function is used to initialize the pointers to the required resources and to set the label of the corresponding sensory units which is used by the graphic interface to display the elements of the robots' controller. In this example the label of the three sensory units created by the sensor will be "m0", "m1", and "m2". The setGraphicProperties() function is also used to specify the range of the units and the colour with which the unit state will be displayed by the "Neurons Monitor" graphic widget of Evonet.

The resourceChanged() function is called every time a resource used by the sensor changes (for more information see this page). To avoid inconsistencies, this implementation checks whether the notification concerns a resource that has been deleted.

:::C++
void MinimalSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    if (changeType == Deleted) {
        resetNeededResourcesCheck();
        return;
    }

    if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        for(int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setGraphicProperties("m" + QString::number(i), 0.0, 1.0, Qt::red);
        }
    } else if (resourceName != m_additionalInputsResource) {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

The update() function takes care of updating the state of the sensory units. This function is called every time step, consequently the sensory units are update every time step. In this case the state of the three sensory units is simply set to 0.5. The checkAllNeededResourcesExist() is a helper function that checks whether all the resources the sensor has declared to use already exists. Its main purpouse is to prevent NULL pointer dereference errors when a resource pointer is stored in the resourceChanged callback instead of being accessed via the getResource method (as, in this case, m_neuronsIterator). Before actually setting the input of the controller, the setCurrentBlock() function of the NeuronsIterator object is called to declare which is the block of sensory units that is going to be filled. The name of the block corresponding to the current sensor has the same name as the sensor itself (i.e. the name is returned by the name() function). Afterwards, the setInput() function is used to set the value of the current element and nextNeuron() is called to pass to the subsequent element.

:::C++
void MinimalSensor::update()
{
    checkAllNeededResourcesExist();

    ResourcesLocker locker(this);

    m_neuronsIterator->setCurrentBlock(name());
    for (unsigned int i = 0; i < m_additionalInputs.size(); i++, m_neuronsIterator->nextNeuron()) {
        m_neuronsIterator->setInput(0.5);
    }
}

A Robot-Specific Sensor

If the sensor you have to implement could be used by a specific robotic platform only, you can declare it as sub-class of a specific robot sensor class (e.g. EpuckSensor, KheperaSensor, MarXbotSensor or iCubSensor). In this case, the name of the neuronsIterator resource as well as the name of the robot resources are automatically modified so that they refer to the correct robot.

As an example, here we enclose the source code of the KheperaProximityIRSensor class. Since this sensors is included inside FARSA, its declaration do not include the macro instructions that specify that it is a component and that it should be registered in the components lists, as for the case of the MinimalSensor described above.

This sensor sets the activation state of the 8 sensory units encoding the state of the corresponding infrared sensors of the Khepera on the basis of whether the virtual rays departing from each simulated sensor intersects an object of the environment and on the basis of the distance between the sensor and the intersection point. This calculation is made by the proximityIRSensorController()->activation() function of the robot class that is called in the update() function of the sensor.

Notice also how the state of the sensor calculated by the proximityIRSensorController()->activation() is not assigned directly to the corresponding sensory unit but it is first passed to the applyNoise() function that returns a value perturbed by noise. The applyNoise() function also receive as parameters the maximum and minimum value that the sensory unit can assume and truncates the value returned when it exceeds the limits specified as a result of the noise application. The applyNoise() function can be applied to all sensors and motors and is indeed included in most of the sensors and motors implemented in FARSA. The type of noise added can be specified in the noiseType and noiseRange parameters of each sensor and motor. For more information on this function see the API documentation. In the case of custom sensors that you implemented, you should remember to use the applyNoise() function if you want to apply noise. Otherwise the parameters noiseType and noiseRange will not have any effect.

:::C++
class KheperaProximityIRSensor : public KheperaSensor
{
public:
    KheperaProximityIRSensor(ConfigurationParameters& params, QString prefix);
    virtual ~KheperaProximityIRSensor();
    virtual void save(ConfigurationParameters& params, QString prefix);
    static void describe(QString type);
    virtual void update();
    virtual int size();
private:
    virtual void resourceChanged(QString resourceName, ResourceChangeType changeType);
    PhyKhepera* m_robot;
    NeuronsIterator* m_neuronsIterator;
};

KheperaProximityIRSensor::KheperaProximityIRSensor(ConfigurationParameters& params, QString prefix)
    : KheperaSensor(params, prefix)
    , m_robot(NULL)
    , m_neuronsIterator(NULL)
{
}

KheperaProximityIRSensor::~KheperaProximityIRSensor()
{
}

void KheperaProximityIRSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    KheperaSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "KheperaProximityIRSensor", this);
}

void KheperaProximityIRSensor::describe(QString type)
{
    // Calling parent function
    KheperaSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The infrared proximity sensors of the Khepera robot", "The infrared proximity sensors of the Khepera II robot. These are the very short range IR sensors all around the base");
}

void KheperaProximityIRSensor::update()
{
    checkAllNeededResourcesExist();

    ResourcesLocker locker( this );

    m_neuronsIterator->setCurrentBlock(name());
    for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
        // Setting the input of the controller after applying noise
        m_neuronsIterator->setInput(applyNoise(m_robot->proximityIRSensorController()->activation(i), 0.0, 1.0));
    }
}

int KheperaProximityIRSensor::size()
{
    return 8;
}

void KheperaProximityIRSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    KheperaSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == m_kheperaResource) {
        m_robot = getResource<PhyKhepera>();

        m_robot->proximityIRSensorController()->setEnabled(true); // Eanbling sensors
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setGraphicProperties("ir" + QString::number(i), 0.0, 1.0, Qt::red);
    }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

A Simple Motor

The implementation of motors is very similar to that of sensors. The only difference is that in the update() function the state of the motor units is read through the getOutput() method of NeuronsIterator and then used to set the state of the robot actuators (e.g. to set the force applied by a motorized joint, to turn the robots' led on or off etc.). As an example, below we report the FakeMotor motor, that is robot independent, and that do not really change the actuators of the robot but simply store the output of the robots' controller into a vector resource called additionalOutputs. This is a resource, costituted by a vector of numbers, that is declared in the shareResourcesWith() function.

:::C++
class FakeMotor : public Motor
{
public:
    FakeMotor(ConfigurationParameters& params, QString prefix);
    ~FakeMotor();
    virtual void save(ConfigurationParameters& params, QString prefix);
    static void describe(QString type);
    virtual void update();
    virtual int size();
    virtual void shareResourcesWith(ResourcesUser* other);
protected:
    void resourceChanged(QString resourceName, ResourceChangeType changeType);
    ResourceVector<real> m_additionalOutputs;
    const QString m_neuronsIteratorResource;
    const QString m_additionalOutputsResource;
    NeuronsIterator* m_neuronsIterator;
};

FakeMotor::FakeMotor(ConfigurationParameters& params, QString prefix)
    : Motor(params, prefix)
    , m_additionalOutputs(ConfigurationHelper::getUnsignedInt(params, prefix + "additionalOutputs", 1))
    , m_neuronsIteratorResource(actualResourceNameForMultirobot(ConfigurationHelper::getString(params, prefix + "neuronsIterator", "neuronsIterator")))
    , m_additionalOutputsResource(actualResourceNameForMultirobot(ConfigurationHelper::getString(params, prefix + "additionalOutputsResource", "additionalOutputs")))
    , m_neuronsIterator(NULL)
{
    usableResources(QStringList() << m_neuronsIteratorResource << m_additionalOutputsResource);

    for (unsigned int i = 0; i < m_additionalOutputs.size(); i++) {
        m_additionalOutputs[i] = 0.0;
    }
}

FakeMotor::~FakeMotor()
{
    try {
        deleteResource(m_additionalOutputsResource);
    } catch (...) {
    }
}

[... save and describe skipped ...]

void FakeMotor::update()
{
    checkAllNeededResourcesExist();

    ResourcesLocker locker(this);

    m_neuronsIterator->setCurrentBlock(name());
    for (unsigned int i = 0; i < m_additionalOutputs.size(); i++, m_neuronsIterator->nextNeuron()) {
        m_additionalOutputs[i] = m_neuronsIterator->getOutput();
    }
}

int FakeMotor::size()
{
    return m_additionalOutputs.size();
}

void FakeMotor::shareResourcesWith(ResourcesUser* other)
{
    Motor::shareResourcesWith(other);

    declareResource(m_additionalOutputsResource, &m_additionalOutputs);
}

void FakeMotor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    if (changeType == Deleted) {
        resetNeededResourcesCheck();
        return;
    }

    if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        for(int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setGraphicProperties("Fk" + QString::number(i), 0.0, 1.0, Qt::red);
        }
    } else if (resourceName != m_additionalOutputsResource) {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}