Robotic Arm: Message Controller Software

A robot arm manipulator consist of joints and servo motors. It’s attached to a fixed base, and upon receiving command, executes movement actions. Moving is a two-step process: First, the arm needs to receive a command, and second it needs to translate this command into coordinated actions that will move the joints accordingly. This article focuses on the first question: How to send commands effectively to the arm?

In the last article, I assembled a 4 degree-of-freedom robotic arm kit for the Raspberry Pi. The kit provides a Raspberry Pi Motor hat that exposes UART, I2C, analog inputs, and enough pins to control 3 stepper and 15 servo motors. The Kit offers a complete control suite in the official repository, but I wanted to create my own software to control the arm.

This article presents a step-by-step implementation of a client-server connection protocol based on Python and TCP sockets. In particular, multiple clients can connect to the server, send messages as a header and payload data packet, and leave the connection opened as long as necessary. The server will be responsible for receiving client connections and messages, parsing the bytes to Python objects, and process them as movement action.

This article originally appeared at my blog admantium.com.

Investigating the Arm Kit

The official repository contains the setup, example code and programs for remote controlling the arm. It provides two interfaces to the end user: A GUI application written in TK which runs on the Raspberry PI itself, or on any computer that can make a socket connection to the Pi. Or a webserver that starts on the Pi and to which any client with the right IP address and port can connect. Looking into the source code files, we can learn this:

• The mechanism to transport movement messages is a TCP socket
• The TCP socket is created on the raspberry Pi with a fixed Port
• The TK GUI client, and the WebServer, accept user interactions, and send commands to the TCP socket server
• The server accepts the message, determines the concrete message content, and instructs the arm to move
• Messages are either simple command like X_minus, X_add, or compound positions of all axis that lead to a continuous movement

Design Considerations for Remote Controlling

With this knowledge, and the prior experience when designing the control software for my moving robot RADU, we can formulate the following requirements:

• The arm needs to be access via a network interface
• This interface can be written in Python and directly manipulate the arm
• Movement commands should be simple text commands
• Commands should be executed immediately
• When integrating ROS, a message gateway class can be added to transform ROS messages to the custom format

So, messages need to be send to the Pi. Effectively, that’s serial data over the network, providing code decoupling, giving the means to implement different transport mechanisms. I liked the simplicity of the TCP sockets used in the official example. And so I choose to replicate the same mechanism.

Implementing the Server

To open a TCP socket, Pythons' built-in socket library is used. This library is versatile and allows several types of sockets to be created. The particular socket type of our choice is AF_INET, which means TCP with IPv4, and the subtype SOCK_STREAM, a bidirectional, continuous data stream. We also define defaults such as the IP and port address.

The first iteration of the server code will open a socket object, bind itself to the specified IP and Port, and then listen to any incoming traffic. In a continuous loop, the sever waits for any received connection. It a client connects, it will print a message to the terminal, then decode and print the message that the client sends.

from socket import socket, AF_INET, SOCK_STREAM

SOCKET_HOST = 'localhost'
SOCKET_PORT = 11314
BUFFER_SIZE = 2048

with socket(AF_INET, SOCK_STREAM) as server:
  server.bind((SOCKET_HOST, SOCKET_PORT))
  server.listen()

  print(f'{repr(server)}\n >> Server started, waiting for connections')

  while True:
    client, addr = server.accept()
    with client:
      print(f'Connected from {addr}')
      msg= client.recv(BUFFER_SIZE).decode()
      print(f'Message: << {msg} >>')

This code works as a proof of concept. But connection handling is not very effective: It waits for exactly one client message, and then terminates the connection immediately again. Also, messages are read with a fixed buffer size. This wastes memory when the messages are way smaller, and it will be faulty when messages exceeding this limit arrive. Let’s improve this.

Server Iteration 2: Flexible Message Passing

Reading data from the client provides us with a set of very different decisions. First, how much data do we want to read? Second, how many data at a time? Assuming that messages are just strings, then we can read one character at a time, all characters until a certain stop signal (for example a newline symbol) or read all data with a fixed buffer size just as we used before.

A better option is to let the client decide and inform the server what is about to come. The protocol looks like this: First, send a message header that contains information about the message length. And then, send the message. I got this idea from this great socket tutorial.

In the following refined version, we first define the fixed length of the header message. Then, upon a successful connection, the first message is decoded and stored, and then converted to the integer value expected_length. And following this, the next message is read one character at a time until this expected length value is met.

HEADER_MSG_PADDING = 20
while True:
  client, addr = server.accept()

  with client:
    print(f'Connected from {addr}')

    header, msg = '', ''
    expected_length, current_length = 0,0

    header = client.recv(HEADER_MSG_PADDING).decode()
    expected_length = int(header)

    print(f'Header: <{header}>\nExpected length: {expected_length}')

    while not expected_length == current_length:
      char = client.recv(1).decode()
      msg += char
      current_length += 1

    print(f'Full Messages Received\n<< {msg} >>')
    print('Terminating connection..\n')

Server Iteration 3: Multithreaded Client Connection Handling

The final iteration adds another two requirements. First, the server accepts and keeps tracks of multiple client connection. Second, the clients decide when they want to close the connection by sending a dedicated message. And third, the server handles repeatedly receiving messages (herder fist, then payload) of the connected clients.

Let’s start with the first part. The list of connected clients is a dictionary. When a new client connects, but still considered connected, an exception is raised. Otherwise, a new thread is created. Then, the new connection and the thread are stored in the connection list.

while True:
    try:
      client, addr = server.accept()

      if client_list.get(addr):
        raise ValueError

      connection_thread = Thread(target=handle_socket_connection, args=(client,))

      client_list[addr] = {'client': client, 'handler': connection_thread}

      print(f'Connected from {addr}')
      print('LIST', client_list)

      connection_thread.start()

    except Exception as e:
      print(e)

The handler method starts a continuous loop that checks for an incoming message. It parses the first message as the header, extracting the expected length. Then it reads all incoming data up to the expected message length. If the message contains just the string TERMINATE, the loop will close, which also stops the thread. Otherwise, the message content is handled, for example as shown here for a move command. At the end of this loop, the thread is put into a short sleep time. This is important, otherwise all those threads would continuously consume all available CPUs.

def handle_socket_connection(client, addr, client_list):
    header, msg = '', ''
    expected_length = 0

    is_alive = True

    while is_alive:
      header = client.recv(HEADER_MSG_PADDING).decode()
      if header:

        expected_length = int(header)
        print(f'Header: <{header}>\nExpected length: {expected_length}')

        msg = deserialize(client.recv(expected_length))

        if match('move_', msg):
          handle_move_command(msg)
        elif msg == 'TERMINATE':
          is_alive = False
        else:
          print(f'Message: \n<< {msg} >>')

      sleep(DELAY_MS)

    client_list.pop(addr)

Complete Server Source Code

The complete server source code is this:

#!/usr/bin/python3

#
# ---------------------------------------
# Copyright (c) Sebastian Günther 2021  |
#                                       |
# devcon@admantium.com                  |
#                                       |
# Created: 2021-08-09  20:41:12         |
# ---------------------------------------
#

from time import sleep
from pickle import loads as deserialize
from threading import Thread
from socket import socket, AF_INET, SOCK_STREAM
from re import match

SOCKET_HOST = 'localhost'
SOCKET_PORT = 11314
HEADER_MSG_PADDING = 10
DELAY_MS = 0.100

def handle_socket_connection(client, addr, client_list):
    header, msg = '', ''
    expected_length = 0

    is_alive = True

    while is_alive:
      header = client.recv(HEADER_MSG_PADDING).decode()
      if header:

        expected_length = int(header)
        print(f'Header: <{header}>\nExpected length: {expected_length}')

        msg = deserialize(client.recv(expected_length))

        if match('move_', msg):
          handle_move_command(msg)
        elif msg == 'TERMINATE':
          is_alive = False
        else:
          print(f'Message: \n<< {msg} >>')

      sleep(DELAY_MS)

    client_list.pop(addr)


def handle_move_command(msg):
  move_cmd = msg[5:len(msg)]
  print('MOVE', move_cmd)

with socket(AF_INET, SOCK_STREAM) as server:
  server.bind((SOCKET_HOST, SOCKET_PORT))
  server.listen()

  client_list = {}

  print(f'{repr(server)}\n >> Server started, waiting for connections')

  while True:
    try:
      client, addr = server.accept()

      if client_list.get(addr):
        raise ValueError

      connection_thread = Thread(target=handle_socket_connection, args=(client,addr, client_list))

      client_list[addr] = {'client': client, 'handler': connection_thread}

      print(f'Connected from {addr}')
      print('LIST', client_list)

      connection_thread.start()

    except Exception as e:
      print(e)

Implementing the Client

The server code is done, now let’s define the client code. Similar as before, the client code also starts simple: Connecting to the server, sending a message, and closing itself.

The first version of the client code looks very similar to the server: It uses the same library, sets the same constants, and will open a connection.

from socket import socket, AF_INET, SOCK_STREAM

SOCKET_HOST = 'localhost'
SOCKET_PORT = 11314
HEADER_MSG_PADDING = 10

def send_socket_msg(client, msg):
  msg = serialize(msg)
  msg_length = len(msg)

  client.send(f'{msg_length:<{HEADER_MSG_PADDING}}'.encode())
  client.send(msg)

with socket(AF_INET, SOCK_STREAM) as client:
  client.connect((SOCKET_HOST, SOCKET_PORT))

  for i in range(10):
    if (i % 2 == 0):
      send_socket_msg(client, 'move_x+')
    else:
      send_socket_msg(client, 'move_x-')

    sleep(1.342)

Client Code: Serializing Objects with Pickle

The second iteration optimized the message type. It’s both an optimization of speed, and optimization of the flexibility what a message contains. In Python, any object can be serialized with the Pickle library. The serialization step will convert the object to bytes, which is the native datatype for socket data anyway. So, instead of explicitly encoding a string, we serialize any object and send it.

from pickle import dumps as serialize

def send_socket_msg(client, msg):
  msg = serialize(msg)
  msg_length = len(msg)

  client.send(f'{msg_length:<{HEADER_MSG_PADDING}}'.encode())
  client.send(msg)

with socket(AF_INET, SOCK_STREAM) as client:
  client.connect((SOCKET_HOST, SOCKET_PORT))
  send_socket_msg(client, {'MOVE_X': 10, 'MOVE_Y': 30})

A possible - and for production use required - extension is to send information about the message type together with the header. This makes handling the message on the server side much easier.

Client Code: Interactive Message sending

The final iteration of the client code will not just open the connection, send a message, and close it. Instead, it will open a command prompt in the terminal, allowing the user to type in a any message that is send to the server. This can be done with the input command in the terminal,

with socket(AF_INET, SOCK_STREAM) as client:
  client.connect((SOCKET_HOST, SOCKET_PORT))

  while True:
    cmd = input(">> ")

    if cmd.lower() == "stop":
      break;
    else:
      send_socket_msg(client, cmd)

    sleep(0.01)

Testing

On the Raspberry Pi, server.py is started.

> python3 server.py

<socket.socket fd=3, family=AddressFamily.AF_INET, type=SocketKind.SOCK_STREAM, proto=0, laddr=('127.0.0.1', 11314)>
 >> Server started, waiting for connections

Then, we connect to it via client.py from any computer in the same network.

> python3 client_interactive_input.py

cmd: center

Once connected, just type any command in the specified language, and the arm will move accordingly.

LIST {('127.0.0.1', 36148): {'client': <socket.socket fd=4, family=AddressFamily.AF_INET, type=SocketKind.SOCK_STREAM, proto=0, laddr=('127.0.0.1', 11314), raddr=('127.0.0.1', 36148)>, 'handler': <Thread(Thread-1, initial)>}}
Header: <16        >
Expected length: 16
Message:
<< center >>

Conclusion

With the Raspberry Pi Robot ARM Kit you can build a 4 degree of freedom robotic manipulator. This article presented a step-by-step tutorial to implement a message controller software. In essence, a socker server is started on the Raspberry Pi, and then a client connects. To test different approaches, the article showed several iterations of the server code. First, messages can be parsed as strings or evaluated as Python data structures. Second, the server can just open one connection, or handle multiple client connections simultaneously that are stored in threads. Third, the message protocol can be improved by separating message header and message body in which the header determines the amount of bytes that the payload provides. The resulting implementation is versatile and robust. The next article continues this series and shows how to implement simple and complex arm movements.