MCA2 Puma Tutorial Chapter 3

From Mca2
Jump to: navigation, search



Simulating a single joint

Until now we have learned how to use the tool mcagui to create a user interface (front end). Now we shall have a look at how to implement back end controller software. So far the robot model does not really move in a realistic way. Real motors have a limited maximum velocity such that the joints cannot follow our slider movements instantaneously. Therefore we will now replace the guitest programmes with our first self-made back end. In this chapter we will develop a programme which simulates one joint of the robot. By starting this programme six times we yield the simulation of a complete 6-DOF robot.

Well prepared ?

The project directory is hopefully prepared as described at the beginning of this tutorial. We change into the main MCA2.4 directory and create our first module by: -C projects/mcap_puma JointSimulation

The script newModule creates the two files mJointSimulation.h and mJointSimulation.cpp in the puma project directory (the leading "m" indicates that the files contain a MCA2 module). These files contain the definition and implementation of the class mJointSimulation, which is derived from tModule. Every module offers five IO interfaces: ControllerInput, ControllerOutput, SensorInput, SensorOutput and Parameters.


After creation new module files contain some default definitions and default code. This code has to be deleted in most cases, but is helpful as it shows how to use the interfaces and define parameters. Do not forget to delete the defaults when continuing the tutorial.

MCA2 parameters are used to change internal module operand during runtime. They are manipulated using the tool mcabrowser. We will learn how to use these later. At the moment we only need the interfaces ControllerInput and SensorOutput.

Well understandable descriptions

All MCA2 IO interfaces are represented by vectors. Particular values of these vectors can be accessed using indices. As our code should be maintained readable for others as well we will define all input and output indices in terms of enumerations. These enumerations are already prepared in the generated file mJointSimulation.h. You only need to add the necessary values for ControllerInputs (eCI) and SensorOutputs (eSO) of our new module:

   * Anonymous enumeration type which contains the indices of the
   * controller inputs.
  DESCR( static, mJointSimulation, m_ci_description, 4, Natural, cDATA_VECTOR_END_MARKER );
  enum {
    eCI_DESIRED_JOINT_ANGLE,  /*!< Desired angle value of joint */
    eCI_DIMENSION  /*!< Endmarker and Dimension */

   * Anonymous enumeration type which contains the indices of the
   * sensor outputs.
  DESCR( static, mJointSimulation, m_so_description, 4, Natural, cDATA_VECTOR_END_MARKER );
  enum {
    eSO_CURRENT_JOINT_ANGLE,  /*!< Simulated joint angle value */
    eSO_DIMENSION  /*!< Endmarker and Dimension */

Now our module can receive one controller input value and set one sensor output value.

As we don't need any sensor input or controller output we do not change the according enumerations:

   * Anonymous enumeration type which contains the indices of the
   * controller outputs.
  DESCR( static, mJointSimulation, m_co_description, 4, Natural, cDATA_VECTOR_END_MARKER );
  enum {
    eCO_DIMENSION /*!< Endmarker and Dimension */

   * Anonymous enumeration type which contains the indices of the
   * sensor inputs.
  DESCR( static, mJointSimulation, m_si_description, 4, Natural, cDATA_VECTOR_END_MARKER );
  enum {
    eSI_DIMENSION /*!< Endmarker and Dimension */

Additionally we will define parameters (ePAR) which represent the joint's boundary values (minimal and maximal joint angle) and its angular velocity. For now only the names of the parameters have to be defined in mJointSimulation.h (do not forget to delete the default parameter definitions):

   * Anonymous enumeration type which contains the indices of the
   * parameters.
  enum {
    ePAR_MIN_ANGLE,         /*!< Minimum joint angle           */
    ePAR_MAX_ANGLE,         /*!< Maximum joint angle           */
    ePAR_ANGULAR_VELOCITY,  /*!< Angular velocity of the joint */
    ePAR_DIMENSION  /*!< Endmarker and Dimension */

If you wonder what eCI_DIMENSION, eSO_DIMENSION, and ePAR_DIMENSION are for: As they are always the last entry they specify the number of ControllerInputs, SensorOutputs, and Parameters respectively.

How to create and read in parameters will be explained in the next section.

All modules implement the methods Control and Sense which are invoked by the MCA2 main loop (See MCA Introduction). Method Control is meant to retrieve information from the ControllerInputs and compute the ControllerOutputs. Its counterpart Sense computes SensorOutput values from the module's SensorInputs. Both methods may modify the internal state of the module. At this point we want to implement a puma robot simulation which represents a virtual compound of sensors and actors. The simulation is passed ControllerInput values from which it computes simulated SensorOutput values which we will visualise in mcagui. This results in a shortened sensor-motor cycle in which we do not consider the SensorInputs and we need not compute ControllerOutputs. That way ControllerInput and SensorOutput are linked via the internal state of the module to be implemented.

Preparing parameters

Parameters can be manipulated using external tools like mcabrowser. In order to prevent accidental setting of inappropriate values parameters are well defined. The definition of numerical parameters (e.g. ParameterInt and ParameterFloat) requires the specification of a minimum and a maximum value. String parameters need to be limited in length. In our example we only need floating point values. These parameters are created within the constructor of our module which is implemented in mJointSimulation.cpp. The order of the creation sequence has to match the enumeration definitions for the parameters in the header file (do not forget to delete the default code for creating parameters at this point).

                new tParameterFloat("min angle value", 0, 2 * M_PI, 0),        //min,max,value
                new tParameterFloat("max angle value", 0, 2 * M_PI, 2 * M_PI), //min,max,value
                new tParameterFloat("angular velocity", 0, 100, 1)             //min,max,value

Because our parameter initialization above uses the constant M_PI, you also need to include the OwnMath.h header file at the beginning of mJointSimulation.cpp:

#include "mcal_math/OwnMath.h"

Whenever a parameter value is changed the method Exception is invoked by the main programme with type eET_ParameterChanged as parameter. The module code frame already defines method Exception. The parameter transfer has to be implemented as listed below (here the default-code for reading parameters has to be deleted as well).

You have to create an exception method as this should not be defined yet. Edit the file mJointSimulation.cpp at the bottom with the method Exception:

void mJointSimulation::Exception(tExceptionType type)

  switch (type)
  case eET_ParameterChanged:
        INFOMSG("%s>> Parameter changed.\n", Description());
        min_angle_value  = ParameterFloat(ePAR_MIN_ANGLE);
        max_angle_value  = ParameterFloat(ePAR_MAX_ANGLE);
        angular_velocity = ParameterFloat(ePAR_ANGULAR_VELOCITY);


This new method has to be added in the header file under public:

    void Exception(tExceptionType type);

In our example we only use floating point parameters which are read out using method ParameterFloat. For different parameter types MCA provides methods ParameterBool, ParameterString, etc.

Now that we have defined the interface of our class we can start to write the joint simulation code.

Simulation of one joint

Our simulation is very simple: If we get a new desired angle value, we move the current angle value towards this desired value each time the control method is called considering the angular velocity. If the target value has been reached we stop. In realtime (RTAI/LXRT) environments the control and sense methods are called with a constant period of time. In other environments this period is not really constant. Therefore we have to determine the time elapsed between two calls manually. Considering the elapsed time and the angular velocity, the resulting simulated joint angle can be determined in a straightforward fashion from the current joint angle which we always hold in an internal state variable. As the desired angle value is passed to the module via the ControllerInput this functionality is implemented in the Control method. Delete the default code in the Control and the Sense method. Then put the following code into file mJointSimulation.cpp.

// class mJointSimulation Control
void mJointSimulation::Control()
  if (ControllerInputChanged())
    // Reset the changed flag of the controller input
    // Get the desired angle:
    this->desired_joint_angle = ControllerInput(eCI_DESIRED_JOINT_ANGLE);

    // Out of Min/Max
    if (this->desired_joint_angle > this->max_angle_value)
      this->desired_joint_angle = this->max_angle_value;
    else if (this->desired_joint_angle < this->min_angle_value)
      this->desired_joint_angle = this->min_angle_value;

  // calculate the elapsed time from last access to control:
  double elapsed_time = (tTime::Now() - this->sense_timer).ToMSec() * 0.001;

  // reset timer:

  if (this->current_joint_angle != this->desired_joint_angle)
    // Not further than this:
    double max_delta_angle = elapsed_time * angular_velocity;

    // Get the differences:
    float diff = this->desired_joint_angle - this->current_joint_angle;

    float sign = 1;
    // Decrease or increase the angle:
    (diff > 0) ? sign = 1 : sign = -1;

    // too fast?
    if (fabs( diff ) > max_delta_angle)
      diff = sign * max_delta_angle;
    M_DEBUG("current_joint_angle = %f, sign = %f, diff = %f\n", this->current_joint_angle, sign, diff);
    float new_joint_angle = this->current_joint_angle + diff;

    // Save the new joint angle in a state variable
    this->current_joint_angle = new_joint_angle;
    this->current_joint_angle_changed = true;
    this->current_joint_angle_changed = false;

// class mJointSimulation Sense
void mJointSimulation::Sense()
  if (this->current_joint_angle_changed)
    SensorOutput(eSO_CURRENT_JOINT_ANGLE) = this->current_joint_angle;

Additionally add the following line to the constructor of our module src/mJointSimulation.cpp in order to set the timer for the first time and for initialisation of class variables:

  this->desired_joint_angle = 0.;
  this->current_joint_angle = 0.;
  this->current_joint_angle_changed = false;

In order to avoid unnecessary code invocation, flags have been introduced to indicate changes of ControllerInput and SensorInput.

Whenever a module changes its SensorOutput or ControllerOutput values this fact has to be indicated to other modules by calling method SetControllerOutputChanged or SetSensorOutputChanged respectively. In case nothing has been changed method ClearControllerOutput (ClearSensorOutput respectively) has to be called to clear the corresponding flag.

The flags indicating ControllerInput and SensorInput changes can be read out using the methods ControllerInputChanged and SensorInputChanged and are reset by ClearControllerInput and ClearSensorInput. In most cases modules only need to do something if the input values have changed. Note that our example represents a special case as we have implemented a simulation module.

To complete our first module we still have to introduce some internal state variables which have to be declared in the header file mJointSimulation.h:

  float desired_joint_angle;
  float current_joint_angle;
  bool current_joint_angle_changed;

  float min_angle_value;
  float max_angle_value;
  float angular_velocity;

  tTime sense_timer;

Class tTime is declared in the header file mcal_kernel/tTime.h which is included by mcal_kernel/tModule.h. As our module inherits from tModule no further includes are necessary in our header file.

Embed It (Ready)

Hooray, our first module is ready, but how can we build and execute it? For that purpose we need a part. MCA parts are independent processes and export all capabilities of an embedded module via TCP/IP to other parts and tools like mcagui and mcabrowser. Using these TCP interfaces several parts can communicate with each other. This results in distributed systems that may be executed on different PCs. If a pJointSimulations.cpp file is not already existent you can make one by typing into the directory console: -C projects/puma JointSimulation

If it is already existent you only need to overwrite the file with the partscript bellow.

The necessary main method and other definitions are already predefined in Main.h. As a user you just have to implement three methods:

  • initPartDescription: where help text and parameters can be defined
  • shutdown: where you are able to do things after stopping the execution of your programs
  • startup, which initiates the creation of your modules and finally invokes the control main loop.

Our example is very simple as we do not need any parameters and only create our mJointSimulation module during startup. We want our module to be executed every 30 milliseconds:

#include "puma/mJointSimulation.h"
#include "mcal_kernel/Main.h"

void initPartDescription()
  SetProgramDescription("\n This Program simulates a single joint.\n");

int startup(tPart* part)
  // Create the module
  mJointSimulation *joint = new mJointSimulation(part);

  return part->Execute(tTime().FromMSec(30));

void shutdown(tPart* part){}

Build everything (Steady)

Well, we changed from make to ninja build system with MCA2. To create an MCA linux program named mcap_puma which is composed of the files pJointSimulation.cpp and mJointSimulation.cpp the following lines have to be added or changed in CMakeLists.txt which is located in your project directory (~/mca2/projects/mcap_puma/src):

INCLUDE (MCADescriptionBuilder)








// projects/mcap_puma/src/CMakeLists.txt

The next change will be in another CMakeLists.txt file. This time it is located in the directory ~/mca2/projects/mcap_puma. We need to add a subdirectory and afterwards it should look like this:

# this is for emacs file handling -*- mode: cmake; indent-tabs-mode: nil -*-
# To be used by other modules


// projects/mcap_puma/CMakeLists.txt

Now you can change back into your main MCA2 directory and compile the project by calling over the scons interactive console which has to be terminated if it is still running and then restarted by:

 [build-dir] $ ninja mcap_puma

If mcap_puma isn't found for compiling you have to update with the command

 [mca2-home-dir] $ script/ic/

Tip: The tutorial is designed such that this procedure should complete without compiler errors. If you experience problems please check whether you have introduced type errors. In case of complications which you are sure result from a mistake in the tutorial or which you are unable to solve alone, please inform one of the tutors.

Execution (Go)

To start our freshly compiled program you have to build it by using the interactive scons console:

  [build-dir] $ mcap_puma

To execute the program you enter the command mcap_puma into the console and press enter. Now we will test the functionality. For that purpose we start mcagui again.

   [mca2-home-dir] $ mcagui

There we need a slider (you already know how to create and configure it) and a LCD-widget (to create this widget select item LCD in menu Sensors - I am sure you are able to move and resize it within the workspace.)

Connect the slider's actor output with the controller input Joint Value and the LCD's widget port with the IO channel Current Joint Angle. If you move the slider very fast, the simulated joint should need some time before it reaches the specified value as it has a limited angular velocity.

Tutorial1 2.1.png

Now we quit mcagui without saving our just created project and start our mcap_puma example another five times in five different consoles (Once for every joint of our puma robot). After that we start mcagui with puma.mcagui (the file we saved within our first mcagui session) as command line parameter (for this we have to be in the home folder of our saved session ..src/mca2/projects/puma/etc):

 [mca2-home-dir] $ mcagui puma.mcagui

or start mcagui again and open the saved GUI via menu.

 [mca2-home-dir] $ mcagui

We have to reconnect the sliders actor output and 3D-widgets sensor inputs with our programmes, because the IO descriptions have changed. After the reconnection we are able to control the simulated robot. The effect of our simulation is, that all axis need some time in order to rotate the joints to the specified angle values.

Tutorial1 2.9.png

Have a closer look

In order to check whether all parts are running and all connections are correct the tool mcabrowser can be used. Furthermore module parameters can be modified. At first we stop all joint_simulation programmes and restart only one of them. Then we start mcabrower:

$ killall joint_simulation      or     going into the console and pressing ctrl + c
[build-dir] $ joint_simulation 
[mca2-home-dir] $ mcabrowser 

Tutorial2 8.1.png

In the mcabrowser window all modules are listed in a tree structure under the top level node virtual MainGroup. By clicking on particular nodes of this tree specific information windows will pop up. Clicking on the top level node will bring forth a graph window which displays all parts that are currently executed (in our case we have only one part named JointSimulation) and how they exchange data via edges. Edges to the virtual controller input and sensor output of the whole system show the way how mcagui interacts with parts. All displayed components can be activated using the left mouse button. Just click on it and a separate window appears which displays additional information.

Tutorial2 8.5.png

If a module features parameters (as ours for example) these parameters can be manipulated during runtime in the corresponding information window. The module information window where all parameters are listed can be opened by clicking on the module in question. If you want to change parameter values, the change has to be confirmed by clicking on the Apply Parameters button in the window's utilities bar.

Tutorial2 8.6.png

The other IO interfaces of the module can be displayed by mcabrowser as well. If you click on one of the interfaces ControllerInput, ControllerOutput, SensorInput or SensorOutput the corresponding signal list together with the current values is shown. The values can be overwritten and sent to the controller programme by pushing the Apply button.

Tutorial2 8.7.png

Edges can also be displayed:

Tutorial2 8.8.png

Well, one part containing a single module is not really complex enough to demonstrate full utility of mcabrowser. We will now restart joint_simulation another five times and reconnect the mcabrowser. To reconnect set the focus on the mcabrowser main window and press ctrl + r. Confirm the reconnection with your local machine in the Connect dialogue which will pop up by pushing the Ok button.

Now six parts will appear in the virtual MainGroup graph window and we will be able to change the parameters of these mJointSimulation modules in different ways such that the six joints have different characteristics.

Tutorial2 8.9.png

< Back=== Forward>

Personal tools