Nonlinear Control of 2-link Manipulator with Scilab and RTSX (Part I)

Part I : PD Control with Gravity Compensation

In a common robotics textbook such as [1], robot control is classified to two types: 1. independent joint control, and 2. multivariable control. For the first category, a controller, either a PID or custom-designed one, is connected with each joint of the robot to form an SISO feedback loop. The interaction between adjacent links is modelled as a disturbance signal entering at some point, normally at the plant input, which can be attenuated using feedback. We discussed this approach in our H-inf synthesis articles. The second category, on the other hand, takes into account the robot dynamics and uses that information to craft a multivariable controller that tackles all robot joints as a whole. Since the robot dynamics is time-varying and configuration-dependent, the resulting controller is governed by a system of nonlinear differential equations.

In this article, we discuss some basics of robot nonlinear control and show how to compute and simulate with Scilab and Xcos. To focus only on the essence, a simple 2-link planar manipulator in Figure 1.1 is used as our robot model. We will see that even for this 2D robot consisting of 2 revolute joints, the control laws can be quite complicated.

Figure 1.1 a 2-link robot manipulator

To narrow down the scope, here we want to formulate the problem and perform simulation for 3 control schemes:

  1. PD with gravity compensation
  2. inverse dynamics
  3. adaptive control

These 3 schemes will be discussed separately in a trilogy.

To get started, the equation of motion for the 2-link robot can be described by [1]

(1)   \begin{eqnarray*} d_{11}\ddot{q}_1 +d_{12}\ddot{q}_2 +c_{121}\dot{q}_1\dot{q}_2+c_{211}\dot{q}_2\dot{q}_1+c_{221}\dot{q}_2^2+g_1 = \tau_1 \\ d_{21}\ddot{q}_1+d_{22}\ddot{q}_2+c_{112}\dot{q}_1^2+g_2 = \tau_2 \end{eqnarray*}

with the corresponding terms

(2)   \begin{eqnarray*} d_{11}= m_1l_{c1}^2+m_2\left(l_1^2+l_{c2}^2+2l_1l_{c2}\cos q_2\right)+I_1+I_2\\ d_{12}= d_{21}=m_2\left(l_{c2}^2+l_1l_{c2}\cos q_2\right) +I_2\\ d_{22}=m_2l_{c2}^2+I_2\\ c_{121}=c_{211}=c_{221}=\frac{1}{2}\frac{\partial d_{11}}{\partial q_2}=-m_2l_1l_{c2}\sin q_2=\zeta\\ c_{112}=-\zeta\\ g_2=\frac{\partial P}{\partial q_2}=m_2l_{c2}g\cos \left(q_1+q_2\right) \\ g_1=\frac{\partial P}{\partial q_1}=\left(m_1l_{c1}+m_2l_1\right)g\cos q_1+m_2l_{c2}g\cos \left(q_1+q_2\right) \end{eqnarray*}

This is just an example of a general robot equation of motion in matrix form

(3)   \begin{equation*}  M(q)\ddot{q}+C(q,\dot{q})\dot{q}+B\dot{q}+g(q)=u \end{equation*}

with in this case M(q)=D(q)(neglecting actuator inertia), and no friction term B=0 . So for compactness, we can write (1) in matrix form as

(4)   \begin{equation*}  D(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)=u \end{equation*}

PD Control Scheme with Gravity Compensation

A PD control is a subset of PID (Proportional Integral Derivative), which is a linear, general purpose controller used in various industrial applications. A PD control law results when the integral term of PID is turned off. When a gravity compensation term is added, the control law becomes nonlinear. A block diagram for PD control with gravity compensation is shown in Figure 1.2, with the control law described as

(5)   \begin{equation*}  u=g(q)+K_p\tilde{q}-K_d\dot{q} \end{equation*}

Figure 1.2 block diagram of PD control with gravity compensation

where \tilde{q}=q_d-q is the error between the command and joint variable. It can be shown by the use of Lyapunov and Lasalle theorems that the equilibrium point for this system is golbally asymptotically stable. The detail can be studied from [1]. Below we demonstrate how to build an Xcos diagram and simulate this control scheme.

An Xcos model can be constructed directly from the block diagram in Figure 1.2 by using standard palettes provided by the software. If we connect all blocks in one level, the resulting model becomes too messy. So better wrap the robot dynamics described by (1) into a superblock, with internal structure as shown in Figure 1.3.

Figure 1.3 internal structure of 2-link manipulator superblock

To get a better view and start experimenting, download the Xcos file mdl_2link.zcos and run it in Scilab. You’d see the diagram in Figure 1.4. Doubleclick on the 2-link planar robot for the live preview of the diagram in Figure 1.3. The main elements in this superblock are 5 nonlinear expression blocks from terms in (1). These terms and gains are multiplied and added according to (1) by the multiplication, gain, and summation blocks. The joint position and velocity are derived from acceleration using integrator blocks. Also notice the 1/z blocks that represent unit delays. These blocks are not part of the continuous-time dynamical equations (1), but are necessary for the simulation to prevent algrebraic loops.

Figure 1.4 open-loop simulation of the 2-link manipulator

Note that there are certain variables in the expression blocks that need to be initialized. Here we assume these parameters for our 2-link robot

 m1 = 5; m2 = 5; l1 = 1; l2 = 1; lc1 = l1/2; lc2 = l2/2;
I1 = 2; I2 = 2; g = 9.81;

Enter these variables to Scilab workspace. Now we can run mdl_2link.zcos to see an open-loop responses to sinusoidal commands 0.01\sin (2\pi t) at the two joints. The position, velocity, and acceleration for each of the two joints are plotted. Try changing the magnitude and frequency of the commands, and turn on/off either of them to see the effect on the responses. You can also vary the robot parameters and evaluate whether the responses change as expected.

After getting a bit comfortable with the 2-link manipulator plant, we move on to add the feedback controller to the model, as depicted in Figure 1.5, which conforms to the block diagram in Figure 1.2. The step blocks are used as command inputs, with switches to turn them on/off. The PD feedback part is trivial. The gravity compensation terms need some more clarification.

Figure 1.5 Xcos model for PD control with gravity compensation pd_2link_step.zcos

It is well-understood in control engineering that, whenever plant model data is used in a controller, one always has to take into account the model uncertainty. In practice, we can never achieve a perfect model of a physical plant. So, to make the simulation realistic, we assume that 3 parameters m_i, l_{ci}, I_i, i = 1,2 used for gravity compensation deviate randomly within \pm 35 \% of their actual values.

et = 0.7;
m1_e = m1 + et*m1*(rand()-0.5); m2_e = m2 + et*m2*(rand()-0.5);
lc1_e = lc1 + et*lc1*(rand()-0.5);lc2_e = lc2 + et*lc2*(rand()-0.5);
I1_e = I1 + et*I1*(rand()-0.5); I2_e = I2 + et*I2*(rand()-0.5);

where m1_e, m2_e, lc1_e, lc2_e, I1_e, I2_e represent the estimated plant data. These values are used by the nonlinear expression blocks which are a part of the controller.

Note: when parameters vary randomly, the simulation result is slightly different between each run.

To see clearly the effect from interaction between the robot joints, we set the step command activation time of joint 1 and 2 to t=1 and t=2 second, respectively. The proportional and derivative gains can be set to your desired values by clicking on the corresponding gain blocks. To simplify the discussion, we choose the K_p and K_d gains for each joint identical; i.e., K_p = k_pI and K_d = k_dI with I a 2 x 2 identity matrix and k_p, k_d kp, kd are scalars. Figure 1.6 results from setting K_d = 200Iand varying K_p to 3 values: 1000I, 2000I, 3000I. As expected, the response is faster with increasing K_p, though it also worsens the overshoots and disturbance responses

Figure 1.6 step response when K_d fixed and K_p varied

Next, we keep the K_p values fixed to 3000I and vary K_d to 150I, 250I, 350I. The result is shown in Figure 1.7. We see that the step response goes to steady-state faster with increasing K_d, though too high values could destabilize the system. From our experiment, the feedback system is unstable when K_d is increased to 400I.

Figure 1.7 step response when K_p fixed and K_d varied

For some application that requires the robot to move along a smooth trajectory, the step response simulation may be inadequate. In such case, we can feed some setpoint vector to the model and see the resulting tracking response. Simply replace the step inputs by “From Workspace” blocks and setup the trajectory data to confrom to the format used by Xcos. Download this file pd_2link_track.zcos to see how this is done.

RTSX has a few trajectory generating commands for your convenience. For example, suppose we want to command the first joint from 0 to 30 degrees, and the second joint from 0 to -10 degrees simultaneously within 5 seconds using LSPB (Linear Segment with Parabolic Blend)


Now, put this trajectory data to a format that Xcos could read. Suppose we name the variables in From Workspace blocks for the first and second joints as Q1_d and Q2_d, respectively. Issue the following commands

Q1_d.time = t'; Q1_d.values = q1_d;
Q2_d.time = t'; Q2_d.values = q2_d;

Running the model pd_2link_track.zcos with 3 different gain values yields the tracking responses in Figure 1.8. Tracking performance can be determined more clearly by plotting the tracking errors as in Figure 1.9 and computing their RMS values.

Figure 1.8 tracking responses of PD control with gravity compensation

Figure 1.9 tracking errors of PD control with gravity compensation

Scilab/Xcos Files

For your convenience, here are the model and script files used in this article.

  1. mdl_2link.zcos open-loop 2-link manipulator with sinusoidal commands
  2. pd_2link_step.zcos step response simulation
  3. pd_2link_track.zcos tracking response simulation (requires RTSX)


  1. M.W.Spong, S. Hutchinson and M. Vidyasagar, Robot Modeling and Control. John Wiley & Sons. 2006.
  2. V. Toochinda, Robot Analysis and Control with Scilab and RTSX, Mushin Dynamics, 2014.



Comments are closed.