# 2-DOF Robotic Manipulator Trajectory Tracking Simulation

I am trying to simulate a 2-DOF planar robotic manipulator (have its joints follow a predefined trajectory) that's described by its dynamic model:

$$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q) = \tau$$

where $$q = [q_1 \ q_2]^T$$ are the angles of the two joints, $$\dot{q}$$ the angular velocities and $$\ddot{q}$$ the angular accerations of the joints. In order to simulate it, I convert it into its state-space representation (matrix $$M$$ is positive definite) by defining the state variables $$x=[q_1 \ \dot{q_1} \ q_2 \ \dot{q_2}]^T$$.

$$\dot{x_1} = x_2$$ $$\dot{x_2} = -M^{-1}(1,:)\cdot [C\dot{q} + G] + M^{-1}(1,:)\tau$$ $$\dot{x_3} = x_4$$ $$\dot{x_4} = -M^{-1}(2,:)\cdot [C\dot{q} + G] + M^{-1}(2,:)\tau$$

In order to close the loop, I have designed a prescribed performance controller, which uses performance functions making the whole system of the differential equations stiff. I managed to simulate the system by using the ode15s(...) MATLAB solver by setting its tolerances as follows:

$$\text{RelTol = 1e-6}$$ $$\text{AbsTol = 1e-10}$$

However, in order to simulate the system I had to select a really large value for a controller gain $$K$$, which appears at the numerator of the controller expression:

$$u = \frac{K\gamma(p-\Delta)s_i}{\dot{T}(w_{sq}-1)}$$

where $$p$$ is the performance function, $$T$$ is a strictly increasing function $$T:(-1,1) \rightarrow \mathbb{R}$$ and $$\gamma, \Delta, s_q, w_{sq}$$ some signals. I am wondering about the selection of the options for the ODE-Solver. My wish would be to try and select the least possible value for the gain $$K$$ of the controller. What do the solver's tolerances mean practically or would it really be a good idea to provide the solver with the Jacobian matrix of the differential equations I want to simulate (cause I know the solver approximates it using Finite Differences) ? I know this could be really hard and large to answer, so any possible proposal about a decent resource where I could read about these aspects (other than the matlab documentation) would be really appreciated.

• What happens when $K$ is small? ode15s doesn't converge? Is there anything preventing you from computing the Jacobian? Unless your mass matrix is very simple, I would recommend using the Mass option for odeset so the inverse of the mass matrix doesn't make the Jacobian any more complex. – Steven Roberts Nov 18 '20 at 15:32
• When $K$ is small the solver won’t converge and either gets stuck at some time instant or results are completely wrong. The fact is that I want to simulate a 7-DOF manipulator after that so computing the jacobian would be my very last choice. I am just passing the sparse matrix as ode option to the solver. Do you mean that although I am solving this system and not the $M(q)\ddot{q}$ I should put the mass matrix as ode option ? – Teo Protoulis Nov 18 '20 at 16:02

As far as I understand the fist part of your question is regarding the options of the ode15s, and which ones you can use to make the solver more efficient/accurate.

1. Providing the Jacobian to the solver, especially if its reasonably easy to calculate, is a good idea to make things faster and more accurate.
2. Using sparse systems for small systems of equations like yours will not make things better in terms of speed. This is vaguely mentioned in MATLAB's website here, and from experience I might go so far as to say that it might actually make things slower.
3. Since you have a state dependent mass matrix called $$M(q)$$, it is also a good idea to insert it as an ode option as also suggested in the comments. This is done the same way that you defined your tolerances as options, only this time you name-value arguments in the odeset are 'Mass',@(t, q) Mf(t, q, ...) where Mf is the function of your mass matrix, t is the time variable which you have to define as input within said function regardless of whether you utilise it, and q is the state dependence, along with ... symbolising any other arguments the mass function might need to work. See MATLAB's specific instructions regarding this here. By doing this the solver processes the mass matrix more efficiently and aids the solver.
4. If things don't work out with ode15s, attempt also other implicit solvers such as ode23t and ode23tb. The former is a first order implicit Euler scheme, while the latter is an implicit trapezoidal rule combined with an implicit backward differentiation formula, known as TR-BDF2. I have been working on stiff problems myself where ode15s didn't work, whist the other two that I mentioned did, so its worth a try.

The second part of your question is what is the physical meaning of the tolerances. The formula for the tolerances within matlab, is this:

$$e_i \leq \max(\text{rtol}\cdot |y_i|, \text{atol})$$

This means that ode15s will adjust its time step internally such that the local truncation error at the $$i^\text{th}$$ step symbolised by $$e_i$$, is always less than or equal to the expression on the right hand side. Therefore, physically your rtol or relative tolerance sets the number of digits that you whish to have correct in your solution of your ODE, while the atol or absolute tolerance is there as a safety factor, such that if your $$y_i$$ becomes zero or extremely close to it, it takes over to satisfy the inequality.

I hope this helps!

• Really great and useful answer. Especially the last part regarding the tolerances made things crystal clear for me. Thank you ! – Teo Protoulis Nov 20 '20 at 20:25