Tutorial: close loop control of a robotic platform

Goal

This walk-through that shows how to:

  1. write, compile and install two blocks:
    • the plant (a two DoF robot that accepts velocity as input, and gives the relative position), and
    • the controller, that given as a property the set-point and the gain, computes the desired velocity to be set to the robot.
  2. instantiate the blocks via the ubx-launch tool, and
  3. instantiate the blocks with a C program.

All the files can be found in the examples/platform folder.

Introductory steps

First of all, we need need to define the interface of and between our two components.

The plant and controller have two ports, that exchange position and velocity, each of dimension two, and some properties (initial position and velocity limits for the plant, gain and setpoint for the controller). These properties are described it two lua files:

The plant, platform_2dof.lua

return block
{
   name="platform_2dof",
   license="MIT",
   meta_data="",
   port_cache=true,

   configurations= {
      { name="joint_velocity_limits", type_name="double", min=2, max=2 },
      { name="initial_position", type_name="double", min=2, max=2 },
   },

   ports = {
      { name="pos", out_type_name="double", out_data_len=2, doc="measured position [m]" },
      { name="desired_vel", in_type_name="double", in_data_len=2, doc="desired velocity [m/s]" }
   },

   operations = { start=true, step=true }
}

The controller, platform_2dof_control.lua

return block
{
   name="platform_2dof_control",
   license="MIT",
   meta_data="",
   port_cache=true,

   configurations= {
      { name="gain", type_name="double", min=1, max=1 },
      { name="target_pos", type_name="double", min=2, max=2 },
   },

   ports = {
      { name="measured_pos", in_type_name="double", in_data_len=2, doc="measured position [m]" },
      { name="commanded_vel", out_type_name="double", out_data_len=2, doc="desired velocity [m/s]" },
   },

   operations = { step=true }
}

Let us have these file in a folder (e.g. microblx_tutorial). From these file we can generate the two blocks using the the following bash commands

$ cd microblx_tutorial/
$ ubx-genblock -c platform_2dof.lua -d platform_2dof
generating platform_2dof/bootstrap
...
$ ubx-genblock -c platform_2dof_control.lua -d platform_2dof_control
generating platform_2dof_control/bootstrap
...

Each command generates a directory with the name specified after the -d with six files. For the plant, we will have:

  • boostrap
  • configure.ac
  • Makefile.am
  • platform_2dof.h
  • platform_2dof.c
  • platform_2dof.usc

The only files we will modify are the C files platform_2dof.c and platform_2dof_control.c.

Code of the blocks

The auto-generated files already give some hints on how to approach the programming.

#include "platform_2dof.h"

/* define a structure for holding the block local state. By assigning an
 * instance of this struct to the block private_data pointer (see init), this
 * information becomes accessible within the hook functions.
 */
struct platform_2dof_info
{
        /* add custom block local data here */

        /* this is to have fast access to ports for reading and writing, without
         * needing a hash table lookup */
        struct platform_2dof_port_cache ports;
};

/* init */
int platform_2dof_init(ubx_block_t *b)
{
        int ret = -1;
        struct platform_2dof_info *inf;

        /* allocate memory for the block local state */
        if ((inf = calloc(1, sizeof(struct platform_2dof_info)))==NULL) {
                ubx_err(b, "platform_2dof: failed to alloc memory");
                ret=EOUTOFMEM;
                goto out;
        }
        b->private_data=inf;
        update_port_cache(b, &inf->ports);
        ret=0;
out:
        return ret;
}

/* start */
int platform_2dof_start(ubx_block_t *b)
{
        /* struct platform_2dof_info *inf = (struct platform_2dof_info*) b->private_data; */
        ubx_info(b, "%s", __func__);
        int ret = 0;
        return ret;
}

/* cleanup */
void platform_2dof_cleanup(ubx_block_t *b)
{
        /* struct platform_2dof_info *inf = (struct platform_2dof_info*) b->private_data; */
        ubx_info(b, "%s", __func__);
        free(b->private_data);
}

/* step */
void platform_2dof_step(ubx_block_t *b)
{
        /* struct platform_2dof_info *inf = (struct platform_2dof_info*) b->private_data; */
        ubx_info(b, "%s", __func__);
}

We will need then to insert the code indicated by the comments.

Step 1: add block state and helpers

First add variables for storing the robot state, and implement the other helper functions. At the beginning of the file we insert the following code, to save the state of the robot:

struct robot_state {
	double pos[2];
	double vel[2];
	double vel_limit[2];
};

double sign(double x)
{
	if(x > 0) return 1;
	if(x < 0) return -1;
	return 0;
}

struct platform_2dof_info
{
	/* add custom block local data here */
	struct robot_state r_state;
	struct ubx_timespec last_time;

	/* this is to have fast access to ports for reading
         * and writing, without needing a hash table lookup */
	struct platform_2dof_port_cache ports;
};

The last_time variable is needed to compute the time passed between two calls of the platform_2dof_step function.

Step 2: Initialization and start functions

The init function is called when the block is initialized; it allocates memory for the info structure, caches the ports, and initializes the state given the configuration values (these values are specified in the .usc or main application file).

int platform_2dof_init(ubx_block_t *b)
{
	long len;
	struct platform_2dof_info *inf;
	const double *pos_vec;

	/* allocate memory for the block local state */
	if ((inf = calloc(1, sizeof(struct platform_2dof_info)))==NULL) {
		ubx_err(b,"platform_2dof: failed to alloc memory");
		return EOUTOFMEM;
	}

	b->private_data=inf;
	update_port_cache(b, &inf->ports);

	/* read configuration - initial position */
	len = cfg_getptr_double(b, "initial_position", &pos_vec);

	/* this will never assert unless we made an error
	 * (e.g. mistyped the configuration name), since min/max
	 * checking will catch misconfigurations before we get
	 * here. */
	assert(len==2);

	inf->r_state.pos[0] = pos_vec[0];
	inf->r_state.pos[1] = pos_vec[1];

	/* read configuration - max velocity */
	len = cfg_getptr_double(b, "joint_velocity_limits", &pos_vec);
	assert(len==2);

	inf->r_state.vel_limit[0] = pos_vec[0];
	inf->r_state.vel_limit[1] = pos_vec[1];
	inf->r_state.vel[0] = 0.0;
	inf->r_state.vel[1] = 0.0;

	return 0;
}

The function

long cfg_getptr_double(ubx_block_t *b, const char *name, const double **ptr)

returns the address of the double configuration in the pointer ptr. In this case the return value will be 2 (the length of the data) or -1 (failure, e.g. mistyped configuration name). Because we set min and max in the configuration declaration, we can be sure that at this point the array length is not anything but 2.

In the start function we only need to initialize the internal timer

int platform_2dof_start(ubx_block_t *b)
{
	struct platform_2dof_info *inf = (struct platform_2dof_info*) b->private_data;
	ubx_info(b, "platform_2dof start");
	ubx_gettime(&inf->last_time);
	return 0;
}

Step 3: Step function

In the step function, we compute the time since last iteration, read the commanded velocity, integrate to position, and then write position.

void platform_2dof_step(ubx_block_t *b)
{
	int32_t ret;
	double velocity[2];
	struct ubx_timespec current_time, difference;
	struct platform_2dof_info *inf = (struct platform_2dof_info*) b->private_data;

	/* compute time from last call */
	ubx_gettime(&current_time);
	ubx_ts_sub(&current_time, &inf->last_time, &difference);
	inf->last_time = current_time;
	double time_passed = ubx_ts_to_double(&difference);

	/* read velocity from port */
	ret = read_double_array(inf->ports.desired_vel, velocity, 2);
	assert(ret>=0);

	if (ret == 0) { /* nodata */
		ubx_notice(b,"no velocity setpoint");
		velocity[0] = velocity[1] = 0.0;
	}

	for (int i=0; i<2; i++) {
		/* saturate and integrate velocity */
		velocity[i] =
			fabs(velocity[i]) > inf->r_state.vel_limit[i] ?
			sign(velocity[i]) * (inf->r_state.vel_limit[i]) : velocity[i];

		inf->r_state.pos[i] += velocity[i] * time_passed;
	}
	/* write position in the port */
	ubx_debug(b, "writing pos [%f, %f]",
		  inf->r_state.pos[0], inf->r_state.pos[1]);
	write_double_array(inf->ports.pos, inf->r_state.pos, 2);
}

In case there is no value on the port, a notice is logged and the nominal velocity is set to zero. This will always happen for the first trigger, since the controller did step yet and thus has not produced a velocity command yet.

Step 4: Stop and clean-up functions

These functions are OK as they are generated, since the only thing we want to take care of is that memory is freed.

Final listings of the block

The plant is, mutatis mutandis, built following the same rationale, and will be not detailed here. The final code of the plant and the controller can be retrieved here:

Compiling the blocks

In order to build and install the blocks, you must execute the following bash commands in each of the two directories:

$ ./bootstrap
...
$ ./configure
...
$ make
...
$ sudo make install
...

See also the quickstart.

Deployment via the usc (microblx system composition) file

ubx-genblock generated sample .usc files to run each block independently. We want to run and compose them together and make the resulting signals available using message queues. The composition file platform_2dof_and_control.usc is quite self explanatory: It contains

  • the libraries to be imported,
  • which blocks (name, type) to create,
  • the configuration values of blocks.
  • the connections among ports

The file platform_2dof_and_control.usc is shown below:

-- -*- mode: lua; -*-

return bd.system
{
   imports = {
      "stdtypes",
      "ptrig",
      "lfds_cyclic",
      "platform_2dof",
      "platform_2dof_control",
      "mqueue"
   },

   blocks = {
      { name="plat1", type="platform_2dof" },
      { name="control1", type="platform_2dof_control" },
      { name="ptrig1", type="ubx/ptrig" },
   },

   configurations = {
      { name="plat1", config = {
	   initial_position={1.1,1},
	   joint_velocity_limits={0.5,0.5} }
      },

      { name="control1", config = {  gain=0.1, target_pos={4.5,4.5} } },

      { name="ptrig1", config = { period = {sec=0, usec=100000 },
				  sched_policy="SCHED_OTHER",
				  sched_priority=0,
				  chain0={
				     { b="#plat1" },
				     { b="#control1" } } } },
   },

   connections = {
      { src="plat1.pos", tgt="control1.measured_pos" },
      { src="control1.commanded_vel",tgt="plat1.desired_vel" },
      { src="plat1.pos", type="ubx/mqueue" },
      { src="control1.commanded_vel", type="ubx/mqueue" },
   },
}

It is worth noting that configuration types can be arrays (e.g. target_pos), strings (file_name and report_conf) and structures (period) and vector of structures (chain0). Types can be checked using ubx-modinfo:

$ ubx-modinfo show platform_2dof
module platform_2dof
  license: MIT

  blocks:
    platform_2dof [state: preinit, steps: 0] (type: cblock, prototype: false, attrs: )
       configs:
           joint_velocity_limits [double] nil //
           initial_position [double] nil //
       ports:
           pos  [out: double[2] #conn: 0] // measured position [m]
           desired_vel [in: double[2] #conn: 0]  // desired velocity [m/s]

The file is launched with the command

ubx-ilaunch -c platform_2dof_and_control.usc

or

ubx-ilaunch -webif -c platform_2dof_and_control.usc

to enable the web interface at localhost:8888 .

To show the position and velocity signal, use the ubx-mq tool:

$ ubx-mq list
e8cd7da078a86726031ad64f35f5a6c0  2    vel_cmd
e8cd7da078a86726031ad64f35f5a6c0  2    pos_msr

$ ubx-mq read pos_msr
{1.1,1}
{1.13403850806,1.03503964065}
{1.1679003576875,1.0698974270313}
{1.2012522276799,1.1042302343764}
{1.2342907518755,1.1382404798718}
...

Some considerations about the fifos

First of all, consider that each (iblock) fifo can be connected to multiple input and multiple output ports. Consider also, that if multiple out are connected, if one block read one data, that data will be consumed and not available for a second port.

The more common use-case is that each outport is connected to an inport with it’s own fifo. If the data that is produced by one outport is needed to be read by two oe more inports, a fifo per inport is connected to the the outport. If you use the DSL, this is automatically done, so you do not have to worry to explicitly instantiate the iblocks. This also happens when adding ports to the logger.

Deployment via C program

Warning

the following example is to illustrate the possibility of C only lauching, however generally, the usc DSL should be preferred. Furthermore, an usc compiler that can automatically and safely generate the code below is planned. If interested, please ask on the mailing list.

This example is an extension of the example examples/C/c-launch.c. It will be clear that using the above DSL based method is somewhat easier, but if for some reason we want to eliminate the dependency from Lua, this example show that is possible.

First of all, we need to make a package to enable the building. This can be done looking at the structure of the rest of packages.

we will create a folder called platform_launch that contains the following files:

  • main.c
  • Makefile.am
  • configure.am

Setup the build system starting from the build part

configure.ac

m4_define([package_version_major],[0])
m4_define([package_version_minor],[0])
m4_define([package_version_micro],[0])

AC_INIT([platform_launch], [package_version_major.package_version_minor.package_version_micro])
AM_INIT_AUTOMAKE([foreign -Wall])

# compilers
AC_PROG_CC

PKG_PROG_PKG_CONFIG
PKG_INSTALLDIR

AC_CONFIG_HEADERS([config.h])
AC_CONFIG_MACRO_DIR([m4])

# Check if the `install` program is present
AC_PROG_INSTALL

m4_ifdef([AM_PROG_AR], [AM_PROG_AR])
LT_INIT(disable-static)

PKG_CHECK_MODULES(UBX, ubx0 >= 0.9.0)

PKG_CHECK_VAR([UBX_MODDIR], [ubx0], [UBX_MODDIR])
  AC_MSG_CHECKING([ubx module directory])
  AS_IF([test "x$UBX_MODDIR" = "x"], [
  AC_MSG_FAILURE([Unable to identify ubx module path.])
])
AC_MSG_RESULT([$UBX_MODDIR])

AC_CONFIG_FILES([Makefile])
AC_OUTPUT

Makefile.am

ubxmoddir = ${UBX_MODDIR}

ACLOCAL_AMFLAGS= -I m4
ubxmod_PROGRAMS = platform_main
platform_main_SOURCES = main.c
platform_main_CFLAGS = @UBX_CFLAGS@ \
		       -I${top_srcdir}/../../../std_blocks/trig/types/

platform_main_LDFLAGS = -module -avoid-version -shared -export-dynamic  @UBX_LIBS@ -ldl -lpthread

Here, we specify that the name of the executable is platform_main It might be possible that, if some custom types are used in the configuration, but are not installed, they must be added to the CFLAGS:

platform_main_CFLAGS = -I${top_srcdir}/libubx -I path/to/other/headers @UBX_CFLAGS@

In order to compile, we will use the same commands as before (we do not need to install).

autoreconf --install
./configure
make

The program

The main follows the same structure of the .usc file.

Logging

Microblx uses realtime safe functions for logging. For logging from the scope of a block the functions ubx_info, ubx_info, etc are used. In the main we have to use the functions, ubx_log, e.g.

ubx_log(UBX_LOGLEVEL_ERR, &ni, __func__,  "failed to init control1");

More info on logging can be found in the Real-time logging.

Libraries

It starts with some includes (structs that are needed in configuration) and loading of the libraries

#include <ubx/ubx.h>
#include <ubx/trig_utils.h>

#define WEBIF_PORT "8810"

#include "ptrig_period.h"
#include "signal.h"

def_cfg_set_fun(cfg_set_ptrig_period, struct ptrig_period);
def_cfg_set_fun(cfg_set_ubx_triggee, struct ubx_triggee);

static const char* modules[] = {
	"/usr/local/lib/ubx/0.9/stdtypes.so",
	"/usr/local/lib/ubx/0.9/ptrig.so",
	"/usr/local/lib/ubx/0.9/platform_2dof.so",
	"/usr/local/lib/ubx/0.9/platform_2dof_control.so",
	"/usr/local/lib/ubx/0.9/webif.so",
	"/usr/local/lib/ubx/0.9/lfds_cyclic.so",
};

int main()
{
	int ret = EXIT_FAILURE;
	ubx_node_t nd;
	ubx_block_t *plat1, *control1, *ptrig1, *webif, *fifo_vel, *fifo_pos;

	/* initalize the node */
	nd.loglevel = 7;
	ubx_node_init(&nd, "platform_and_control", 0);

	/* load modules */
	for (unsigned int i=0; i<ARRAY_SIZE(modules); i++) {
		if(ubx_module_load(&nd, modules[i]) != 0){
			ubx_log(UBX_LOGLEVEL_ERR, &nd,__func__,  "fail to load  module %s %i",modules[i], i);
			goto out;
		}
	}

Block instantiation

Then we instantiate blocks (code for only one, for sake of brevity):

	if((plat1 = ubx_block_create(&nd, "platform_2dof", "plat1"))==NULL){
		ubx_log(UBX_LOGLEVEL_ERR, &nd,__func__,  "fail to create plat1");
		goto out;
	}

Property configuration

Now we have the more tedious part, that is the configuration. We use the type safe helper functions, for example

int cfg_set_double(const ubx_block_t *b, const char *cfg_name, const double *valptr, const long len);

where

  • b is the block
  • cfg_name the name of the config to set
  • valptr is a pointer to the value to assign to the config
  • len is the array size of valptr

String property

	/* webif port config */
	if (cfg_set_char(webif, "port", WEBIF_PORT, strlen(WEBIF_PORT))) {
		ubx_log(UBX_LOGLEVEL_ERR, &nd,__func__,  "failed to configure port_vel");
		goto out;
	}

The string can be passed as a static const char[] or using a #define.

Double property

	/* gain */
	double gain = 0.12;

	if (cfg_set_double(control1, "gain", &gain, 1)) {
		ubx_log(UBX_LOGLEVEL_ERR, &nd, __func__,  "failed to configure gain");
		goto out;
	}

In this case, memory allocation is done for a scalar (i.e. size 1) . The second line says: consider d->data as a pointer to double, and assign to the pointed memory area the value 0.12.

Fixed size array of double

	/* joint_velocity_limits */
	const double joint_velocity_limits[2] = { 0.5, 0.5 };

	if (cfg_set_double(plat1, "joint_velocity_limits", joint_velocity_limits, 2)) {
		ubx_log(UBX_LOGLEVEL_ERR, &nd,__func__,  "failed to configure joint_velocity_limits");
		goto out;
	}

Almost the same as before, but being an array of two elements, we don’t need to take the reference & here.

Structure property

Same thing for struct types:

	/* ptrig config */
	const struct ptrig_period period = { .sec=1, .usec=14 };

	if (cfg_set_ptrig_period(ptrig1, "period", &period, 1)) {
		ubx_log(UBX_LOGLEVEL_ERR, &nd,__func__,  "failed to configure ptrig_period");
		goto out;
	}

Note that for custom types, it is necessary to define the accessor using a typemacro, e.g:

def_cfg_set_fun(cfg_set_ptrig_period, struct ptrig_period);

Array of structures:

	/* chain0 */
	const struct ubx_triggee chain0[] = {
		{ .b = plat1, .num_steps = 1 },
		{ .b = control1, .num_steps = 1 },
	};

	if (cfg_set_ubx_triggee(ptrig1, "chain0", chain0, ARRAY_SIZE(chain0))) {
		ubx_log(UBX_LOGLEVEL_ERR, &nd,__func__,  "failed to configure chain0");
		goto out;
	}

Port connection

To connect we have first to retrieve the ports, and then connect to an a iblock, the fifos. In the following, we have two inputs and two output ports, that are connected via two fifos:

	/* connections setup - first ports must be retrived and then connected */
	ubx_port_t* plat1_pos = ubx_port_get(plat1,"pos");
	ubx_port_t* control1_measured_pos = ubx_port_get(control1,"measured_pos");
	ubx_port_t* control1_commanded_vel = ubx_port_get(control1,"commanded_vel");
	ubx_port_t* plat1_desired_vel = ubx_port_get(plat1,"desired_vel");

	ubx_ports_connect(plat1_pos, control1_measured_pos, fifo_pos);
	ubx_ports_connect(control1_commanded_vel, plat1_desired_vel, fifo_vel);

Init and Start the blocks

Lastly, we need to init and start all the blocks. For example, for the control1 iblock:

	if(ubx_block_init(control1) != 0) {
		ubx_log(UBX_LOGLEVEL_ERR, &nd,__func__,  "failed to init control1");
		goto out;
	}

	if(ubx_block_start(control1) != 0) {
		ubx_log(UBX_LOGLEVEL_ERR, &nd,__func__,  "failed to start control1");
		goto out;
	}

The same applies to all other blocks.

Once all the blocks are running, the ptrig1 block will step all the blocks in the configured order. To prevent the main process to terminate, we can use ubx_wait_sigint to wait for the user to type ctrl-c:

	ubx_wait_sigint(UINT_MAX);
	printf("shutting down\n");

Note that we have to link against pthread library, so the Makefile.am has to be modified accordingly:

platform_main_LDFLAGS = -module -avoid-version -shared -export-dynamic  @UBX_LIBS@ -ldl -lpthread

Next steps

Some suggestions for next steps:

  • it can be necessary to make the array size of data sent and received via ports configurable. Checkout the saturation block for a simple example.
  • sometimes a block shall support multiple types. This can be done at