Wearable Intertace for Teleoperation of Robot Arms - WITRA

[Update 19] How WITRA's code works
Update #10153  |  24 Dec 2014

Key-points of WITRA's code

Developing WITRA required a multi-threading implementation, in order to meet the real-time constraints. For that purpose we had to learn and implement a program that would perform the following tasks:

  • Read from 3 COM ports continuously;
  • Check for interrupt on a GPIO pin;
  • Parse incoming data;
  • Calculate the direct kinematics of the human arm;
  • Map the user's reachable space to the robot's one;
  • Format an outgoing string to be sent to a TCP server to which the robot is connected;
  • Send this data over TCP/IP;
  • Save a datalog containing teleoperation information in order to be possible to repeat those movements again and again;
  • Do all that in real-time.

The platform used was Windows Embedded Compact 7, and the programming environment, Visual Studio 2008. Before starting coding, a few steps were followed:

  • Prepare the programming environment, following the instructions on [1];
  • Create a new VC++ project in visual studio [2];
  • Download and include the Interrupt library to the project [3] [4];
  • Install WiFi drivers for Ambicom Wifi USB adapter.


With that done, we could start coding. The basic structure of the code is shown below:
That is, each of the three IMUs is connected to a COM port through which they send serial data to the Colibri module. There is also a interrupt event triggered by the gripper button. You can also check how the hardware is connected by checking Update 18 - WITRA's hardware - final version
The workerThread(), called MainThread() in our program is responsible for doing all the calculations. It receives data from the three IMUs, parses the strings, filters noise and then calculates the direct kinematics of the human arm. After that, the user's reachable space is mapped to the robot's work space. With the four variables calculated (px, py, pz, gripper), a string is formatted and sent to the TCP server on the robot's side. A statechart of the code is shown next, in order to better illustrate how the data flow happens. 


WITRA is versatile

Since WITRA is a motion capture system in its essence, we wouldn't develop it only for the purpose of working with robots. Therefore, we've implemented a message formatting way that can be read and interpreted by many different systems.

The string that is sent over TCP/IP to a TCP server is formatted using a Rapidjson standard [8]. The send string has the following form:


Or, in a text form (example values):


That means that the control mode is set to Impedance control, X, Y and Z position of the robot's end-effector is set to (0.6500,0,0) and the gripper is closed. 

As one can notice, the string can be easily changed to send any parameter desired by the user/developer. Furthermore, at the other end, the code has only to have rapidjson implemented (you can get it from [8]) in order to interpret and parse the string and retrieve useful information. 

Hence, WITRA could be not only used with robot arms, but also with other applications, such as:

  • Drones - controlling their flight position by moving the arm
  • Mouses - control the pointer on the screen
  • PowerPoint presentations - have you ever thought that the movie "Minority Report" could become reality? 
  • Remote control cars
  • 3D mouses for CAD
  • Gaming
  • Motion capture and analisys in physiotherapy sessions.
A whole range of possibilities can be achieved and it is only up to the developer to choose and implement them.

A guided explanation

The next video shows in details, line by line, how the final code works. It is quite long (32 minutes), but whatever you need to understand about the code is present there. I hope you enjoy it! 


Where can I find the code?

The final code can be found at https://github.com/VBazan/WITRAv11_FINAL. This repository contains the include files provided by Toradex (they have to be downloaded for the projects to work), as well as two VS2008 projects:

The first one is the actual code from WITRA. The second is an application that reads from the saved datalog files and repeats the last saved teleoperation movements. 


[1] - How to setup development environment for WinCE and VS2008

[2] - Create a new VC++ project

[3] - How to include libraries to VC++ project in VS2008

[4] - How to use Interrupt library

[5] - How to use UART

[6] - Wifi

[7] - How to send and receive data over Ethernet

[8] - Rapidjson


Make sure you also follow our news and updates on our social channels below. 

youtube       facebook       email