How do I solve forward dynamics 2nd order ODE with time-dependent parameters using ODE45?

I'm trying to solve a forward dynamics problem using ODE45, but the problem is tau is a vector in part (c) and so ODE45 is showing error when I'm using tau vector,

I'm not able to solve (c) part (denoted by red dot) of my assignment. here is my code,

This is the function I'm using, here tau = Torque, I've already calculated tau in first practical and I've to use it in this practical but I don't know how to do it,

[Torsten]

Call ODE45 for each value of tau separately, i.e. loop over the tau-vector:

for i=1:numel(tau)
    tauh = tau(i);
    [time,Y] = ode45(@(t,y)Diffeqn(t,y,tauh,m,L,Lc,Izz,g),t,y);
    ...
end

[Akash Vyas]

I've already tried this solution but I don't think it's correct since for each value of tau, Diffeqn() function will take t = 0:0.01:3 (t vary in this range) which I don't think is correct.

[Torsten]

So tau is not an independent constant which gives separate solutions for each value of tau, but a time-dependent array for one simulation ?

[Sam Chak]

Hi @Akash Vyas

The symbol tau (τ) is the control torque.

You've got the error because you put tau into an algebraic loop error that ODE45 cannot effectively solve:

Either it is a feedback control law (a formula) to compute the desired torque value that depends on θ and , or a fixed torque value (or a series of torque values at certain time intervals where you need to inject them in to the system) that you obtained from Practical 1.

If it is a feedback control law, then tau looks like this:

.

The closed-loop system then becomes

,

which can be simplified to

.

So, the desired response of the 2nd-order system is determined by the selection of

the fundamental proportional gain, and the fundamental derivative gain, .

Since the operation time only lasts for 3 seconds, I assume that the robot arm shall achieve the steady-state () at 1 second from the initial time. If that is acceptable to you, then you can try for

and , or and .

Don't forget to multiply the fundamental control gains with the inertia .

kp = 36;
kd = 12;
tau = - Izz*kp*theta - Izz*kd*dtheta + m*g*Lc*cos(theta);

[Akash Vyas]

Hii.. @Sam Chak, in practical-1 we did inverse dynamics in which the initial condition were ,

and the link followed a cycloidal trajectory:

Now, from this we'll calculate for t = 0:0.01:T and using these values we did inverse dynamics to calculate tau, now this tau values we'll use in next practical (forward dynamics) as mention in first post.

one more thing 't' is same in both the analysis.

So, yes you are right.

So, this is the complete code, the part in black box is performing inverse dynamics and tau from this is used in forward dynamics in the red box,

jus for your referance this is the torque profile which I'm getting from inverse dynamics which is used in forward dynamics,

Answer 1

[Sam Chak]

With the new info on the desired joint angle trajectory profile , the following simulations are based on 3 approaches on the control torque, τ.

Approach #1: Based on the computed torque from your Practical #1 when the desired , , are known.

Approach #2: Based on the computed torque from Approach #1, but is replaced by .

Approach #3: Based on the computed torque from Approach #2 and PD feedback control law.

function dydt = odefcn(t, y)
    dydt = zeros(2,1);
        % desired angular position profile
		yd = ((120*pi/180)/3)*(t - (3/(2*pi))*sin((2*pi/3)*t));
        % desired angular velocity profile (time derivative of yd)
		dyd = (2/9)*pi*(1 - cos((2*pi/3)*t));
        % desired angular acceleration profile (time derivative of dyd)
		ddyd = (4/27)*(pi^2)*sin((2*pi/3)*t);
        % parameters
		m = 1;              % mass of robot arm
		g = 9.81;           % gravity
		L = 1;              % length of robot arm
		l = 0.5;            % length of link
		Iz = (1/3)*m*L^2;   % moment of inertia
		omega = 6;          % natural frequency
		zeta = 1;           % damping ratio
		kp = omega^2;       % fundamental proportional control gain
		kd = 2*zeta*omega;  % fundamental derivative control gain
        % Control Torque (3 approaches)
		tau1 = Iz*ddyd + m*g*l*cos(yd);
		tau2 = Iz*ddyd + m*g*l*cos(y(1));
		tau3 = Iz*ddyd + m*g*l*cos(y(1)) - Iz*kp*(y(1) - yd) - Iz*kd*(y(2) - dyd);
    dydt(1) = y(2);
    dydt(2) = (tau1 - m*g*l*cos(y(1)))/Iz;
end

Results

tspan = linspace(0, 3, 3001)';  
y0 = [120*pi/180; 0];		    % initial condition (based on Problem 3.a)
[t, y] = ode45(@(t,y) odefcn(t, y), tspan, y0);

Approach #1 does not work because the initial joint angle is , but is computed from when .

Despite both trajectories have the same pattern, Approach #2 is unsuccessful because there is an offset of .

By introducing the PD control to correct the offset error, Approach #3 can drive the joint angle θ to follow the desired profile after 1 second.

[Akash Vyas]

Thanks @Sam Chak for the solution.

Answer 2

[Bjorn Gustavsson]

Following from the comments above it seems as if you have a smoothly varying time-dependent torque that you know at a set of points in time. For that you could use simple interpolation of the torque at any given point in time. To do that you need to modify your ODE-function to allow for both a torque_of_t and a t_for_torque variables, instead of a single torque-input. Perhaps something like this:

function dy = ODE_fcn(..., tau_of_t,t_for_tau)
tau_now = interp1(t_for_tau,tau_of_t, t,'pchip');
% Then plug in tau_now where you use tau.
% The rest should be fine
end

Or you could wrap the interpolation into a function-handle:

tau_fcn =  @(t) interp1(t_for_tau,tau_of_t,t,'pchip');
% then send this into your ODE
% But then you need to change the use of tau to a call to the function:
% Inside ODE:
tau_now = tau(t);

HTH

[Akash Vyas]

I don't think this will work since the step size in Inverse dynamics is 0.01 but in forward dynamics step size of tspan varies in function formulation, so there steps need to be synchronized.

But thanks for your efforts.