How to write your own robot simulator ?

:!: DEPRECATED Information

This document explains how to simulate a new robot. In our example we'll simulate a two wheels robot.

Creation of the shared variable

Create the input data structure

First you need to create the structure of the shared variable, this is the shared variable used to control the robot. In our example we control the speed of each wheel.

class TwoWheelControl
{
        public:
                double left_speed, right_speed;
};
Write the serialization method

The serialization method is needed to write the data in the shared memory. The serialization uses the boost serialization library which ever contain serialization methods for most used types. for example : int, char, bool[15], std::string, std::list.

namespace boost
{
   namespace serialization
   {
            template<class Archive> void serialize(Archive &ar, TwoWheelControl &c, const unsigned int version)
        {
            ar & c.left_speed;
            ar & c.right_speed;
        }
    }
}

Simulate the robot in mgEngine

  • First you can write the header file
The robot object needs to inherit from class VirtualMotor.
  • The object needs to contain an instance of hugr::Store to read the commands from the shared memory.
#ifndef __TWO_WHEELS_SIMULATOR_HPP
#define __TWO_WHEELS_SIMULATOR_HPP
 
#include <hugr.hpp>
#include <string>
 
#include "VirtualMotor.hpp"
#define DLC_DECLARED_COMPONENT "TwoWheelsSimulator"
#include <dlc/dlcDynamic.h>
 
#include "TwoWheelsStruct.hpp"
 
class DLC_DYNAMIC TwoWheelsSimulator :
    public VirtualMotor
{
protected:
    std::string name;
    hugr::Store store;
    hugr::VariableId controlId;
    TwoWheelsControl control;
 
    /* Matrice homogene de position. */
    mutable Mat4 position;
    double x, y, theta;
 
public :
    TwoWheelsSimulator( const std::string & Name );
    ~TwoWheelsSimulator();
 
    const std::string ClassName( void ) const
             { return "TwoWheelsSimulator"; }
    void init (const std::string & filename);
    void reinit (void); 
    void step(const double dT);
    const Mat4& getMotorPosition() const;
};
#endif
  • Now you write the implementation.
#include "TwoWheelsSimulator.hpp"
#include <cmath>
#include <stdexcept>
 
#define DLC_COMPILED_COMPONENT "TwoWheelsSimulator"
#include <dlc/dlc.h>
 
#include "hugr.hpp"
 
using namespace std;
using namespace hugr;
 
DLC_FACTORY_1( mgScriptable, TwoWheelsSimulator, const string& );
 
 
#define ROBOT_RADIUS 0.20
#define WHEEL_RADIUS 0.04
 
TwoWheelsSimulator::TwoWheelsSimulator( const string &name )
    : VirtualMotor(name)
    , name(name)
    , position()
    , x(0.)
    , y(0.)
    , theta(0.)
{
    control.left_speed = 0;
    control.right_speed = 0;
    init("");
}
 
TwoWheelsSimulator::~TwoWheelsSimulator()
{
}
 
void TwoWheelsSimulator::init( const string & filename )
{
    store.connect(); //connect to the store
    try
    {
         //try to find the variable
         controlId = store.lookupVariable( name+"_control" );
         store.readVariable(controlId, control);
    }
    catch(runtime_error &e)
    {
        //if the variable doesn't exit the exeption is cought and
        //we create a new variable
        controlId = store.registerVariable(name+"_control", control);
    }
    store.writeVariable(controlId, control);
}
 
void TwoWheelsSimulator::reinit (void)
{
}
 
void TwoWheelsSimulator::step( const double dT )
{
    //retrieve the robot command
    store.readVariable(controlId, control);
 
    double dleft = WHEEL_RADIUS * 2 * M_PI * control.left_speed * dT;
    double dright = WHEEL_RADIUS * 2 * M_PI * control.right_speed * dT;
 
    double dtheta = atan2(dleft, ROBOT_RADIUS) - atan2(dright, ROBOT_RADIUS);
    double dl = (dleft + dright) / 2;
 
    x += dl * cos(theta);
    y += dl * sin(theta);
    theta += dtheta;
}
 
const VirtualMotor::Mat4& TwoWheelsSimulator::getMotorPosition() const
{
    const double cth = cos(theta);
    const double sth = sin(theta);
 
    position[0]  = cth;
    position[1]  = -sth;
    position[2]  = 0.0;
    position[3]  = x;
 
    position[4]  = sth;
    position[5]  = cth;
    position[6]  = 0.0;
    position[7]  = y;
 
    position[8]  = 0.0;
    position[9]  = 0.0;
    position[10] = 1.0;
    position[11] = 0.0;
 
    position[12] = 0.0;
    position[13] = 0.0;
    position[14] = 0.0;
    position[15] = 1.0;
 
    return position;
}  
  • Now you can compile your code and create a dynamic library.

Using the robot in mgEngine

To use the robot created in the previous part, you need to import it in mgEngine, to do that, you can both use the mgEngine's shell or you can edit a script.

  • First, you need to declare the robot :
new TwoWheelsSimulator robot
robot.Init
robot.Start
  • Next, you can attach a mesh to the robot (in our example we use the mesh TwoWheelsRobot/robot.mesh):
new mgMeshActor robot_mesh
robot_mesh.SetMesh TwoWheelsRobot/robot.mesh
robot.AttachTo robot_mesh
  • The next point you would do, is to export intrinsic sensor like odometry, accelerometer, etc. To do so, see the section How to write your own sensor simulator ? or get an eye on the Cycab Simulator source code.
deprecated/howto/devel-robot.txt · Last modified: 2012/01/24 15:44 by arias
Recent changes RSS feed Creative Commons License Donate Minima Template by Wikidesign Driven by DokuWiki