Dynamic Modeling of Robotic Systems: Part 2 — Setting up a Dynamix ModelNarendran Muraleedharan
Narendran Muraleedharan
narendran.m@aptusai.com
Published on Thu May 21 2020

In the first article of the series, we went through the derivation of the equations of motion of a three-link robotic manipulator in Python. In this article, we’ll set up the model in Dynamix for simulation and controller development. This is a very simple example and a lot more work is needed to get the robot to a state for realistic controller deveolopment — including parameter identification, electro-mechanical modeling and setting up for Hardware-In-The-Loop simulation. I’ll be writing more ariticles which will go through them as well.

If you don’t already know, Dynamix is a tool I originally started developing to be able to efficiently simulate a high dimensionality robotic system with complex dynamix coupling. I used it to for a lot of research in the past and ended up using it for some of our clients including Cleo Robotics through Aptus. We’ve decided to keep developing it and make it available to the other users!

You can get a free non-commercial (for research purposes only) license for Dynamix from pinetree.ai — our commercial software division. When you download the program and obtain the license key, place the license file in the root directory of Dynamix. Follow the instructions on the readme file to install all the dependencies.

We are going to be creating a Dynamix model for the anthropomorphic arm with no wrist — the three-link robot we derived the equations of motion for in the previous article. Dynamix models are in the Dynamix/model directory The various files used to define a robot model in Dynamix are:

  • Model Definition File
  • Model-specific Makefile Arguments
  • Hard-Coded Dynamic Model
  • Robot Controller
  • 3D Model and Animations
  • Dynamic Parameters
  • Run-time Scripting

We start by creating a folder for the model. The folder be named after the model — in this case, we have anthropomorphic_arm_no_wrist. Also by default, Dynamix includes this model! So, while I take you through how I created this model, you can work on your own model.

Model Definition File

Dynamix looks for a *.model file in the model directory to read the configuration and paths to the other files specific to the model. The filename must be the same as that of the model directory. In this case, the model definition file must be named anthropomorphic_arm_no_wrist.model.

The syntax for the model definition file is very straight forward. The configuration name is followed by a color and the value. Lines that are comments and to be ignored must start with “#”. And that’s it! Below are the configuration properties we can set in the model definition file.

We start by defining the names and initial conditions for the generalized coordinates, the generalized velocities, and the inputs. We use these names to address the states and inputs in the controller.

# Define state variables - initial conditions
GAMMA:			th1=0,  th2=0,  th3=0

# Define state velocities - initial conditions
GAMMA_DOT:		th1d=0,  th2d=0,  th3d=0

# Define input variables - initial conditions
INPUTS:			F1=0,	F2=0,	F3=0

We then need to define the paths to the other model-related files.

The Dynamic model can be defined either with a C++ file or a *.dmx file. Dynamix uses one of two methods to compute the dynamic responses — either the Newton-Euler recursive algorithm for numerical integration or for users to provide a custom hard-coded function. The Newton-Euler method allows high performance dynamical computations for large systems. The custom hard-coded function gives full flexibility to the user to define the dynamics. The ability to have the dynamic model hard-coded allows it to be run with the compiled program and hence very fast, with minimal latency.

In part 1 of this article series, we used the Euler-Lagrange formulation to derive the closed-form dynamics of the manipulator. We’ll use those equations and create a custom hard-coded dynamic function. Define the path to the dynamics function as follows:

# Set a *.cxx filepath to use custom hard-coded dynamics - write
# the model dynamics as dynamics.cxx in the model root directory.
# Set a *.dmx filepath to use the dynamix-defined model
DYN_PATH:		dynamics.cxx

The Controller can either be defined in Lua or hard-coded. For this example, we’ll use a Lua controller due to its simplicity and ability to modify it without having to re-compile the model. We define the controller path as follows:

# Set a *.cxx filepath to use a customer hard-coded controller - write
#  the controller as controller.cxx in the model root directory.
# Set a *.lua or *.py file to use a script as the primary controller.
# NOTE: To use a combination of scripting and a hard-coded controller,
#  define the script below in the SCRIPTS path variable and set
#  properties in the tree which can be accessed by controller.cxx. Then,
#  define the controller path as the *.cxx file.
CTL_PATH:	    controller.lua

The path to the 3D model is defined using the following snippet:

MODEL_PATH:	    3d/model.ac

The animations are defined in a separate file and the path is set with:

ANIM_PATH:	    animations.dmx

The dynamic parameters for variables in the dynamical model are defined in an external file as well. This is to give users the ability to change these parameters without having to re-compile the model. Dynamix is designed to bring a good balance and compromise between fast simulation speeds and ease of development. The parameters are defined uwith the line:

PARAMS_PATH:	    parameters.dmx

In the spirit of ease of development, Dynamix allows users to write Lua scripts other than for just the controller. Using the built-in Lua interface, users have much more functionality and flexibility that can be tested and modified without having the re-compile the model. Paths to Lua scripts can be set in the model definition file as follows, separated by commas:

# Run-time script paths
SCRIPTS:		scripts/ground.lua

Dynamix supports three different integrators — the Euler method, the second order Runga-Kutta integrator and the fourth order Runga-Kutta integrator. Each have their own benefits. You can set the integrator using the following line:

# If the desired integrator is not explicitely specified, the Euler
# method (fastest, but lease accurate) is used
# The 4th order Runga-Kutta method (RK4) is the slowest but most 
# accurate.
INTEGRATOR:		RK4

There are a few other optional configurations such as file paths for hard-coded sensors, camera etc. We’re not going to use these for the anthropomorphic arm example, but you can define them if you’d like.

Makefile Arguments

A separate model.mk has to be present in the root directory of the model if either a hard-coded dynamic model definition or controller file is used. In this example, we need to define the argument for the dynamic model.

# If using a C++ controller, make sure to have that set below!
# CONTROLLER = -DCONTROLLER_CXX
# If using a customer C++ dynamics definition, make sure to have that set below!
DYNAMICS = -DDYNAMICS_CXX

Hard-Coded Dynamic Model

In the last article, we went over how to derive the equations of motion of the simple three-link manipulator. We got some equations that define the system mass matrix H, the vector of Coriolis and centripetal forces d, and the vector of gravitational forces g. We’ll now prepare the dynamic model function in C++. Dynamix uses the Armadillo Linear Algebra library for matrix operations. We need to include the header Dynamix.hxx to have access to the appropriate function definitions and available libraries. Before we define the actual dynamic model function, we set up some variables. The below code must be placed in a new file dynamics.cxx (or with the name specified in the model definition file) in the root directory of the model.

#include "Dynamix.hxx"

#define PI 3.14159265358979323846
#define GAMMA_LEN 3

// NOTE: The armadillo linear algebra library is available. Use it!!! :)

// Output: Xdot [gammad, gammadd]
// Inputs: X [gamma, gammad]
//		   U (control inputs)
//		   params (loaded parameters) - access with params[""]

double gz = 9.81;

mat H (GAMMA_LEN, GAMMA_LEN);
vec D (GAMMA_LEN);
vec G (GAMMA_LEN);
vec u (3);

vec gammad(GAMMA_LEN);
vec gammadd(GAMMA_LEN);

vec Xdot(GAMMA_LEN * 2);

We initialize H as a 3x3 matrix, and D, G, and u (the vector of inputs) as 3x1 vectors. We also initialize the generalized velocities, generalized accelerations and state rate of change vectors to avoid re-initializing and re-allocating memory every iteration of the function call.

The dynamic model function must be based on the prototype:

vec dynamics_cxx(vec X, map U, map params, map tables)
{
    ...

    return Xdot;
}

In the function, we receive the state vector, a list of inputs, the dynamic parameters and any tables. Tables are used for interpolation — this is generally common for aerodynamic models. We are required to compute the state rate of change vector and return it out of the function.

We defined the state vector as the generalized coordinates augmented with the generalized velocities. We have three generalized coordinates. This means that the first three elements of the state rate of change vector is comprised of the generalized velocities. We can write this in code using the following snippet:

for (int i=0; i<GAMMA_LEN; i++) {
    Xdot(i) = X(GAMMA_LEN+i);
}

The remaining three elements are the generalized accelerations which we will be solving for.

We then need to set up variables for the functions. When we derived the equations of motion, the program spit out H, d and g. The output for H looks like the following for this example:

#include "H.h"
#include <math.h>

void H(double frame1_Jzz, double frame2_Jyy, double frame3_Jyy, double th1double th2, double th3, double *out_6675323565019646154) {


   out_6675323565019646154[0] = 1.0*frame1_Jzz*pow(sin(th1), 2) + 1.   0*frame1_Jzz*pow(cos(th1), 2);
   out_6675323565019646154[1] = 0;
   out_6675323565019646154[2] = 0;
   out_6675323565019646154[3] = 0;
   out_6675323565019646154[4] = 1.0*frame2_Jyy*pow(sin(th2), 2) + 1.   0*frame2_Jyy*pow(cos(th2), 2);
   out_6675323565019646154[5] = 0;
   out_6675323565019646154[6] = 0;
   out_6675323565019646154[7] = 0;
   out_6675323565019646154[8] = 1.0*frame3_Jyy*pow(sin(th3), 2) + 1.   0*frame3_Jyy*pow(cos(th3), 2);
}

We’re going to take that and clean it up a little. We’ll leave the equations as they are but just re-name and re-arrange them a little.

// DEFINE System Mass Matrix (H)
H(0,0) = 1.0*frame1_Jzz*pow(sin(th1), 2) + 1.0*frame1_Jzz*pow(cos(th1), 2);
H(0,1) = 0;
H(0,2) = 0;
H(1,0) = 0;
H(1,1) = 1.0*frame2_Jyy*pow(sin(th2), 2) + 1.0*frame2_Jyy*pow(cos(th2), 2);
H(1,2) = 0;
H(2,0) = 0;
H(2,1) = 0;
H(2,2) = 1.0*frame3_Jyy*pow(sin(th3), 2) + 1.0*frame3_Jyy*pow(cos(th3), 2);

We’ll do the same for D and G.

// DEFINE Vector of Coriolis and centripetal forces
// D is a vector of zeros
D.zeros();

// DEFINE Vector of Gravitational forces
G(0) = 0;
G(1) = frame2_m*frame2_rCMx*gz*cos(th2) - frame3_m*gz*(frame3_rCMx*(si(th2)    *sin(th3) - cos(th2)*cos(th3)) - frame3_rx*cos(th2));
G(2) = -frame3_m*frame3_rCMx*gz*(sin(th2)*sin(th3) - cos(th2)*cos(th3));

We want to simulate some sort of friction as well — this wasn’t modeled using the Euler-Lagrange formulation but we can just add it to d as follows:

// Add Actuator friction parameters to D
D(0) += frame1_b1*th1d + frame1_b2*sign(th1d);
D(1) += frame2_b1*th2d + frame2_b2*sign(th2d);
D(2) += frame3_b1*th3d + frame3_b2*sign(th3d);

We have these functions, but they use variables like th1, frame_1_Jzz and so on which are not defined in this function. So before you actually get to these lines, you’d need to define them. You can copy the generalized coordinates and their velocities from the state vector as follows:

// Copy variables: generalized co-ordinates and derivatives
double th1 = X(0);
double th2 = X(1);
double th3 = X(2);
double th1d = X(3);
double th2d = X(4);
double th3d = X(5);

You also need the dynamic parameters. For sake of simplicity, we’ll name the dynamic parameters the same as the symbols when we derived the equations of motion. You just need to copy them from the params map to their own variables like:

// Copy variables: loaded parameters
double frame1_m = params["frame1_m"];

This is just one variable — you need to copy all of them.

The inputs are passed in as a map so you can access the input names in the controller script. To use them to solve for the generalized accelerations, we need to get them into an Armadillo vector.

// DEFINE Vector of Inputs (u)
u(0) = U["F1"];
u(1) = U["F2"];
u(2) = U["F3"];

We then solve for the generalized accelerations using Armadillo’s built-in linear algebra solver and copy them into the state rate of change vetor as follows:

gammadd = solve(H, u - D - G);

for (int i=0; i<GAMMA_LEN; i++) {
    Xdot(GAMMA_LEN+i) = gammadd(i);
}

And that’s it! We now have a function that computes the dynamics of the robot and returns the state rates of change.

Robot Controller

In the model definition file, we setup a scripted Lua controller. You have full freedom to use Lua’s available functions and modules to develop your own controller. If you setup a C++ controller, you have even more flexibility. Our team is also working on integrating Python into Dynamix so you can write your controller in Python.

For this example, we’ll setup some simple PID controllers to track a desired trajectory. I would not recommend using a setup like this for your actual robot — this example is only for the purpose of understanding how Dynamix works. I will write another article about using pre-compiled C++ controllers.

When a Lua controller in is use, the init variable is set to True for the first instance and then turns to False during run-time. We can use this to initialize any variables or controllers. In this example, we’ll initialize a variable to hold time t as 0 and then increment it with dt. The variable dt is set to the actual controller script interval.

if init then
    t = 0
    
    ...
end

t = t + dt

You can also get time from the property tree. The property tree allows for variables to be stores and managed easily between multiple Lua scripts and pre-compiled C++ code. You can find out more about the property tree in the documentation for Dynamix.

Dynamix has built-in PID functions that you can use. If you’d like to write your own PID controllers (or other controllers), go ahead! In this example, we’ll initialize a PID controller for each joint and then run them.

To initialize a PID controller, you use the code:

--[[ Controller_instance = pid_init(Kp, Ki, Kd) ]]
C1 = pid_init(80,20,20)

The arguments for pid_init are the proportional, integral and derivative gains respectively.

We can set constants of functions for the desired joint angles. I’m just going to play around with some waves for this example:

th1_des = math.sin(t)
th2_des = math.pi/2 + math.cos(t)
th3_des = -math.cos(t)

Finally, we run the PID controllers using the built-in pid_run function. I also use the built-in saturate function to limit the outputs to reasonable torque limits. The first argument for pid_run is the controller instance. The second argument is the interval — you can use dt here. The third and last argument is the error you are driving to zero. Here is what the line of code would look like for one of the joints:

U_F1 = saturate(pid_run(C1, dt, th1_des - X_th1),-10,10)

Again, all of this is available as an example model in the free release of Dynamix for you to follow along.

3D Model and Animations

In order to visualize our model, we need to provide a 3D model and animations. Dynamix uses OpenSceneGraph to display and animate the 3D model. OpenSceneGraph (and hence Dynamix) supports a wide variety of 3D model formats listed here. I use the *.ac format by Ac3D because I like modeling in it and have a license. There are many formats supported including *.obj, *.3ds and *.stl which can be used with free 3D modeling programs like Blender.

Here at Aptus, we use Autodesk Fusion 360 for 3D CAD designs. I used Fusion 360 to draft up a 3D model for the anthropomorphic arm, exported it to the *.stl format and then used Ac3D to color it, set material properties etc. and finally saved as a *.ac file for Dynamix. Note that you can use the file in the *.stl format as well but you need to make sure the objects are named for animation.

You can use the 3D Print Utility in the Make Toolbar group in Fusion 360 to export *.stl files. In order to keep the bodies separate for animation, I export each of the bodies as separate files. In this case, we have the base, frame1, frame2 and frame3 (the end-effector). You can import all four files into Ac3D and they will keep their default positions.

You then need to name each of the objects using the Object Property Editor (F9). I also set different materials for easier visualization. Please excuse my choice of colors for this example. Also, make sure the default units are consistent in Fusion 360 and in Ac3D.

Also, record the axes for animation. These should be consistent with the coordinate frame positions set for the dynamic model.

Dynamix uses a separate file to define animations for the model objects. Dynamix supports a few different transformations — static, rotate, translate and scale. At least one animation needs to be set for the object to be displayed. If you need the object to just be displayed, use the static animation type. Since the animation of the last frame depends on the rotations of all frames, the object needs to be animated three times. Below are the formats for animations:

Static transformation

animate: [objects]
	type	=	static
end

Replace [objects] with either the single object name or a list of object names separated by commas.

Rotate transformation

animate: [objects]
	type	=	rotate
	center	=	[x-center], [y-center], [z-center]
	axis	=	[x-axis], [y-axis], [z-axis]
	factor	=	[factor]
	prop	=	[property]
end

You can rotate objects about a line defined by an axis through a point. The axis is a vector about which the object rotates and center defines the point in the model coordinate frame.

The rotation angle is defined by the [property] multiplied by the [factor]. The property can either by a generalized coordinate (defined as GAMMA in the model definition file) or a property if the first character of the property is a forward slash (/).

Translation

animate: [objects]
    type	=	translate
    axis	=	[x-axis], [y-axis], [z-axis]
    prop	=	[property]
end

Translation occurs along a vector defined by the [axis] variable and the distance of translation is defined by the [property] multipled by the magnitude of the [axis] vector. For instance, if you need one unit of the [property] to result in a translation of 0.1 units about the x-axis and 0.2 units about the y-axis, you would set the axis to 0.1,0.2,0.

Scaling transformation

animate: [objects]
    type	=	scale
    axis	=	[x-axis], [y-axis], [z-axis]
    prop	=	[property]
end

Scaling occurs by different proportions about each axis, and is also dependent on the [property].

For this example, the animation file looks like:

# Animations need to be declared in reverse kinematic order

animate: frame3
    type	=	rotate
    center	=	0.3, 0, 0
    axis	=	0, -1, 0
    factor	=	1
    prop	=	th3
end

animate: frame2, frame3
    type	=	rotate
    center	=	0, 0, 0
    axis	=	0, -1, 0
    factor	=	1
    prop	=	th2
end

animate: frame3, frame2, frame1
    type	=	rotate
    center	=	0, 0, 0
    axis	=	0, 0, 1
    factor	=	1
    prop	=	th1
end

animate: base
    type	=	static
end

We keep the base static. We rotate all three frames by the z-axis dependent on th1. We rotate frames 2 and 3 about the negative y-axis by angle th2 and finally, the last frame about the negative y-axis by angle th3. And of course, this is declared in reverse order.

Dynamic Parameters Definition

At this point, most of the work is done in setting up the model. We have the equations of motion defined with a lot of variables. In order to numerically integrate for simulation, these variables need values. Dynamix allows the definition of these parameters in a separate file so that values need not be hard-coded into the equations of motion. This gives flexibility to the user and ability to change dynamix parameters without re-compiling.

In the model definition file, we set the parameters file path to parameters.dmx. When we derived the equations of motion, we used prefixes for each frame (i.e. frame1_, frame2_, and frame3_). The format for definition of dynamic parameters is very simple. If you set a prefix, you can use the useprefix: keyword. If not, start blocks with the noprefix keyword. For instance, if you need the ability to change the gravitational acceleration g, you can use the noprefix keyword for that. In this case, we definied gz in the C++ file so it’s not a parameter that can be set here.

Following are the contents of the parameters file for this example. I came up with reasonable numbers for the parameters. If you have a real robot you’re modeling, I would suggest you run some system identification on the joints of the robot and use those numbers. You can also get estimates for the inertia and center of mass parameters from the CAD files. I will write another article shortly that explains how you can get the dynamix parameters from CAD.

# Dynamic parameters of each frame of the Anthropomorphic arm with no wrist
# Frame 1
useprefix: frame1_
    m	=	1.5
    Jzz	=	0.1
    b1	=	0.8
    b2	=	0.3
end

# Frame 2
useprefix: frame2_
    m	=	1.5
    Jyy	=	0.5
    rCMx=	0.12
    b1	=	0.8
    b2	=	0.3
end

# Frame 3
useprefix: frame3_
    m	=	0.5
    Jyy	=	0.3
    rCMx=	0.05
    rx	=	0.3
    b1	=	0.8
    b2	=	0.3
end

Run-time Scripting

As I mentioned earlier, Dynamix allows users to write custom scripts in Lua. These scripts need to be in dedicated *.lua files whose relative paths are set by the SCRIPTS: parameter in the model definition file. In this example, we only defined one script: scripts/ground.lua which we’ll use to display a grid on the ground. Dynamix comes with a few included 3D modeles including a ground grid, a center of mass marker, and a target marker.

Dynamix also has a powerful interface for Lua which gives the user the ability to access a lot of functions and utilities. We’re preparing a function reference which will be available with the documentation for Dynamix. To display a model of the ground, we need need to create a variable for the model and set its translation.

--[[ For all lua scripts defined in the SCRIPTS path variable of the model file, the global variable 'init' is set to true on the first call. Then, the variable value is false. Use this to control loops instead of an infinite while ]]

if init then
    gnd_model = gfx_load3d('data/3d/ground8.ac')
    gfx_setTrans(gnd_model,0, 0, -0.16)
end

We also only need to do this once on initialization. The path for gfx_load3d is relative from the Dynamix root directory. In this case, we place the ground 0.16m below the z-axis. This is how we prepared the 3D model of the manipulator.


And that’s it! We can build the model from the Dynamix root directory with the command line call:

./build.sh anthropomorphic_arm_no_wrist

And run Dynamix with the call:

./dynamix

If you need to add scripts, change scripts, change parameters or animations, you can make the changes and run Dynamix without rebuilding the model. You only need to rebuild the model if you change any of the C++ files — such as the equations of motion.

Have fun modeling your own robots! Feel free to contact us if you need any help setting up your model or using Dynamix!

In our data-driven world, businesses have more potential to be successful than ever before. By implementing the three types of analytics into a business setting, you can find your optimal target audience, see why your sales were down last month, and predict how a new product will be received. The role of Data Analytics can help a business see things in a new way and help business leaders make smarter decisions for the future.