MoDeS3 Lego Robot – Modelling and simulation of the robot arm

Simulation is an important means in complex cyber-physical and IoT applications as it can provide:

  1. analysis at design time of the development
  2. prediction at runtime

We have developed the physican model of the robot arm (crane) in the open-source OpenModelica framework.

It defines itself with the following goal: "The goal with the OpenModelica effort is to create a comprehensive Open Source Modelica modeling, compilation and simulation environment based on free software distributed in binary and source code form for research, teaching, and industrial usage.“ 

OpenModelica is complex, there are many built-in functions and libraries, and it is also able to compute the complex behaviours of hybrid systems. Our robot arm is inherently hybrid as the controller has discrete modes while the physical system is continuous.

We have built the model of the robot arm.

Lego robot - the physical reality

By decomposing the physical model into smaller pieces, we can get the hugh level modelica model, depicted in the following figure.

Mapping the robot arm to modelica

This hierarchical model can be further refined according to the parts of the physical component.

Two level of hierarchy of the physical model

We have used many different components of the library, we have parameterized them according to the measurements, and what we got is quite close to reality. We have done measurements and the model could predict the real physics with only 2-3% of error. This is a quite good result 🙂

The simulaton is provided to the controller as a service running on a separate linux virtual machine. A virtual controller is developed which compiles the uploaded modelica files with the given parameters and computes the simulation. The results can be showed by the web server of this virtual machine or the results can be sent back to the controller.

the overview of the architecture is depicted on the next figure.

This way the simulation at design and also at runtime is available through a simple interface!

And now let’s show a simple simulation scenario! The model of the next picture is being simulated.


The angle of the three motors in the function of time is depicted on the next picture.


As it can be seen, the three motors can work independently with different speed! Note that this is just a simple example where only some parameters are examined. However, the tool is able to evaluate more complex parameters and movements of the system.

Simulation is a useful feature both at design and also at runtime, various questions regarding the path of the robot arm then can be answered!

Integrating YAKINDU Statechart Tools with MQTT

As we mentioned earlier, we used YAKINDU Statechart Tools for the safety logic, that controls the model railways. In this post we are going to show, how we use MQTT in YAKINDU Statechart Tools, how we combine model based tools with IoT technologies.

We implemented data classes that are converted into the JSON format automatically by Gson. There is a general Command class which identifies what type of JSON message we are sending, and it determines the Payload of the message transfers.

E.g. if we transfer a direction status of a turnout, then the corresponding JSON looks like: {“command”:“SEND_TURNOUT_STATUS”, “content”:“{“id”:12, “status”: “STRAIGHT”}“} . It means that the turnout that has the ID 12 is set into the STRAIGHT direction.

Besides, we have a general PayloadHelper class whose sendCommandWithContent method can create a correct JSON message from a command and a jsonConvertible object. Note that jsonConvertible objects are those that are depicted on the figure above. First, it creates the corresponding JSON message from the command and the jsonConvertible, and finally it publishes the message to the broker through an MQTT Publisher object.

There is another method, called getPayloadFromMessage that extracts the payload from an MQTT Message and converts it to a Payload object whose content’s type always depends on the corresponding command’s value. In this way we implemented a general JSON to Java object conversion solution with the help of Gson.

Finally, let’s see how the Yakindu  model uses these functions. On the following figure a safety-logic specific command, called PASSAGE_REQUEST is sent to the corresponding direction: PASSAGE_REQUEST_TOP, PASSAGE_REQUEST_STRAIGHT, PASSAGE_REQUEST_DIVERGENT. It means the turnout asks the next turnout whether the train can enter its section. The content of the JSON message is generated and sent at the end of the method.

This is where statechart modelling in Yakindu meets IoT!

You can find all the sources codes, along with other modules (MQTT clients, YAKINDU Statechart Tools models, OpenCV codes, Complex Event Processing codes and models, etc) in our GitHub repository at

The integration of the Lego robot with the controller: using MQTT from Python

In the Lego robot subproject of the MoDeS3 it was important to integrate the sensor information from the Lego sensors, the control and also the logic responsible for the safety. For this purpose, we have implemented an advanced control protocol in Python. This script runs on an embedded Linux distribution, called EV3dev. With this operating system we can utilize the Lego devices connected to the EV3 brick, while have access to all generic Linux packages, like the mosquitto broker.

The simplified overview of depdendecies is depicted on the next picture:


Our logic is able to detect when the motors are overdriven, or the robot is getting to a twisted and dangerous position, and prevents it from further attacking its limits by stopping them. By this protocol other components can control the crane by MQTT messages, or stop it if any other sensor detects something dangerous.

The code snippet below runs in a cycle and if it notices chage in the state of the touch-sensor, sends a message through MQTT (Paho). If needed it also executes some safety routines.

using MQTT

Sensors of the hardware are depicted on the following figures: these sensors provide the information:


The solution is built modularly, each part is responsible for certain movements and sensor information. The control software enables the user to control any of the motors individually, and get back raw sensor data through MQTT. A safety modul is observes the behaviours and available information and intervenes if something goes wrong.

Controlling and ensuring safety of the Lego robot arm: the computer vision challenge

As the Lego robot arm executes a mission, information about the environment is required. Beside executing missions correctly, our goal is to detect any kind of danger caused by the robot. The goal is twofold: the robot has to know when to execute a mission i.e. the object to be moved is at the right place. Second, it has to stop when some dangerous situation happens, for example a human is present near to the robot.

We are building the monitoring infrastructure of the robot arm based on computer vision technologies. OpenCV helps us detecting and tracking the movements of the robot In case of a moving robot, no other moving objects should be there. In addition, computer vision will detect if the object to be transported is in the proper place to handle by the robot.

First time we built only a robot arm with limited functionality. According to the experiences, we have totally rebuilt it.

Rebuilding the robot was a big step forward for the project’s computer vision goals. Now, we are able to detect the orientation and also the movement of the arm, without markers.

The new concept is to put on some Lego element in a combination to form a larger component with distinctive shape and colour. The camera observes the whole loading area from the top, and searches for the elements.

Using the same camera frames, we can detect the orientation of the gripper and find the cargos and the train.

For the gripper we needed a marker and that Lego element which we talked about before. The marker is directly connected to the gripper’s motor, so it is moving with the gripper during the rotation and other movements. The orientation is compared to the arm’s orientation so it won’t change during the movement of the arm, only the rotation influences its settings. The marker is a black circle, therefore we replaced the color detection with circle detection for this case.

Cargo has distinctive color.

In the following picture the output of the various steps of the process is depicted.

Output of the image processing steps

In the following we just sketch the working of the detection algorithms. Transforming the picture of the camera to HSV (Hue-Saturation-Value) representation. This will serve as a base representation for further processing. The next step is to decompose the picture according to the information we are looking for. In order to ease the tasks of the further processing, the picture is cut into pieces: The Lego arm, the gripper and also the object to be moved will be in different pieces of the picture.

Detecting the various objects of interest, we need to assess the color and the size of the objects in the picture: this is assessed at the next phase of the processing.

Edge detection algorithms search for the contour of the objects. Pattern matching algorithms try to find rectangles in the picture.

Numerical filters than used to sort the found objects (rectangles) according to their size. From the filtered objects, some special heuristics filter those object which are likely to be the searched object, namely the arm, the gripper and the load to be moved.

The movement of the arm is traced by reducing the problem to finding the moving rectangles in the filtered picture. Computing averages and tracing the middle point of the objects provide quite precise results.

So, as you might see, many algorithms work on the control and safety assurance of the Lego robot arm. Despite its complexity, it works well in practice!

Complex Event Processing #2

So let’s continue the introduction of the complex-event processing work of our IoT challenge project.

In a former post you could read about the computer vision, which will provide the information for the complex-event processing engine. However, answering the question of what and how to process relies only on the complex-event processing. Now, we give some details about our extensions of the VIATRA-CEP framework. Note that it has not yet been merged: we plan to integrate them in the future!

Just a reminder about the workflow of the imagined CEP compiler:


The general idea of the extensions proposed in this project relies on our former work with VIATRA-CEP.

Regular languages were chosen according to their semantics, traditional automata transformation were planned to be used for supporting the work of the execution.

Now let’s see what have been implemented, and how it was done. We have developed the metamodel of the automata representation in EMF. In addition, several executor-related classes had to be developed in Xtend.

EMF model of the Automaton

As the intermediate language is intended to be used as a semantic integration layer, Xtext is used to implement a Regular Expression language.

Various transformations are used to generate the monitoring from the high level requirements description. As the regular formalisms are introduced into the system, we gain two main advantages:

  1. The semantics of the languages are familiar for the developers as regular languages are widely used in various areas of software engineering.
  2. Existing transformation algorithms could be utilized.

From the Regular Expressions, without timing and parameters, a transformation to automata is well-known in the literature so my task was quite simple. I found a well-specified algorithm and I implemented and integrated the compiler to the system. Also note that this algorithm generates a deterministic automaton which can be executed with a single active state, also known as token. This point is really important!

When using monitoring in resource constrained environments, it is useful to be able to give limits for the resource usage. This can be provided by deterministic automata.The timed part of the work was much more difficult!

One of our main goals was to keep the transformed automata deterministic – but as we found out it is mathematically impossible.


Our extensions will increase the usability of the VIATRA-CEP engine and hopefully enable us to limit the resource consumption of the engine. An additional advantage is that we plan to support the analysis of the CEP specification: this automata theoretic approach can help identifying design problems in the rules with the application of rigorous formal techniques.

Computer vision based safety-system: how to get the information

The system we described was originally operated by distributed units, called masters. These masters got the local information about occupancy through a special circuit integrated into the board. However, network problems often caused the error in the safety-logic, so we decided to introduce an additional layer of safety based on computer vision and complex event processing.

Now, we will give some details about the application of computer vision for recognizing the trains and their location.

The safety logic deployed to the embedded controllers have binary information of the trains, namely if a train is on a section of the system, we detect it. There is no information about the direction of travel, and speed. Because these limitations, the information of the safety logic is rather limited.

Because the logic itself cannot determine the direction, it must consider the worst-case scenarios. This causes deadlocks, and unnecessary stops. This is a price we pay for safety.

The previously mentioned solution operates in a distributed manner. It’s safe, it’s reliable, it’s formally verified. If everything works correctly.

So we decided to implement the runtime verification of the local components and we integrated the system level monitor based on computer vision. We show the later now in details.

Our monitoring solution is a computer vision based one, using the open-source OpenCV framework. OpenCV is a very extensive library of optimized image processing, machine learning algorithms, ideal for quick development of computer vision based applications. You are not worried about the performance and programming complexity.

There are other solutions maybe with better performance, however as OpenCV is open-source and there is a huge community behind it, our decision was straightforward.

This is an example marker we use on the top of the trains. There are three markers: red, green, and a blue one.

Our needs were pretty simple: identify the trains, and determine their positions. Circular patterns are great for this kind of computer vision tasks, because if you rotate a circle, nothing happens, therefore you don’t have additional complexity.

So we decided to use markers to make our task easier!

Many of the people reading this article may think about the Hough circle algorithm, which can find circular patterns. The problem with this algorithm is it’s genericity: our board may contain many circles, not just only the train. We needed an error prone algorithm, which can match a specific pattern, if only a partial circle is visible.

What we can do is use some math. Instead of traditional pattern matching, we can turn this into a math problem. Our pattern is very static. By static, I mean the circle pattern doesn’t vary by size. Because of this property, we can create a very specific matcher, using convolution. Convolution is basically two math functions, and we apply one function on the other. The resulting function is the combination of the two. Although convolution is quite difficult to achieve, but if we transfer our image to the frequency domain, the convolution basically becomes a multiplication which is easy to do.

Let’s see an example what is happening:

  1. We create a pattern image, with specific values. These values can be: 0, if we are not interested what’s really there; 1, if we want this area white; -1, if we want this area black.
  2. Convert this pattern image to frequency domain.
  3. Read the image from camera.
  4. Convert this image to the frequency domain.
  5. Multiply the two spectra.
  6. Convert the multiplied image back.

The pattern

This is a pattern, where green has value 0, white has value 1, and black has value -1

The image from the camera looking down the MoDeS3 board

The camera image’s, and the pattern’s convolution result

This is not a pitch black image, if you look closely, you can see the bright points, which marks the points where markers found

Now we have a weird-looking image, where a brighter spot means a bigger match between an image, and the pattern. On this image, we can use a simple threshold, and get a binary image, where it is really trivial to find the brightest points.

We are not saying this is the most efficient algorithm for this solution, but it’s really robust, and precise. The precision is in the millimetre range, and it’s robustness can be described as this solution does not make false detection. It might not detect valid points for a small time period, but we haven’t seen false reading, not even in a 8-hour-long session. On the other side, Complex Event Processing can solve the problem when false values are observed for a small time interval.

So what’s after the detection of the circle pattern? There is a color ID inside the two circle patterns, and this color identifies the train. What we do is search for points, where the distance of these two points exactly matches the distance in the real world. If we find a pair, we can be sure it’s a train marker. After idetifying all the visible trains, we convert the data to JSON, and publish it to the MQTT broker.

Our approach may seem a little non-standard, but it’s proven it’s reliability, and after all, that’s what matters for us.

Complex Event Processing #1

I am Laszlo, and I am currently working on a Complex Event Processing Engine, which could be later integrated to the VIATRA-CEP project. This post will present the theoretical aspects, and the some other things excluding the implementation.

The motivation behind all of my work is simple: In our Scientific Students’ Conference Report we developed a system with multiple levels of runtime verification, and the system level verification was implemented with Complex Event Processing. For that, we have used the Open Source VIATRA-CEP framework which is part of the well-known VIATRA Eclipse project.

The reason for choosing this incredibly novel framework, is simple: It can be easily integrated on the top of live models. To do so the user can define graph patterns with EMF-IncQuery on these EMF models and use the appearance/disappearance of such graph patterns as atomic events in defining complex event patterns.

VIATRA-CEP uses an expressive event pattern language for the complex event pattern definitions. This language is called Viatra Event Pattern Language (or VEPL for short). This language is great for clear CEP proposes, but it lacks a truly clear and analyzable semantics and execution. Without explaining the grammar of this language I just show you a simple illustrative example of usage.

VEPL example

Of course instead of using atomicEvents it would be wise to use query events but that would just make this example longer. To show you, what I am working on right now let’s take a closer look to the architecture of the VIATRA-CEP

Architecture of the VIATRA-CEP

To extend this system towards the world of runtime verification, our idea was to create a similar language to VEPL but with the semantics of regular expressions. Also our plans are to map the VEPL to our Regular Expression language for debug and analysis purposes.

Architecture with the intermediate language

To create this intermediate language layer so we first developed a Parametric Timed Regular Expression formalism, which extends the well-known regular expressions with timing and parameters. For accepting languages generated by parametric timed regular expressions, we introduced the concept of the parametric timed event automata.


It’s always good to help!

Yesterday I was playing around the Texas Instruments 4.4 real-time kernel. After installing the image it turned out the script copying the contents of the SD card to the eMMC script was broken.

After some poking around, and debugging I found the source of the mystery, and opened an issue with the solution.

Just after some hour, RobertCNelson, the maintainer of everything in the BeagleBone Black project issued a fix. That was fast!