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

Question

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?

Answer 1

According to this master thesis, the Dynamic Model obtained from the Newton Euler Method for articulated arms cannot be controlled (as in control theory) by applying torque directly without feedback. I suppose the floating point lack of precision is sufficient to make the robot deviate from the wanted behavior.

This is the best answer I could conclude, somebody with knowledge of control theory and robotics can confirm this.