Lab 3: Solving Robotics Dynamics

Introduction

In lectures, we have discussed how to establish the dynamics equation for robotic manipulators. The final dynamics equation is essentially a set of ordinary differential equations (ODEs). How to solve such ODEs will be important to simulate the motion of robotic manipulators. For instance, if we know the force/torques generated by the motor for each joint, how can we predict the motion for the manipulator? In this lab, we will learn how to use built-in functions in Matlab to numerically solve ODEs, and we will also apply the methods to simulate the dynamics motion for a given robotic manipulator.

Learning Objectives:

Introduction to ODE solving
Use MATLAB ODE solvers to solve sample ODEs.
Solve robotics dynamics

Task 1 – Make a Function to Solve a 1st Order ODE using ODE45

In Lab 1, you learned the format of custom MATLAB™ functions. You wrote a function to evaluate a nonlinear mathematical equation.

Simple ODEs that have a single solution component can be specified as a function or an anonymous function in the call to the solver. The anonymous function must accept two inputs (t,y), even if one of the inputs is not used in the function.

Solve the ODE

Specify a time interval of [0 5] and the initial condition y0 = 0.

figure('Renderer', 'painters','units', 'pixels', 'Position', [10 10 400 300])
tspan = [0 5];
y0 = 5;
[t,y] = ode45(@(t,y) 2*t, tspan, y0);
figure('PaperPositionMode','auto')
plot(t,y,'-o')
xlabel('t')
ylabel('y')

We can also plot the exact solution

hold on

plot(t, t.^2+5, 'r')

Task 2 – Make a Function to Solve a 2nd Order ODE using ODE45

Example 1: Free falling of a 1-link robotic manipulator- pendulum model

Fig. 1 1-link manipulator under gravity can be simplified to a pendulum.

If we release a 1-link robotic manipulator from a certain height, it is equivalent to a pendulum. the result is a set of two, 1st order, non-linear, ordinary differential equations of the form:

with

We can evaluate the differential equation at a single value of x1 and x2 by making a custom MATLAB™function that will operate on the vectors x = [x1 ; x2] and the time t to output the vector x_dot = [x1_dot ; x2_dot]:

function [x_dot] = ME564_1_link_pendulum(t,x)

x1 = x(1); % break up x into x1=theta

x2 = x(2); % break up x into x2=theta_dot

g = 9.81; %meter per second squared

L = 1; %meter

x1_dot = x2;

x2_dot = -(g/L)*sin(x1);

x_dot = [x1_dot; x2_dot];

end

x0 = [0; 0]; 
t = 0; 
[x_dot] = ME564_1_link_pendulum(t,x0) 
x_dot = 2×1
     0
     0

clc; clear

x0 = [0.5; 0]; % initial conditions for theta and theta dot

[t_out,x_out] = ode45(@ME564_1_link_pendulum,[0 5],x0);

figure('Renderer', 'painters','units', 'pixels', 'Position', [10 10 400 300])

plot(t_out, x_out(:, 1), '.-');

hold on

plot(t_out, x_out(:, 2), '--');

xlabel('Time, sec')

ylabel('\theta, rad ; d\theta/dt, rad/sec')

legend('Theta', 'Theta dot')

L = 1; 
x_pos = 2*L*sin(x_out(:,1));
y_pos = -2*L*cos(x_out(:,1));
filename = 'mech564.gif';
figure('Renderer', 'painters', 'units', 'pixels', 'Position', [100 100 500 500])
for i = 30:length(x_pos)
    clf
    axis equal
    xlim([-1.5*L, 1.5*L])
    ylim([-2.5*L,0.5]); 
    hold on
    plot(x_pos(i)/2, y_pos(i)/2, 'o');
    plot([0, x_pos(i)], [0, y_pos(i)], 'k')
    drawnow
    frame = getframe(gcf);
    im = frame2im(frame);
    [imind,cm] = rgb2ind(im,256);
    if i == 30
        imwrite(imind,cm,filename,'gif', 'Loopcount',inf);
    else
        imwrite(imind,cm,filename,'gif','WriteMode','append');
    end
end

The code will creat a gif to show the motion. To show the gif run the following code.

web('mech564.gif','-new','-notoolbar')

Tasks 3: Steps to Solve Robot Dynamics

Develop a software to simulate a robot dynamics model, i.e., numerically solve

You can also directly use ODE solvers in Matlab (e.g., ode45, ode23).

In the following, I will use a 2-link robot as an example to demonstrate the process.

Steps:

1) Rearrange the equation

2) Rewrite the dynamics equation into the correct form that can be used by ODE solver

For exmaple, for a two-link robot

then

3) Create a function to return coefficient matrices

[D, C, G] = coeff_matrices(x)

4) Create a function to return the derivatives

q2dot = dynamics_func(Tau_in, D, C, G)

5) Wrap these two function into a function that can be called by ode45

function xdot = dynamics(t, x)

.....

[D, C, G] = coeff_matrices(x)

q2dot = dynamics_func(Tau_in, x, D, C, G)

xdot(1) = x(3);

xdot(2) = x(4);

xdot(3) = q2dot(1);

xdot(4) = q2dot(2);

end

5) Solve the function with the ODE45 solver

Homework:

Parameters for the two robots can be found in the appendix pdf.

1. Solve Dynamics of a 2-link robot (6 points)

Test your dynamics model with the follow two cases, you may use different ode solvers (eg, ode23) in case the system is stiff. Simulate 5 s.

a). With initial angle

rad and initial angular velocity

rad/s, release the robot.

b). With initial angle

rad and initial angular velocity

rad/s, simulate the case using an input

Plot the output

, and

with respect to time in a single figure. Also plot the position of

(not separate x and y) in a single figure. Explain the plots you have generated.

c). Optional. Generate an gif animation to show the pendulum.

2: Solve Dynamics of a Puma 560 (9 points)

With the implementation, test your dynamics model with an input

, plot the output

, and

in a single figure. Also plot the position x, y, and z in a single figure. Explain the plots you have generated.

Requqirement

The report must be typed, and turned in before the due date.

In your report all tasks with proper explanations and discussions must be documented.

The derivation and design steps have to be presented and clearly explained.

The block diagram of all computer programs must be included in the report. All source codes of the computer programs must be clearly documented and submitted as Appendix at the end of the report.

All plots and diagrams must have proper captions and be clearly explained.

References must be provided in the report for any methods used in the project from sources other than the textbook and class notes.

All the documents including report and source codes should be submitted in a single Zip file on Canvas. Please name the zip file as FirstName\_LastName.zip (e.g., Jianguo\_Zhao.zip).

Attached Code

function [x_dot] = ME564_1_link_pendulum(t,x) 
x1 = x(1); % break up x into x1=theta 
x2 = x(2); % break up x into x2=theta_dot 
g = 9.81; %meter per second squared 
L = 1; %meter 
x1_dot = x2; 
x2_dot = -(g/L)*sin(x1); 
x_dot = [x1_dot; x2_dot]; 
end 