User Tools

Site Tools


Sidebar

creating_a_robot

A robot consist of three basic parts:

  • Body
  • Actuators (motors)
  • Sensors

In this tutorial we will cover a differential robot (a robot with two independent wheels) with a motor on each wheel, speed sensors on each wheel and 8 infra-red sensors.

We will be calling our robot Differential, with its header and code files differential.h and differential.cpp.

Each robot has to inherit from the OdeRobot class:

Here is a basic structure for differential.h

// Header guard
#ifndef __DIFFERENTIAL_H
#define __DIFFERENTIAL_H
 
// Include ODE Robot class to inherit from it
#include <ode_robots/oderobot.h>
 
// ODE primitives
#include <ode_robots/primitive.h>
 
// ODE joints for objects
#include <ode_robots/joint.h>
 
// ODE angular motors
#include <ode_robots/angularmotor.h>
 
// ODE infrared distance sensors
#include <ode_robots/raysensorbank.h>
#include <ode_robots/irsensor.h>
 
// Using name space lpzrobots
namespace lpzrobots{
  /**
   * Differential robot: two separated wheel on each side of the body
   * Inherit from OdeRobot
   */
  class Differential: public OdeRobot{
    public:
      /**
       * Constructor
       */
      Differential(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
                   const DifferentialConf &conf = getDefaultConf(),
                   const std::string& name = "Differential");
 
      /**
       * Default configuration of the robot
       */
      static DifferentialConf getDefaultConf(){
      }
 
      /**
       * Destructor
       */
      virtual ~Differential();
 
      /**
       * Place the robot in the desired pose
       * @param pose desired 4x4 pose matrix
       */
      virtual void placeIntern(const osg::Matrix& pose);
 
      /**
       * Create the robot in the desired pose
       * @param pose desired 4x4 pose matrix
       */
      virtual void create(const osg::Matrix& pose);
 
    private:
    ...
  };
 
} // end namespace lpzrobots
 
 
// End of header guard
#endif

And a basic differential.cpp

#include "differential.h"
 
// Using namespaces
using namespace osg;
using namespace std;
namespace lpzrobots{
 
  Differential::Differential(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
                             const DifferentialConf& conf, const string& name)
    : OdeRobot(odeHandle, osgHandle, name, "2.0"), conf(conf){
  }
 
  Differential::~Differential(){
  }
 
  void Differential::placeIntern(const Matrix& pose){
  }
 
}

Creating the body

So far we know that our robot will look like a disk with two wheel on each side. We need to define the radius, the height and the mass of the body (disk) and the wheels. We start by defining a struct type that will hold the configuration of the robot.

differential.h

namespace lpzrobots{
...
  // structure to hold configuration of the robot
  typedef struct{
    double bodyRadius;          // Radius of the cylinder defining the body
    double bodyHeight;          // Height of the cylinder defining the body
    double bodyMass;            // Mass of the body
    double wheelRadius;         // Radius of the cylinder defining the wheel
    double wheelHeight;         // Height of the cylinder defining the wheel
    double wheelMass;           // Mass of the wheel
    double wheelMotorPower;     // Maximum power allowed to the motor to reach MaxSpeed
    double wheelMotorMaxSpeed;  // Maximum speed of the wheel
  } DifferentialConf; 
...
}

We define the function getDefaultconf() that will be called at the initialization of the robot with the default values for each parameter of the robot:

differential.h

      static DifferentialConf getDefaultConf(){
        DifferentialConf conf;
        conf.bodyRadius         = 1.;
        conf.bodyHeight         = .5;
        conf.bodyMass           = 1.;
        conf.wheelRadius        = .3;
        conf.wheelHeight        = .1;
        conf.wheelMass          = 5.;
        conf.wheelMotorPower    = 5.;
        conf.wheelMotorMaxSpeed = 5.;
        return conf;
      }

Note that the simulator does not work with units. The radius of the body could be in any measure of distance (cm, m, km, inch, yard, etc.) the only important part for the simulation is the relation between magnitudes (more information can be found in the ODE manual)

Placing the body

The first function that the simulation will call to create the robot will be placeIntern(const Matrix& pose). This function will place the robot in the desired position pose. Given the configuration of the robot (a disk with two wheels) we have to make two checks: 1) that the diameter of the wheels are bigger than the body (so the wheels touch the ground) and 2) that the position where the robot will be created does not collide with the ground (so we translate the body upwards).

For the first condition we use the assert() function that will stop the execution of the program if the condition is not true.

assert(2. * conf.wheelRadio > conf.bodyHeight);

For the second condition we translate the original pose in the z axis the size of the wheel. differential.cpp

Matrix initialPose;
initialPose = Matrix::translate(Vec3(0, 0, conf.wheelRadius) * pose);

After the checks, the function create() is called with the new position for the robot. Function place() looks like this, differential.cpp:

  void Differential::placeIntern(const Matrix& pose){
    // Configuration check: wheels have to be bigger than the body
    assert(2. * conf.wheelRadio > conf.bodyHeight);
    // Moving robot upward such that the wheel are not stuck on the ground
    Matrix initialPose;
    initialPose = Matrix::translate(Vec3(0, 0, conf.wheelRadius) * pose);
    // Creating the robot
    create(initialPose);
  }

Creating the primitives

The next step is to create the primitives for the body and the wheels. The function create() is called with the pose of the robot.

Body

First, we create the body of the robot, it is a cylinder primitive. The parameters for its creation are the radius and the height:

Primitive* body = new Cylinder(conf.bodyRadius, conf.bodyHeight);

We add a texture taken from the image library (see INSTALLPATH/share/lpzrobots/data/):

body->setTexture("Images/purple_velour.jpg");

After that, the primitive is ready to be initialized. The parameters are the OdeHandle to attach it to the physical engine, its mass and the OsgHandle to attach it to the visual engine.

body->init(odeHandle, conf.bodyMass, osgHandle);

Now the primitive is ready to be placed in the environment and to be added to the objects list:

body->setPose(pose);
objects.push_back(body);

The creation of the body looks like this, differential.cpp:

    void Differential::create(const Matrix& pose) {
    /* Creating body */
    // Cylinder geometry primitive as body
    Primitive* body = new Cylinder(conf.bodyRadius, conf.bodyHeight);
    // Setting texture from Image library
    body->setTexture("Images/purple_velour.jpg");
    // Initializing the primitive
    body->init(odeHandle, conf.bodyMass, osgHandle);
    // Setting the pose of the primitive
    body->setPose(pose);
    // Adding the primitive to the list of objects
    objects.push_back(body);
    ...
}

Wheel

In order to create the wheels we will create another cylinder that will be rotated and translated to the side of the body of the robot. Then it will be attached to the body by a joint that will allow it to rotate. Creating the left wheel cylinder primitive, adding a texture and initialising it:

    Primitive* lWheel = new Cylinder(conf.wheelRadius, conf.wheelHeight);
    lWheel->setTexture("Images/chess.rgb");
    lWheel->init(odeHandle, conf.wheelMass, osgHandle);

We need the new pose for the wheel. The original position pose of the robot is the center of the body's cylinder, we will have to translate the wheel to the side. Also, the orientation of the cylinder is horizontal so we will have to rotate it. As a general rule, when you need to rotate and translate a primitive first execute the rotation and then the translation. This will guarantee that the object is placed in the correct position.

To rotate an object you use the rotate() function. The first parameter is the amount of rotation, and the second to fourth parameter tells in what axis the rotation should be made. We want to rotate the wheel 90º in the y-axis:

      Matrix::rotate(M_PI / 2.0, 0, 1, 0)

Then we want to translate it to the side of the body plus half of the height of the wheel, the function translate() will take as parameters the amount of translation in each axis, in this case we want to move it on the x-axis:

      Matrix::translate(conf.bodyRadius + conf.wheelHeight / 2.0, .0, .0)

Sometimes we use the abbreviations ROTM() and TRANSM() to substitute the longer versions. To make the position of the wheel relative to the original pose we apply the rotation and transformation to the original pose. Mind the order of the rotation and the translation. The execution order is from left to right.

    Matrix lWheelPose =
      Matrix::rotate(M_PI / 2.0, 0, 1, 0) *
      Matrix::translate(conf.bodyRadius + conf.wheelHeight / 2.0, .0, .0) *
      pose;

Now we set the pose of the wheel and add it to the objects list:

    lWheel->setPose(lWheelPose);
    objects.push_back(lWheel);

Joint

We need to attach the wheel to the body through a joint. The class HingeJoint() represents a hinge joint between two primitives. The constructor parameters are the two primitives, the position where the anchor will be placed and the axis of rotation. We can also make use of the C++11 auto keyword here.

auto bodyLeftWheelJoint = new HingeJoint(body, lWheel,
                                         lWheel->getPosition(),
                                         Axis(0, 0, 1) * lWheelPose);

In this case the primitives are the body and the left wheel, the position of the anchor is the same as the pivot of the wheel, and the axis of ration relative to the wheel position is the z-axis.

We initialize the joint and add it to the list of joints:

    bodyLeftWheelJoint->init(odeHandle, osgHandle);
    joints.push_back(bodyLeftWheelJoint);

The right wheel is analogous to the left one but the translation is to the opposite side. Here is how the create() looks so far:

  void Differential::create(const Matrix& pose) {
    ...
    /* Creating the left wheel */
    Primitive* lWheel = new Cylinder(conf.wheelRadius, conf.wheelHeight);
    // Setting texture from Images library
    lWheel->setTexture("Images/chess.rgb");
    lWheel->init(odeHandle, conf.wheelMass, osgHandle);
    // The cylinder is rotated 90º on the Y axis
    // then translated outside the radius of the body plus half of
    // its own height
    // -- All transformations have to be relative to the position so
    // the robot can be initialised in any position --
    Matrix lWheelPose =
      Matrix::rotate(M_PI / 2.0, 0, 1, 0) *
      Matrix::translate(conf.bodyRadius + conf.wheelHeight / 2.0, .0, .0) *
      pose;
    // Setting the pose of the wheel
    lWheel->setPose(lWheelPose);
    // adding the wheel to the list of objects
    objects.push_back(lWheel);
    // Joining the wheel to the body by a hingejoint
    // the anchor comes from the wheel and the axis of rotation
    // is relative to the pose of the left wheel
    auto* bodyLeftWheelJoint = new HingeJoint(body, lWheel,
                                             lWheel->getPosition(),
                                             Axis(0, 0, 1) * lWheelPose);
    // Initializing the joint
    bodyLeftWheelJoint->init(odeHandle, osgHandle);
    // Adding the joint to the list of joints
    joints.push_back(bodyLeftWheelJoint);
 
    /* Creating the right wheel */
    // Analog to left wheel but changing translation direction
    auto rWheel = new Cylinder(conf.wheelRadius, conf.wheelHeight);
    rWheel->setTexture("Images/chess.rgb");
    rWheel->init(odeHandle, conf.wheelMass, osgHandle);
    Matrix rWheelPose = Matrix::rotate(M_PI / 2.0, 0, 1, 0) *
      Matrix::translate(-(conf.bodyRadius + conf.wheelHeight / 2.0), .0, .0) *
      pose;
    rWheel->setPose(rWheelPose);
    objects.push_back(rWheel);
    auto bodyRightWheelJoint = new HingeJoint(body, rWheel,
                                              rWheel->getPosition(),
                                              Axis(0, 0, 1) * rWheelPose);
    bodyRightWheelJoint->init(odeHandle, osgHandle);
    joints.push_back(bodyRightWheelJoint);
}

Actuators

In order for a robot to modify the environment it has to be able to act over it. For example, a hand, a leg or a wheel. In our case we need a motor in each wheel. A motor is attached to a joint and acts based on a command. The motor commands are assumed to be bounded to [-1,+1] values. For this robot we will add an angular motor (the motor command is the speed of rotation, with +1 as the maximum forward speed, -1 the maximum reverse speed and 0 means that the motor is stopped, real values are possible within [-1,+1] range).

After creating the joints, a motor can be attached to it. At the construction of the motor the OdeHandle, the joint and the maximum power that will be used by the motor to achieve the desired speed (motor command) are passed as parameters. For easy memory management we have to use smart-pointers of C++11 here, such that the creation is a bit different. For the left wheel:

differential.cpp

auto motor = std::make_shared<AngularMotor1Axis>(odeHandle, bodyLeftWheelJoint,
                                                 conf.wheelMotorPower);
motor->setBaseName("left motor"); 
motor->setVelovityFactor(conf.wheelMotorMaxSpeed);                                            

The motor can then be registered as a generic motor. Additionally the motor provides sensor information, in the case of the AngularMotor this is the actual rotation velocity. Thus we can use the Motor also as a generic sensor as follows: differential.cpp

    addSensor(motor);
    addMotor(motor);

The motor for the right wheel is analogous. Then the create() function is:

  void Differential::create(const Matrix& pose) {
    ...
    /* Motors */
    // Left wheel motor, the OdeHandle, the joint and the maximun
    // power that motor will be used to achieve desired speed
    auto motor = std::make_shared<AngularMotor1Axis>(odeHandle, bodyLeftWheelJoint,
                                                     conf.wheelMotorPower);
    motor->setBaseName("left motor");
    motor->setVelovityFactor(conf.wheelMotorMaxSpeed);
    addSensor(motor);
    addMotor(motor);
 
    // Right wheel motor
    motor = std::make_shared<AngularMotor1Axis>(odeHandle, bodyRightWheelJoint,
                                                conf.wheelMotorPower);
    motor->setBaseName("right motor");
    motor->setVelovityFactor(conf.wheelMotorMaxSpeed);
    addSensor(motor);
    addMotor(motor);
  }

Sensors

In order for the robot to perceive the environment we need to add sensors. These could be for sound, temperature, contact, proximity, pressure, acceleration, etc. In the present robot we will use the actual speed of the wheels as sensors (already achieved above) and add some infra-red (IR) proximity sensors such that the robot could avoid obstacles.

Infra-red sensors

IR sensors can tell you if the is an object within its range of measure, and how close it is. An IR sensor will report 0 if there is no object within its range, this value will increase when an object is found. The closer the object the higher the value.

For example, if you want to avoid your obstacles on your left side you would want to keep the IR sensors on your left closer to 0, and if the values goes higher a threshold you should start a escaping strategy.

We will create an array of 8 IR around the robot. With two pointing to the front, two pointing backward, one pointing to the left and one to the right, with two other pointing in diagonal. Similar to a Khepera configuration:

We define the range of the IR sensors, its default value and a bank collection of sensors:

differential.h

class Differential: public OdeRobot{
  public:
  ...
  typedef struct{
    ...
    double irRange;             // Range (max distance) of the infra-red sensors
  } DifferentialConf;
 
 
  static DifferentialConf getDefaultConf(){
    ...
    conf.irRange            = 2.;
    return conf;
  }
};

At the create() function, we create a sensor bank, which is a object to keep several IR sensors together. However, we could as well add the sensors individually.

differential.cpp

    auto irSensorBank = std::make_shared<RaySensorBank>();
    // Initialising infra-red sensor bank
    irSensorBank->setInitData(odeHandle, osgHandle, TRANSM(0,0,0));
    irSensorBank->setBaseName("IR ");
    irSensorBank->setNames({"left", "left front", "front left","front right",
          "right front", "right","back left", "back right"});

With the setNames() function we can provide the sensor names for each of the sensors. Otherwise they would be simply numbered which is less helpful here. The names can later be used in the controller to access the sensor values.

Then, each IR sensor is registered into the bank. For this, you need an IRSensor() object, the primitive to which it will be attached (in this case body), the pose relative to the attached body, the range, and a boolean that is true if you want a graphical representation of the sensors in the simulation.

For the first IR sensor we have to rotate -90º in the x-axis, then 90º in the z-axis, then translate to the radius of the body in the x- and y-axis, and to highest position of the body in the z-axis so the sensor is at the top of the vehicle:

    irSensorBank->registerSensor(new IRSensor(), body,
                                 Matrix::rotate(-M_PI / 2.0, 1, 0, 0) *
                                 Matrix::rotate( M_PI / 2.0, 0, 0, 1) *
                                 Matrix::translate(-conf.bodyRadius * sin(M_PI * .4),
                                                   conf.bodyRadius * cos(M_PI * .4),
                                                   conf.bodyHeight / 2.0),
                                conf.irRange,
                                RaySensor::drawAll);

Finally, we have to add the irSensorBank to the list of sensors with addSensor(irSensorBank). For all the IR sensors, the create() function is: (this code could be made more compact but we this way it is easier to read)

  void Differential::create(const Matrix& pose) {
    ...
    /* Infra-red sensors */
    auto irSensorBank = std::make_shared<RaySensorBank>();
    // Initialising infra-red sensor bank
    irSensorBank->setInitData(odeHandle, osgHandle, TRANSM(0,0,0));
    irSensorBank->setBaseName("IR ");
    irSensorBank->setNames({"left", "left front", "front left","front right",
          "right front", "right","back left", "back right"});
 
    // Registering the sensor in the bank (set of ir sensors), fixed to body
    // For the first sensor it is rotated to point forward
    // translation from center of body to outside and middle of height
    // pose is relative to the parent body - no need to multiply by 'pose'.
    // Maximum range of sensor value.
    // drawAll will display a line and the sensor body in the rendered scene.
    irSensorBank->registerSensor(new IRSensor(), body,
                                 Matrix::rotate(-M_PI / 2.0, 1, 0, 0) *
                                 Matrix::rotate( M_PI / 2.0, 0, 0, 1) *
                                 Matrix::translate(-conf.bodyRadius * sin(M_PI * .4),
                                                   conf.bodyRadius * cos(M_PI * .4),
                                                   conf.bodyHeight / 2.0),
                                conf.irRange,
                                RaySensor::drawAll);
 
    irSensorBank->registerSensor(new IRSensor(), body,
                                Matrix::rotate(-M_PI / 2.0, 1, 0, 0) *
                                Matrix::rotate( M_PI / 3.5, 0, 0, 1) *
                                Matrix::translate(-conf.bodyRadius * sin(M_PI * .25),
                                                  conf.bodyRadius * cos(M_PI * .25),
                                                  conf.bodyHeight / 2.0),
                                conf.irRange,
                                RaySensor::drawAll);
 
    irSensorBank->registerSensor(new IRSensor(), body,
                                Matrix::rotate(-M_PI / 2.0, 1, 0, 0) *
                                Matrix::translate(-conf.bodyRadius * sin(M_PI * .05),
                                                  conf.bodyRadius * cos(M_PI * .05),
                                                  conf.bodyHeight / 2.0),
                                conf.irRange,
                                RaySensor::drawAll);
 
    irSensorBank->registerSensor(new IRSensor(), body,
                                Matrix::rotate(-M_PI / 2.0, 1, 0, 0) *
                                Matrix::translate(conf.bodyRadius * sin(M_PI * .05),
                                                  conf.bodyRadius * cos(M_PI * .05),
                                                  conf.bodyHeight / 2.0),
                                conf.irRange,
                                RaySensor::drawAll);
 
    irSensorBank->registerSensor(new IRSensor(), body,
                                Matrix::rotate(-M_PI / 2.0, 1, 0, 0) *
                                Matrix::rotate(-M_PI / 3.5, 0, 0, 1) *
                                Matrix::translate(conf.bodyRadius * sin(M_PI * .25),
                                                  conf.bodyRadius * cos(M_PI * .25),
                                                  conf.bodyHeight / 2.0),
                                conf.irRange,
                                RaySensor::drawAll);
 
    irSensorBank->registerSensor(new IRSensor(), body,
                                Matrix::rotate(-M_PI / 2.0, 1, 0, 0) *
                                Matrix::rotate(-M_PI / 2.0, 0, 0, 1) *
                                Matrix::translate(conf.bodyRadius * sin(M_PI * .4),
                                                  conf.bodyRadius * cos(M_PI * .4),
                                                  conf.bodyHeight / 2.0),
                                conf.irRange,
                                RaySensor::drawAll);
 
    irSensorBank->registerSensor(new IRSensor(), body,
                                Matrix::rotate(M_PI / 2.0, 1, 0, 0) *
                                Matrix::translate(-conf.bodyRadius * sin(M_PI * .9),
                                                  conf.bodyRadius * cos(M_PI * .9),
                                                  conf.bodyHeight / 2.0),
                                conf.irRange,
                                RaySensor::drawAll);
 
    irSensorBank->registerSensor(new IRSensor(), body,
                                Matrix::rotate(M_PI / 2.0, 1, 0, 0) *
                                Matrix::translate(conf.bodyRadius * sin(M_PI * .9),
                                                  conf.bodyRadius * cos(M_PI * .9),
                                                  conf.bodyHeight / 2.0),
                                conf.irRange,
                                RaySensor::drawAll);
    addSensor(irSensorBank);
  }

The final files differential.h and differentail.cpp for this tutorial can be found in the 'ode_robots/example/basic' folder of your LpzRobots installation.

After finishing the creation of the robot you will need a controller and an environment. The next tutorial will show you how to set-up both of them.

creating_a_robot.txt · Last modified: 2014/08/22 10:00 by georg