# NMPC with CasADi and Python – Part 3: State-space equations of a 2-DOF robot with SymPy or MATLAB

CasADi is a powerful open-source tool for nonlinear optimization. It can be used with MATLAB/Octave, Python, or C++, with the bulk of the available resources referencing the former two options. This post series is intended to show a possible method of developing a simulation for an example system controlled by Nonlinear Model Predictive Control (NMPC) using CasADi and Python.

In this post, we will look at determining the system equations of a robot with two degrees of freedom in stace-space form. These state-space equations can be used for simulation and for developing a nonlinear model predictive controller. For some transformation tasks, Python with SymPy will be utilized and an alternative using the MATLAB Symbolic Math Toolbox will be shown.

During the following, we are considering a robot with two links, two masses at the end of the links, and motors applying torque as control input at the joints. A diagram of the robot is shown below.

The set of nonlinear differential equations describing the dynamic behavior of a robot with joint angle vector is often given in following form (or similarly; see e.g. *Robot Modeling and Control* by Spong et al.):

In this equation, is the mass or inertia matrix, is the coriolis matrix, and is the gravity vector. is the vector of control input torques applied at the joints.

Determining the relevant matrices of the equation above for the considered robot is possible through multiple methods. A commonly used one is applying the Lagrangian method. For this, the kinetic energy and the potential energy of the robot system must first be calculated using the generalized coordinates . The Lagrangian is then given by . The control input torque at each joint can be seen as a generalized force . Now, one differential equation for each generalized coordinate can be found by using Lagrange’s equation of motion of the second kind:

For an equivalent robot system, the calculation steps are shown in *Modeling of 2-DOF Robot Arm and Control* by Okubanjo et al. Note, however, that there are some inaccuracies in this paper: Equation 13 is incorrect due to two missing squared operators. This is corrected from equation 15 onwards. Furthermore, the matrix form of the differential equation shown in equation 38 contains an additional after the coriolis matrix. While this is generally not incorrect and an alternative form of the convention, the results in equation 34 and 35 do not fit to this form. Due to these problems, using the results from this paper cannot be recommended without double-checking. Bingül et al. (*A Fuzzy Logic Controller tuned with PSO for 2 DOF robot trajectory control*) considered the same robot configuration though, and with their results the calculations should lead to the following matrices and vectors (for the aforementioned matrix formulation of the robot system equation):

These equations cannot be fed directly into CasADi though – we need a state-space representation. An obvious choice of a state vector for this system would be the two joint angles and angular velocities (). Due to the relation of a joint angle and its angular velocity, two rows of the system equation will simply be

The other two rows for and can be calculated from the set of nonlinear differential equations above. This transformation can either be done by hand, or by using a tool like SymPy or the MATLAB Symbolic Math Toolbox. For this, all symbolic variables must first be defined, then the equations need to be defined, and finally it is possible to tell the tool to solve for the variables of interest.

Code for Python / SymPy:

#!/usr/bin/env python3 from sympy import * # masses of robot arms m1, m2 = symbols("m1 m2") # lengths of robot arms l1, l2 = symbols("l1 l2") # acceleration due to gravity g = symbols("g") # angles at robot joints q1, q2 = symbols("q1 q2") # 1st derivatives of angles dq1, dq2 = symbols("dq1 dq2") # 2nd derivatives of angles ddq1, ddq2 = symbols("ddq1 ddq2") ddq = Matrix([ddq1, ddq2]) # control input torque u1, u2 = symbols("u1 u2") U = Matrix([u1, u2]) # matrix coefficients for intertia matrix M(q) M11 = (m1+m2)*l1**2 + m2*l2**2 + 2*m2*l1*l2*cos(q2) M12 = m2*l2**2 + m2*l1*l2*cos(q2) M21 = m2*l2**2 + m2*l1*l2*cos(q2) M22 = m2*l2**2 M = Matrix([[M11, M12], [M21, M22]]) # matrix coefficients for coriolis matrix C(q, dq) C11 = -m2*l1*l2*(2*dq1*dq2 + dq2**2)*sin(q2) C21 = m2*l1*l2*dq1**2*sin(q2) C = Matrix([C11, C21]) # matrix coefficients for gravity vector G(q) G11 = (m1 + m2)*g*l1*cos(q1) + m2*g*l2*cos(q1+q2) G21 = m2*g*l2*cos(q1+q2) G = Matrix([G11, G21]) # dynamic equations of robot eq = Eq(U, M*ddq + C + G) sol = solve(eq, (ddq1, ddq2)) print("ddq1 = {} \n\nddq2 = {}".format(sol[ddq1], sol[ddq2]))

This yields the missing two lines of the state-space system equation:

> python3 transform_equations_2dof.py ddq1 = (l2*(2*dq1*dq2*l1*l2*m2*sin(q2) + dq2**2*l1*l2*m2*sin(q2) - g*l1*m1*cos(q1) - g*l1*m2*cos(q1) - g*l2*m2*cos(q1 + q2) + u1) + (l1*cos(q2) + l2)*(dq1**2*l1*l2*m2*sin(q2) + g*l2*m2*cos(q1 + q2) - u2))/(l1**2*l2*(m1 + m2*sin(q2)**2)) ddq2 = -(l2*m2*(l1*cos(q2) + l2)*(2*dq1*dq2*l1*l2*m2*sin(q2) + dq2**2*l1*l2*m2*sin(q2) - g*l1*m1*cos(q1) - g*l1*m2*cos(q1) - g*l2*m2*cos(q1 + q2) + u1) + (dq1**2*l1*l2*m2*sin(q2) + g*l2*m2*cos(q1 + q2) - u2)*(l1**2*m1 + l1**2*m2 + 2*l1*l2*m2*cos(q2) + l2**2*m2))/(l1**2*l2**2*m2*(m1 + m2*sin(q2)**2))

Code for MATLAB / Symbolic Math Toolbox (this script can also be run with Octave, which then uses SymPy in the background):

% masses of robot arms syms m1 m2; % lengths of robot arms syms l1 l2; % acceleration due to gravity syms g; % angles at robot joints syms q1 q2; % 1st derivatives of angles syms dq1 dq2; % 2nd derivatives of angles syms ddq1 ddq2; ddq = [ddq1; ddq2]; % control input torque syms u1 u2; U = [u1; u2]; % matrix coefficients for inertia matrix M(q) M11 = (m1+m2)*l1^2 + m2*l2^2 + 2*m2*l1*l2*cos(q2); M12 = m2*l2^2 + m2*l1*l2*cos(q2); M21 = m2*l2^2 + m2*l1*l2*cos(q2); M22 = m2*l2^2; M = [M11, M12; M21, M22]; % matrix coefficients for coriolis matrix C(q, dq) C11 = -m2*l1*l2*(2*dq1*dq2 + dq2^2)*sin(q2); C21 = m2*l1*l2*dq1^2*sin(q2); C = [C11; C21]; % matrix coefficients for gravity vector G(q) G11 = (m1 + m2)*g*l1*cos(q1) + m2*g*l2*cos(q1+q2); G21 = m2*g*l2*cos(q1+q2); G = [G11; G21]; % dynamic equations of robot eq = M*ddq + C + G == U; sol = solve(eq, [ddq1, ddq2]); disp(ddq1 == sol.ddq1); disp(''); disp(ddq2 == sol.ddq2);

#### Recent Posts

#### Recent Comments

- Arjun on Fix for Latex4CorelDraw with CorelDraw 2017
- Nigel De Gillern on No wired (LAN) connection in Linux, although everything looks right
- Harald H on Automatically reboot TP-Link router WDR4300 / N750 with bash script
- Gene on Running multiple Django projects on one Apache instance with mod_wsgi
- nspo on NMPC with CasADi and Python – Part 1: ODE and steady state

## Leave a Reply