User Tools

Site Tools


Sidebar

creating_a_controller

– This tutorial assumes that you are familiar with the robot tutorial –

In the previous step we saw how to create a robot. Now we need to make this robot to act and to react to the environment. In this tutorial we will cover how to create a controller for the robot that will tell him how to behave.

For our differential robot, we want a very simple behaviour: if an obstacle is detected try to avoid it otherwise move forward.

Usually, a controller receives input information from the robot's sensors, and according to them it creates a motor command that is propagated to the actuators of the robot.

In this example, we are going to use the differential robot that we created in the previous tutorial. This robot has an array of IR sensors that can detect if there is an object within their range. We will use this information to make the robot to move forward when is possible and to avoid obstacles when they are acknowledged by the sensors. When there is no reading from the IR sensors (no objects detected) the robot will move forward, if there is reading on any side of the robot it will turn to the opposite side. If there is an object just in front of the robot it will move backwards.

As usual we have the header and the source code files, in their basic form they look like this:

basiccontroller.h

// Header guard
#ifndef __BASIC_CONTROLLER_H
#define __BASIC_CONTROOLER_H
 
#include <selforg/abstractcontroller.h>
 
class BasicController : public AbstractController{
  public:
 
    BasicController(const std::string& name);
 
    /** initialisation of the controller with the given sensor/ motornumber
      Must be called before use. The random generator is optional.
      */
    virtual void init(int sensornumber, int motornumber, RandGen* randGen = 0);
 
    /** @return Number of sensors the controller
      was initialised with or 0 if not initialised */
    virtual int getSensorNumber() const;
 
    /** @return Number of motors the controller
      was initialised with or 0 if not initialised */
    virtual int getMotorNumber() const;
 
    /** performs one step.
      Calculates motor commands from sensor inputs.
      @param sensors sensors inputs scaled to [-1,1]
      @param sensornumber length of the sensor array
      @param motors motors outputs. MUST have enough space for motor values!
      @param motornumber length of the provided motor array
      */
    virtual void step(const sensor* sensors, int sensornumber,
        motor* motors, int motornumber);
 
 
    /** performs one step without learning.
      @see step
      */
    virtual void stepNoLearning(const sensor* , int number_sensors,
        motor* , int number_motors);
    /** stores the object to the given file stream (binary).
    */
    virtual bool store(FILE* f) const;
 
    /** loads the object from the given file stream (binary).
    */
    virtual bool restore(FILE* f);
 
  private:
  ...
};
 
#endif // Header guard

basiccontroller.cpp

#include "basiccontroller.h"
 
#include <assert.h>
using namespace std;
using namespace matrix;
 
BasicController::BasicController(const std::string& name)
  : AbstractController(name, "1.0") {
}
 
void BasicController::stepNoLearning(const sensor* sensors, int number_sensors,
                                     motor* motors, int number_motors) {
}
 
void BasicController::step(const sensor* sensors, int sensornumber,
                           motor* motors, int motornumber) {
}
 
void BasicController::init(int sensornumber, int motornumber, RandGen* randGen) {
}
 
int BasicController::getSensorNumber() const {
}
 
int BasicController::getMotorNumber() const {
}
 
bool BasicController::store(FILE* f) const {
}
 
bool BasicController::restore(FILE* f) {
}

We need to be sure that the controller has been initialised before it can be used. We will define a boolean variable set to false at the call of the constructor: basiccontroller.h

class BasicController : public AbstractController{
  ...
  private:
    bool initialised;
};

basiccontroller.cpp

BasicController::BasicController(const std::string& name)
  : AbstractController(name, "1.0") {
  initialised=false;
}

IR sensors give a value of 0 when there is no object within the range. Values are increasing when an object is getting closer to the sensors and a maximum of 1 can be read when the object is very close to the sensor.

We will define a threshold that will tell the robot when an object is “too close” to the robot in order to change the behaviour.

This threshold is a parameter of the controller that we would modify in order to have different behaviours of the robots. The simulator allow to define these parameters in a very easy way. This will allow the online modification of the value of the parameter at the command line or at the configurator window (we will see these functions in detail in the creating a simulation tutorial).

  addParameterDef("threshold", &threshold, 0.2, 0, 1, "threshold for IR-sensor");

The first parameter is the name (no space allowed), the second is a pointer to the variable, third is the default value, fourth and fifth parameters are the range of allowed values, and the sixth parameter is a description.

We define the variable at the header: basiccontroller.h

class BasicController : public AbstractController{
  ...
  private:
    ...
    double threshold;
};

And the final constructor looks like this: basiccontroller.cpp

BasicController::BasicController(const std::string& name)
  : AbstractController(name, "1.0") {
  initialised=false;
  // add threshold parameter to configurable parameters, setable on console
  addParameterDef("threshold", &threshold, 0.2, 0, 1, "threshold for IR-sensor");
}

Next step is to initialise the controller. The function init() is called with the number of sensors and motors provided by the robot (automatically calculated from the list of sensors and motors of the robot).

For this controller to work we need at least 2 motors and 8 sensors, so we set an assert to validate this.

  assert(motornumber >=2 && sensornumber >=8);

And then we store this values in private variables of the controller: basiccontroller.h

class BasicController : public AbstractController{
  private:
    ... 
    double nSensors;
    double nMotors;
    bool initialised;
};

At the end of this function we know that the controller is initialised so we change the flag.

The final init() function looks like this:

basiccontroller.cpp

void BasicController::init(int sensornumber, int motornumber, RandGen* randGen) {
  assert(motornumber >=2 && sensornumber >=8);
  // Set the number of sensors and motors
  nSensors = sensornumber;
  nMotors  = motornumber;
  initialised=true;
}

We update the functions getSensorNumber() and getMotorNumber() to return the values stored in the private variables:

basiccontroller.cpp

int BasicController::getSensorNumber() const {
  return nSensors;
}
 
int BasicController::getMotorNumber() const {
  return nMotors;
}

Now for the real control of the robot the function step() is called at each time step by the simulator. The parameters are pointers to an array of sensors and motor as well as the number of sensors and motors.

Usually we divide internally the function step() in two function where one realises a step without changing internal parameters of the controller (stepNoLearning()) and other that do change them. In this case we will be using just the first one since we don't want to change any internal parameter (for example the threshold). The step() function calls stepNoLearning():

basiccontroller.cpp

void BasicController::step(const sensor* sensors, int sensornumber,
                           motor* motors, int motornumber) {
  stepNoLearning(sensors,sensornumber, motors, motornumber);
}

In the stepNoLearning() function we will change the values of the motor commands based on the IR sensors. In order to access the right sensors we can make use of their names. The controller base class provides SIdx() and MIdx() to obtain the index of a sensor and motor respectively. First, if both frontal sensors have a high value we will make the robot to move backward, both motor will rotate backwards at maximum speed:

 if (sensors[SIdx("IR front left")] > 2*threshold ||
      sensors[SIdx("IR front right")] > 2*threshold) { // move backward
    motors[MIdx("left motor")] = -1.;
    motors[MIdx("right motor")] = -1.;
  }

In order to avoid objects on the left side of the robot, if any of the left side IR sensors have a value higher than the threshold the robot will turn right:

  ... else 
  if (sensors[SIdx("IR left")] > threshold ||
            sensors[SIdx("IR left front")] > threshold ||
            sensors[SIdx("IR front left")] > threshold) { // turn right
    motors[MIdx("left motor")] = .1;
    motors[MIdx("right motor")] = 1.;
  }

In order for the differential robot to rotate to the right the left wheel have to move faster than the right wheel. To avoid objects in the right side is analogous.

The default action is to move forward in full speed:

    motors[MIdx("left motor")] = 1.;
    motors[MIdx("right motor")] = 1.;

The final function looks like this:

void BasicController::stepNoLearning(const sensor* sensors, int number_sensors,
                                     motor* motors, int number_motors) {
  // Very simple controller, if IR sensors are higher than threshold then
  // the robot will turn oposite direction, or backward when to close
  // Sensors indices and motor indices can be accessed by the SIdx and MIdx functions
  // provided by the controller base class
  if (sensors[SIdx("IR front left")] > 2*threshold ||
      sensors[SIdx("IR front right")] > 2*threshold) { // move backward
    motors[MIdx("left motor")] = -1.;
    motors[MIdx("right motor")] = -1.;
  }else if (sensors[SIdx("IR left")] > threshold ||
            sensors[SIdx("IR left front")] > threshold ||
            sensors[SIdx("IR front left")] > threshold) { // turn right
    motors[MIdx("left motor")] = .1;
    motors[MIdx("right motor")] = 1.;
  }
  else if (sensors[SIdx("IR right")] > threshold ||
           sensors[SIdx("IR right front")] > threshold ||
           sensors[SIdx("IR front right")] > threshold) {
    motors[MIdx("left motor")] = 1.;
    motors[MIdx("right motor")] = .1;
  }
  else { // Move forward
    motors[MIdx("left motor")] = 1.;
    motors[MIdx("right motor")] = 1.;
  }
}

We have added a hierarchy by defining first the backward movement, then the left avoidance, right avoidance and finally forward movement.

The files for the controller basiccontroller.h and basiccontroller.cpp can be found in ode_robots/examples/basic at your installation.

Now that we have the robot and the controller we need to create the environment and bind everything together in one simulation. See the next tutorial.

creating_a_controller.txt · Last modified: 2014/08/22 10:29 by georg