# Integrating direct dynamics form more than 1 second does not give back the correct result

I am trying to simulate a robot manipulator dynamics in SciLab.

Basically, I generated a step function that has constant acceleration for half of the time and then the same acceleration but negative for the other half, so I get a smooth transition between the manipulator positions.

This code generates the velocity and position from the step function I mentioned:

function [position,velocity,acceleration,time]=smoothTransition(initialPosition,finalPosition,resolution,timeSpan)

a=(finalPosition-initialPosition)/((timeSpan/2)**2);//magnitud of acceleration and deceleration so I get to final position in timespan
acceleration=[ones(1,resolution/2)*a -ones(1,resolution/2)*a];
if modulo(resolution,2) ~= 0 then
acceleration=[acceleration -a];//case where time resolution is odd
end
time=linspace(0,timeSpan,resolution);
function dx=f(t,x)
dx(1)=x(2);
dx(2)=linear_interpn(t,time,acceleration);

endfunction
x=ode([0;0],time(1),time,f);

velocity=x(2,:);
position=x(1,:);
endfunction


Basically, I integrate the step function twice.

The formula to get the torque required is:

$$\tau=gm_1s_{1x}\cos(q)+\ddot{q}m_1s_{1x}^2$$

(this is a simplified version with one link)

Where $g$ is the gravity magnitude, $s_{1x}$ is how far is the center of mass in the x-direction, $m_1$ is the mass of the link, and $q$ is the angle.

What I am trying to do is generate a torque input with this equation an then do the numeric integration to get $q$ and its derivative back (mostly for testing purpose).

So I am trying to solve this numerically: $$\ddot{q}=\frac{\tau-gm_1s_{1x}\cos(q)}{m_1s_{1x}^2}$$

The problem is that I don't get the same behavior back when I integrate for more than 1 second.

The code to do this is as follows

m_1=1;
g=9.81;
s_1x=1;
[position,velocity,acceleration,time]=smoothTransition(0,%pi/2,100,10);

tau=g*m_1*s_1x*cos(position)+acceleration*m_1*s_1x**2;

function dx=f(t,x)
torque=linear_interpn(t,time,tau);
dx(1)=x(2);
dx(2)=(-g*m_1*s_1x*cos(x(1))+torque)/(m_1*s_1x**2);
endfunction
q0=[0;0];
q=ode(q0,0,time,f);
plot(time,position,'r');
plot(time,q(1,:),'g');


In this code I plug the $\tau$ input and integrate two times, I suspect the problem is in this part.

With this code I get the following figure:

Where the red curve is the expected behavior and the green curve is the obtained one.

By the way, the problem persists if I increase the time resolution.

Edit:

I realize that when I crank up the resolution (let say 10000) the result curve (green) does approximate the the correct behavior (red).

Here the result with 10000 time resolution:

Is there a way to do a more exact integration without so much time resolution?

• In your differential equations is $\tau$ a function of time? – nicoguaro Sep 10 '18 at 2:55
• nicoguaro, that is a good question, I think is a function of time because the second derivative of q is over time (d²q/dt²). – user1006274 Sep 10 '18 at 12:32
• @LonelyProf "time" and "tiempo" are the same variable, the same for "acceleration" and "aceleration" (changed variable names for the question, that is why the inconsistencies). The two equations are the same solved for different variables. I edited the question. I hope it is clear now. – user1006274 Sep 10 '18 at 12:53
• I don't understand how is your $\tau$ defined. – nicoguaro Sep 11 '18 at 16:03
• Can you try to set the value for rtol at $1e-12$ or so (smaller than the default value)? – GertVdE Sep 12 '18 at 7:58