NewRobot

Prev: Implementing a new world object	Up: Home	Next: FARSA APIs Documentation

Table of contents

• Implementing a new robot
  • Introduction
  • The physical model
  • Sensor and motor controllers
  • Kinematic model
  • Component for robots
  • Wheeled robots and the Arena component

Implementing a new robot

FARSA by default supports the simulation of some robots, but it is possible to create models for custom robots using the Worldsim library. In this page we will describe how to to this and show code snippets taken from the implementation of the Khepera robot.

Introduction

Robots in FARSA are implemented as subclasses of WObject (see this page for details). They are a collection of PhyObjects, plus sensor and motor controllers. Robots in FARSA are generally implemented in two steps. For example, there are two classes for the Khepera robot, PhyKhepera and Khepera. The former is the actual model of the robot, with all the low-level sensor and motor controllers. The latter is the associated component, that can be used as the robot type in configuration files (as, for example, in the [ROBOT] subgroup of the EvoRoboExperiment component). We will first show how to implement the model, taking the Khepera robot as an example, and then we will demostrate how to create a component wrapping the robot model.

The physical model

The Kehpera robot in FARSA is a model of the Khepera II robot developed by K-Team. It is a small differential wheeled robot with a circular body surrounded by eight infrared sensors. The following code snippet shows the declaration of the PhyKhepera class:

:::C++
class PhyKhepera : public WObject
{
    Q_OBJECT

public:
    // Static constants with robot dimensions
    [...]

public:
    // Constructor and destructor
    PhyKhepera(World* world, QString name, const wMatrix& transformation = wMatrix::identity());
    virtual ~PhyKhepera();

    // Accessors for low-level motor and sensor controllers
    WheelMotorController* wheelsController();
    SimulatedIRProximitySensorController* proximityIRSensorController();

    // Virtual functions from WObject
    virtual void preUpdate();
    virtual void postUpdate();

    // Functions to manage graphical properties of the robot
    void setProximityIRSensorsGraphicalProperties(bool drawSensor, bool drawRay = false, bool drawRealRay = false);
    void setDrawFrontMarker(bool drawMarker);
    bool getDrawFrontMarker() const;

    // Functions to set and get whether the robot model is dynamic or a simpler (and faster) kinematic one
    void doKinematicSimulation(bool k);
    bool isKinematic() const;

protected slots:
    // Slots used when in kinematic mode
    void setLeftWheelDesideredVelocity(real velocity);
    void setRightWheelDesideredVelocity(real velocity);

protected:
    // Virtual function from WObject
    virtual void changedMatrix();

private:
    // Data members
    [...]
};

The class has several functions, whose aim is briefly described in the comments above. In this section we will describe the constructor, destructor and the changedMatrix functions. The functions managing the graphical properties of the robot allow the user to decide whether to display an arrow indicating the front of the robot or not and whether to show the position of infrared sensors or not. The other functions will be described in the subsequent sections.

The physical model of the robot is created in the constructor of the class, which is displayed below (some code has been omitted):

:::C++
PhyKhepera::PhyKhepera(World* world, QString name, const wMatrix& transformation)
    : WObject(world, name, transformation, false)
    , [...] // Data members initialization
{
    // Creating a material for the khepera
    world->materials().createMaterial("kheperaMaterial");
    world->materials().setProperties("kheperaMaterial", "default", 0.0f, 0.0f, 0.01f, 0.01f, true);
    world->materials().createMaterial("kheperaTire");
    world->materials().setProperties("kheperaTire", "default", 1.2f, 0.9f, 0.01f, 0.01f, true);

    // Now creating the body of the khepera.
    {
        // Creating the body
        m_body = new PhyCylinder(bodyr, bodyh, world, "body");
        m_bodyTransformation = wMatrix::yaw(toRad(90.0f));
        m_bodyTransformation.w_pos.z = bodydistancefromground + (bodyh / 2.0f);
        m_body->setMatrix(m_bodyTransformation * matrix());
        m_body->setMass(bodym);
        m_body->setMaterial("kheperaMaterial");
        m_body->setOwner(this, false);
        m_bodyInvTransformation = m_bodyTransformation.inverse();
    }

    // Creating the first motorized wheel and its joint
    {
        // The matrix is relative to the robot frame of reference. First creating the wheel
        wMatrix tm = wMatrix::identity();
        tm.w_pos = wVector(axletrack / 2.0f, 0.0f, wheelr);
        PhyObject* wheel = new PhyCylinder(wheelr, wheelh, world, "motorWheel");
        [...]
        m_wheels.append(wheel);
        m_wheelsTransformation.append(tm);

        // Now creating the joint
        PhyJoint* joint = new PhySuspension(wVector::Z(), m_body->matrix().untransformVector(wheel->matrix().w_pos), wVector::X(), m_body, wheel);
        joint->dofs()[0]->disableLimits();
        joint->setOwner(this, false);
        joint->updateJointInfo();
        m_wheelJoints.append(joint);
    }

    // Creating the second motorized wheel and its joint
    [...]

    // Creating the first passive wheel and its joint
    {
        // The matrix is relative to the robot frame of reference. First creating the wheel
        wMatrix tm = wMatrix::identity();
        tm.w_pos = wVector(0.0f, bodyr - passivewheelr, passivewheelr);
        PhyObject* wheel = new PhySphere(passivewheelr, world, "passiveWheel");
        [...]
        m_wheels.append(wheel);
        m_wheelsTransformation.append(tm);

        // Now creating the joint
        PhyJoint* joint = new PhyBallAndSocket(m_body->matrix().untransformVector(wheel->matrix().w_pos), m_body, wheel);
        joint->setOwner(this, false);
        joint->updateJointInfo();
        m_wheelJoints.append(joint);
    }

    // Creating the second passive wheel and its joint
    [...]

    // Now we can create the motor controller
    QVector<PhyDOF*> motors;
    motors << m_wheelJoints[0]->dofs()[0] << m_wheelJoints[1]->dofs()[0];
    m_wheelsCtrl = new WheelMotorController(motors, world);
    m_wheelsCtrl->setOwner(this, false);
    const real maxAngularSpeed = 1.1f * 2.0f * PI_GRECO;
    m_wheelsCtrl->setSpeedLimits(-maxAngularSpeed, -maxAngularSpeed, maxAngularSpeed, maxAngularSpeed);

    // Connecting wheels speed signals to be able to move the robot when in kinematic
    connect(m_wheelJoints[0]->dofs()[0], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setRightWheelDesideredVelocity(real)));
    connect(m_wheelJoints[1]->dofs()[0], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setLeftWheelDesideredVelocity(real)));

    // Creating the proximity IR sensors
    QVector<SingleIR> sensors;

    // Adding the sensors. The khepera has 8 proximity infrared sensors
    const double sensorsAngles[] = {-60.0, -36.0, -12.0, 12.0, 36.0, 60.0, 160.0, 200.0};
    for (unsigned int i = 0; i < 8; i++) {
        const double curAngle = sensorsAngles[i] - 90.0;
        const double radius = bodyr;

        wMatrix mtr = wMatrix::yaw(toRad(curAngle)) * wMatrix::roll(PI_GRECO / 2.0);
        mtr.w_pos.x = -bodydistancefromground;
        mtr.w_pos.y = radius * sin(toRad(curAngle));
        mtr.w_pos.z = radius * cos(toRad(curAngle));

        sensors.append(SingleIR(m_body, mtr, 0.00, 0.05, 10.0, 5));
    }
    m_proximityIR = new SimulatedIRProximitySensorController(world, sensors);

    // Adding the robot to the world
    world->pushObject(this);
}

The constructor creates all the "pieces" of the robot and connects them using joints. The Khepera is modelled as a cylinder to which the two motorized wheels are attached with a couple of hinge joints. Two additional small passive spherical wheels are placed in the front and in the back of the robot with a ball-and-socket joint to reduce friction with ground. Afterwards, the low-level motor and sensor controllers are created (they will be discussed in the next section). The last line of the function adds the robot to the World.

The destructor simply deletes all robot components and controllers in reverse creation order. The changedMatrix function, instead, is implemented to move all robot pieces as the robot is moved. Its implementation is the following:

:::C++
void PhyKhepera::changedMatrix()
{
    m_body->setMatrix(m_bodyTransformation * matrix());
    for (int i = 0; i < m_wheels.size(); i++) {
        m_wheels[i]->setMatrix(m_wheelsTransformation[i] * matrix());
    }
    foreach (PhyJoint* j, m_wheelJoints) {
        j->updateJointInfo();
    }
}

The function moves all bodies and then updates the joints. The code also explains why it is necessary to keep the relative transformation matrices of all bodies making up the Khepera.

Sensor and motor controllers

Each robot should have one or more sensor controller and one or more motor controller. These objects give low-level access to motors and sensors. The base classes for motors and sensors are similar and their public interface is the following:

:::C++
class SensorController : public Ownable
{
public:
    SensorController(World* world);
    virtual ~SensorController();
    virtual void update() = 0;
    bool isEnabled();
    void setEnabled(bool b);
    World* world();
private:
    [..]
};

class MotorController : public Ownable
{
public:
    MotorController( World* world );
    virtual ~MotorController();
    virtual void update() = 0;
    bool isEnabled();
    void setEnabled( bool b );
    World* world();
private:
    [..]
};

They both can have the same owner/owned relationship of WObjects (see this page for more information). The only function that is requested to be implemented in subclasses is the update function that is called to actually read sensor or activate motors. Moreover they can be enabled or disable.

The Khepera robot has one motor controller (to control the movement of the two motorized wheels) and one sensor controller (to read the status of the infrared proximity sensors). They are created in the constructor (see above and refer to the API documentation for the documentation of the specific controllers). The update of the sensor and motor controllers is performed in the postUpdate and preUpdate functions respectively, as shown below:

:::C++
void PhyKhepera::preUpdate()
{
    if (m_wheelsCtrl->isEnabled()) {
        m_wheelsCtrl->update();
    }

    if (m_kinematicSimulation) {
        wMatrix mtr = matrix();
        wheeledRobotsComputeKinematicMovement(mtr, m_leftWheelVelocity, m_rightWheelVelocity, wheelr, axletrack, world()->timeStep());

        setMatrix(mtr);
    }
}

void PhyKhepera::postUpdate()
{
    if (m_proximityIR->isEnabled()) {
        m_proximityIR->update();
    }

    tm = m_bodyInvTransformation * m_body->matrix();
}

The preUpdate function checks that the motor controller of the motorized wheels is enabled and performs an update if this is the case. It also contains some code regarding the behaviour of the robot in kinematic mode (explained in the next section). The postUpdate function reads the new status of sensors after the simulation has advanced one step. The last instruction also sets the transformation matrix of the robot: because of motor activation the pieces making up the robot could have moved. To keep the transformation matrix of the robot synced with that of the components, the position and orientation of the piece modelling the body is read and the position and orientation of the robot is computed.

Typically, after the robot is created, you will also need to create some motors and sensors to interface the low-level controllers with the robot controller. This page contains information on this topic.

Kinematic model

When creating a new robot it is advisable (though not necessary) to also define a kinematic model (all the robots already present in FARSA have a one). The aim of a kinematic model of the robot is to have a very computationally cheap representation of the robot that can be used to drammatically reduce simulation time in case an experiment does not require an accurate simulation of interactions among all the bodies in the world (e.g. in case of navigation setups). There is not a predefined method to implement the kinematic model, so the following ones are just guidelines. They are already followed by the robot classes in FARSA, though, and new robots should also follow them (they should also use the same names of methods).

Here are the functions that are specific for the kinematic modality of the Khepera:

:::C++
void PhyKhepera::doKinematicSimulation(bool k)
{
    if (m_kinematicSimulation == k) {
        return;
    }

    m_kinematicSimulation = k;
    if (m_kinematicSimulation) {
        for (int i = 0; i < m_wheelJoints.size(); i++) {
            m_wheelJoints[i]->enable(false);
        }

        m_body->setKinematic(true, true);
        for (int i = 0; i < m_wheels.size(); i++) {
            m_wheels[i]->setKinematic(true, true);
        }
    } else {
        m_body->setKinematic(false);
        for (int i = 0; i < m_wheels.size(); i++) {
            m_wheels[i]->setKinematic(false);
        }

        for (int i = 0; i < m_wheelJoints.size(); i++) {
            m_wheelJoints[i]->enable(true);
            m_wheelJoints[i]->updateJointInfo();
        }
    }
}

void PhyKhepera::setLeftWheelDesideredVelocity(real velocity)
{
    m_leftWheelVelocity = velocity;
}

void PhyKhepera::setRightWheelDesideredVelocity(real velocity)
{
    m_rightWheelVelocity = velocity;
}

The doKinematicSimulation function allows to switch between dynamic and kinematic behaviour. The switch can happend at any time during the simulation. When the kinematic model of the robot is used, all joints are disabled and all the pieces of the model are switched to kinematic mode (see API documentation of the PhyObject class for more information). In kinematic mode the motor controller cannot move the robot, because the joints on which it acts are disabled. For this reason a different approach needs to be used. First of all the desired velocity of the two wheels has to be recorded. This is done in the setLeftWheelDesideredVelocity and setRightWheelDesideredVelocity slots. They are connected to the signal of the joints DoF object emitted when the desired velocity changes (the connection is performed in the constructor, just after the motor controller is created, see above). Secondly the new position of the robot is computed in the preUpdate function and the robot position is changed (the utility function wheeledRobotsComputeKinematicMovement can be used for differential wheeled robots).

Component for robots

We have shown how to create the model of a new robot, with its low-level sensor and motor controllers. To be used directly in configuration files, however a component has to be created. The base class for robot components is Robot. This class inherits from ParameterSettableInConstructor, so configuration parameters are passed to the constructor (see this page for more information). To create a component for a robot inherit from both Robot and the robot model.

Wheeled robots and the Arena component

If you want to create a wheeled robot that can be used together with the Arena component, there is one further step to perform. Instead of inheriting directly from Robot, when creating a robot component, you should inherit from the RobotOnPlane class. This class has some methods that make it easier to use robots as if they were in a 2D environment. Furthermore it is needed for some sensors, that require a simple 2D model of the robots to pereceive them. Here is the code for the Khepera component, showing a typical implementation and the helper functions to simplify development:

:::C++
class Khepera : public RobotOnPlane, public PhyKhepera
{
public:
    Khepera(ConfigurationParameters& params, QString prefix);
    virtual void save(ConfigurationParameters& params, QString prefix) {}
    static void describe(QString type);
    virtual ~Khepera() {}

    virtual void setPosition(const Box2DWrapper* plane, real x, real y);
    virtual wVector position() const;
    virtual void setOrientation(const Box2DWrapper* plane, real angle);
    virtual real orientation(const Box2DWrapper* plane) const;
    virtual real robotHeight() const;
    virtual real robotRadius() const;
    virtual bool isKinematic() const;
    virtual QColor robotColor() const;
};

Khepera::Khepera(ConfigurationParameters& params, QString prefix)
    : RobotOnPlane(params, prefix)
    , PhyKhepera(extractWorld(params), extractRobotName(params, prefix, "khepera"), extractRobotTranformation(params, prefix))
{
    doKinematicSimulation(ConfigurationHelper::getBool(params, prefix + "kinematicRobot", false));

    const bool enableWheels = ConfigurationHelper::getBool(params, prefix + "enableWheels", true);
    const bool enableProximityIR = ConfigurationHelper::getBool(params, prefix + "enableProximityIR", false);
    const bool drawProximityIR = ConfigurationHelper::getBool(params, prefix + "drawProximityIR", false);
    const bool drawIRRays = ConfigurationHelper::getBool(params, prefix + "drawIRRays", false);
    const bool drawIRRaysRange = ConfigurationHelper::getBool(params, prefix + "drawIRRaysRange", false);

    wheelsController()->setEnabled(enableWheels);
    proximityIRSensorController()->setEnabled(enableProximityIR);
    setProximityIRSensorsGraphicalProperties(drawProximityIR, drawIRRays, drawIRRaysRange);

    setDrawFrontMarker(true);

    setColor(configuredRobotColor());
}

void Khepera::describe(QString type)
{
    RobotOnPlane::describe(type);

    Descriptor d = addTypeDescription(type, "The simulated wheeled Khepera II robot in a physical world", "This type models the wheeled Khepera II robot in a physical world. Note that to create a robot we need a resource named \"world\" with a valid World instance");
    d.describeBool("kinematicRobot").def(false).help("Whether only the kinematic behaviour of the robot has to be simulated or not", "If true only the kinematic properties of the robot are simulated (i.e. velocity); is false (the default) the whole dynamic of the robot is simulated");
    d.describeString("name").def("khepera").help("The name of the Khepera robot", "The name of the Khepera robot");
    d.describeString("transformation").def("").help("The initial transformation matrix for the robot", "The transformation matrix must be a 4x4 matrix. The format is: a11 a12 a13 a14; a21 a22 a23 a24; a31 a32 a33 a34; a41 a42 a43 a44");
    d.describeBool("enableWheels").def(true).help("If true enables the wheel motors", "When set to false the wheel motors are not enabled, so the robot cannot move by itself");
    d.describeBool("enableProximityIR").def(false).help("If true enables the proximity IR sensors", "When set to false the proximity infrared sensors are not enabled, thus making simulation a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
    d.describeBool("drawProximityIR").def(false).help("If true draws the proximity IR sensors", "When set to true draws the proximity IR sensors");
    d.describeBool("drawIRRays").def(false).help("If true draws rays of enabled IR sensors", "When set to true rays of the enabled IR sensors (i.e. proximity and ground infrared sensors) are drawn");
    d.describeBool("drawIRRaysRange").def(false).help("If true rays of enabled IR sensors are shown in their real range", "When drawIRRays is true, this parameter sets whether rays are drawn in their real range (the parameter is true) or instead only their direction is shown (the parameter is false)");

}

void Khepera::setPosition(const Box2DWrapper* plane, real x, real y)
{
    PhyKhepera::setPosition(positionOnPlane(plane, x, y));
}

wVector Khepera::position() const
{
    return matrix().w_pos;
}

void Khepera::setOrientation(const Box2DWrapper* plane, real angle)
{
    wMatrix mtr = matrix();
    orientationOnPlane(plane, angle, mtr);
    setMatrix(mtr);
}

real Khepera::orientation(const Box2DWrapper* plane) const
{
    return angleBetweenXAxes(plane->phyObject()->matrix(), matrix()) - PI_GRECO / 2.0;
}

real Khepera::robotHeight() const
{
    return PhyKhepera::bodydistancefromground + PhyKhepera::bodyh;
}

real Khepera::robotRadius() const
{
    return PhyKhepera::bodyr;
}

bool Khepera::isKinematic() const
{
    return PhyKhepera::isKinematic();
}

QColor Khepera::robotColor() const
{
    return PhyKhepera::color();
}

The constructor and describe methods are rather typical implementations, with the exception of the parameters that need to be passed to the constructor of PhyKhepera. The extractWorld, extractRobotName and extractRobotTranformation helper functions can be used to extract the parameters that need to be passed to the constructor of the PhyKhepera class. Implementing the other functions is generally rather simple, and for the more complicated calculations, helper functions are available to deal with the typical use cases.