Robotics is a very fast growing industry and with the availability of low-cost motors, actuators, computers and open-source software libraries, it's easier than ever for hobbyists to design and build their own robots. By robots, I don't just mean humanoids or robotic arms. The word "robot" stems from the Crezh word "robotnik" and Slavonic word "rabota", both which mean "slave". This is overly generalized, so for the purposes of this series of articles, we're going to stick to any actuated set of joints designed to do specific tasks. This includes robotic manipulators used in assembly lines and the ISS, mobile robots and drones.

Lots of desktop robotic manipulators are also popping up such as the DOBOT Magician and the 7Bot. Many of these come with open-source software development kits (SDKs) and libraries to program the controller for custom applications. Generally if users would like to develop their own controller, they would need to be a robotics engineer or have extensive knowledge of robot forward and inverse kinematics, dynamics, control systems design and parameter identification. What I'm attempting to do is teach some basic robotics math - sufficient to develop your own robots or controller. In order to do either of those, it's critical to understand the dynamics of the robot - that is, how the robot responds to certain inputs that cause it to be in motion. The dynamic equations of motion of a system address this. They are a set of equations that allow you to calculate the next state of the robot in a dynamic setting, given the current state and some input. This article specifically goes over how you can derive the equations of motion of a robot.

__Prerequisites:__ Yes, there is definitely prior knowledge you must have before we dive into this! This article is ideal for those with a Bachelor's degree in an engineering, physics or mathematics field. I've also provided links to good courses or materials on these topics if you're missing something. You need to understand linear algebra, coordinate frames, and basic Calculus. It is also very helpful to understand Kinematics of robotic systems - specifically the forward and inverse kinematics, before you dig into dynamics.

If you're reading this, you probably either want to design a robot or you have a robot and want to develop a simulator or controller for it. Robotics programs at most Universities use MATLAB to derive the equations of motion of a robot. However, MATLAB is not free so we'll use Python 3 - a great, free programming language with a ton of libraries and toolkits. In this article, we'll use Python to derive the closed form dynamic equations of motion of a simple three-link robotic manipulator. If you're interested in modeling a mobile robot or a drone, this is still very important in order to understand the process and the core equations behind the dynamics. I can also write another article in the future that shows an example of modeling another kind of robot. Once we derive the equations of motion, we're going to develop a simple simulator for it which will allow us to develop and test controllers. In this example, we'll consider an Anthropomorphic Arm without a wrist to keep it simple.

You need the sympy library to process symbolic math in Python. You can install it using the terminal command:

`pip3 install sympy`

I describe the important equations in deriving the dynamical model of a robotic system in this paper on designing an omnidirectional controller for a spherical robot. I'll be referencing equations from the paper in this article. More papers can be found on the Research page of our website that describe dynamical modeling for control systems design.

We start with defining the generalized coordinates of the system. In order to do this, we need to place coordinate frames at each joint. Generally, there are conventions such as the Denavit-Hartenberg convention for where to place coordinate frames but we're going to keep it simple to minimize the number of variables. We'll place the origins of the robot's base frame, first frame and the second frame at the same point - at the intersection of the robot's first and second joint axes. This way, the translations between these joints are zero vectors. To get from the robot's base frame (fixed) to the first frame, we'll rotate about the z-axis (blue) by angle *θ₁*. We then rotate about the y-axis (green) by angle *θ₂* to get to the second frame. Finally, we offset the third frame by the length of the second link about the x-axis (red) of the second frame and rotate about the y-axis by angle *θ₃* to get to the third frame. If we have an end-effector, we can add that in as well however for now, this should be sufficient to get a rough dynamical model.

We first define the generalized coordinates of the system, their velocities and accelerations. In this case, the generalized coordinates vector consists of the three joint angles. We need to create them as symbolic variables for math operations.

```
# Generalized coordinates
th1 = np.symbols('th1')
th2 = np.symbols('th2')
th3 = np.symbols('th3')
gamma = np.Matrix([th1, th2, th3])
# Generalized velocities
th1d = np.symbols('th1d')
th2d = np.symbols('th2d')
th3d = np.symbols('th3d')
gammad = np.Matrix([th1d, th2d, th3d])
# Generalized accelerationsprint(G)
th1dd = np.symbols('th1dd')
th2dd = np.symbols('th2dd')
th3dd = np.symbols('th3dd')
gammadd = np.Matrix([th1dd, th2dd, th3dd])
```

We also combine the generalized coordinates with the velocities for the state vector, and then the generalized velocities with the accelerations for the state rate of change vector. We use this later for integration.
```
# State vector and state vector rates
X = np.Matrix([th1, th2, th3, th1d, th2d, th3d])
Xd = np.Matrix([th1d, th2d, th3d, th1dd, th2dd, th3dd])
```

We then set up the coordinate frames and their parameters. We assume each frame is associated with a rigid body. We need to know the body's mass, center of mass with respect to the reference frame, the inertia tensor corresponding to rotations about the reference frame, the translation from the connected frame to the reference frame, and the rotation matrix from the connected frame to the reference frame. Here is the snippet for the first frame:

```
# Frame 1 - rotate about the z-axis
# Mass
frame1_m = np.symbols('frame1_m')
# Center of mass offsets
frame1_rCM = np.zeros(3,1)
# Inertia tensor
frame1_J = np.zeros(3,3)
frame1_Jzz = np.symbols('frame1_Jzz')
frame1_J[2,2] = frame1_Jzz
# Translation from previous frame (base/inertial) to current frame
frame1_r = np.zeros(3,1)
# Rotation matrix from previous frame to current frame
frame1_T = rotz(th1)
```

We set up symbolic variables for all the parameters so we can define them later for numerical integration. To simplify the math, I've set a lot of variables to zero and assume the frame is at the center of mass. This is probably not the case but since the center of mass of the frame is most likely near the rotational axis, we'll keep it that way. You can also use symbolic variables for all of these parameters if you'd like to change them in run-time. We only need to worry about the inertia about the z-axis as this frame is linked to the base and only rotates in that axis. Other rotational inertia parameters are important in forward links, unless the number is small enough to be negligible. Generally, I use symbolic variables for all parameters - I just set most of them to zero in this article for sake of simplicity.
Similarly, we set up the dynamic parameters for the remaining frames. rotx, roty and rotz are three-dimensional rotation matrices in each of the orthogonal axes.

We use what's known as the Euler-Lagrange formulation to derive the equations of motion for the system. The formulation is equation (3) from the paper (referenced above). The equation relates the *Lagrangian* of the system to the generalized coordinates and the forces and torques about these generalized coordinates. The Langragian of the system is the total kinetic energy minus the total potential energy of the system. We're going to compute the kinetic and potential energies for each link and use them to get the equations of motion.

Equation (4) from the paper describes the computation of the kinetic energy of a given body. We can write a python function to compute the kinetic energy given the mass, velocity, rotation rate, center of mass and inertia tensor as follows:

```
# Kinetic Energy function
def KineticEnergy(mB, IIrBdot, ITBdot, BBrCM, BBJ):
# Pseudo Inertia tensor
BBJhat = 0.5 * np.trace(BBJ) * np.eye(3) - BBJ
K = 0
# Translational kinetic energy
K += 0.5 * mB * (np.transpose(IIrBdot) * IIrBdot)[0,0]
# Rotational kinetic energy
K += 0.5 * np.trace(ITBdot * BBJhat * np.transpose(ITBdot))
# Coupling kinetic energy
K += mB * (np.transpose(IIrBdot) * ITBdot * BBrCM)[0,0]
return K
```

Equation (6) from the paper describes the computation of the potential energy of a given body. The python function for that is as follows:

```
# Potential Energy function
def PotentialEnergy(mB, IIrB, ITB, BBrCM, g):
return mB * (np.transpose(g) * (IIrB + ITB * BBrCM))[0,0]
```

We need to compute the kinetic and potential energies for each frame with respect to a universal inertial frame. We use the base frame as an inertial frame for most scenarios to keep things a little easier. The next step is to transform each frame to the inertial (or in this case, base) frame to compute the kinetic and potential energies. Below is a snippet of code for the three frames in this robotic system:

```
## Compute system Kinetic and Potential energies
K = 0 # Start from 0
U = 0
# Frame 1
IIr1 = frame1_r
IIr1d = ChainDiff(frame1_r, X, Xd)
IT1 = frame1_T
IT1d = ChainDiff(frame1_T, X, Xd)
K += KineticEnergy(frame1_m, IIr1d, IT1d, frame1_rCM, frame1_J)
U += PotentialEnergy(frame1_m, IIr1, IT1, frame1_rCM, g)
# Frame 2
IIr2 = IIr1 + IT1 * frame2_r
IIr2d = ChainDiff(frame2_r, X, Xd)
IT2 = IT1 * frame2_T
IT2d = ChainDiff(frame2_T, X, Xd)
K += KineticEnergy(frame2_m, IIr2d, IT2d, frame2_rCM, frame2_J)
U += PotentialEnergy(frame2_m, IIr2, IT2, frame2_rCM, g)
# Frame 3
IIr3 = IIr2 + IT2 * frame3_r
IIr3d = ChainDiff(frame3_r, X, Xd)
IT3 = IT2 * frame3_T
IT3d = ChainDiff(frame3_T, X, Xd)
K += KineticEnergy(frame3_m, IIr3d, IT3d, frame3_rCM, frame3_J)
U += PotentialEnergy(frame3_m, IIr3, IT3, frame3_rCM, g)
```

ChainDiff is a custom function that I wrote. The function performs differentiation on vectors and matrices and applies the chain rule given the function to differentiate, the state vector and the derivative of the state vector.
```
# Differentiate using chain rule
def ChainDiff(y, x, xd):
ydot = np.zeros(y.shape[0], y.shape[1])
for n in range(x.shape[0]):
ydot += np.diff(y, x[n]) * xd[n]
return ydot
```

The Euler-Lagrange formulation can be re-written in the form of equation (7) from the paper. This is done to be able to isolate the generalized accelerations vector. The goal is to solve the system for the generalized accelerations and integrate the accelerations to get the velocities and coordinates for the next time step. In the paper, we have a Pfaffian constraint that is applied which isn't necessary for this simple example. We can therefore ignore the **A** and λ terms. We need to compute the **H** matrix - which represents the system mass matrix, the **d** vector - which represents the Coriolis and centripetal forces on the links and the **g** vector - which represents the Gravitational forces on the links.

From the kinetic and potential energy functions, we can extract **H**, **d** and **g** using the following equations:

```
## Compute H, D and G
# H: System mass matrix
H = np.transpose(np.Matrix([[K]]).jacobian(gammad)).jacobian(gammad)
# D: Vector of Coriolis and centripetal forces
D = np.transpose(np.Matrix([[K]]).jacobian(gammad)).jacobian(gamma) * gammad - np.transpose(np.Matrix([[K]]).jacobian(gamma))
# G: Vector of Gravitational forces
G = np.transpose(np.Matrix([[U]]).jacobian(gamma))
```

And there it is!! We now have the equations of motion of a robotic system! The next step is to prepare it in the correct format for integration. We'll use a program called **Dynamix** to integrate and simulate the system. Dynamix is a program I developed for my Master's Thesis (in Space Robotics! Check out my work on our research page). We're going to be providing a free license of Dynamix for research and non-commercial use very shortly. Dynamix uses either the Newton-Euler recursive algorithm for numerical integration or allows users to provide custom equations of motion in a C++ file. So, in order to use the equations of motion, we need to export it to C/C++ code. We're going to use Sympy's codegen utilities for that. The following snippet of code achieves that:

```
## Export hard-coded dynamics code
[(_, H_code), (_, _)] = codegen([("H", H)], "C99", header=False, empty=True)
print(H_code)
[(_, D_code), (_, _)] = codegen([("D", D)], "C99", header=False, empty=True)
print(D_code)
[(_, G_code), (_, _)] = codegen([("G", G)], "C99", header=False, empty=True)
print(G_code)
```

In the next article of this series, we'll discuss how to use these with Dynamix to setup and simulate the robot.

These equations of motion allow you to control the inputs to the joints as torques at each joint. However, you probably care more about voltages applied to motors at these joints. In order to do this, we need to develop electrical dynamic equations of the actuators. We can combine those equations with **H**, **d** and **g** derived in this article to develop the combined electro-mechanical equations for the system and use voltages to drive the actuators. We will be going over that in this series as well!

You can download the full code for what we went over using the link below.

I've also attached the paper that was referenced for the equations. This is publication by myself, Daniel Cohen and Dr. Douglas Isenberg presented at the 2016 IEEE Southeast Conference in Norfolk, VA.