Lab 2: Position and Velocity Kinematics
2/26/2022
Table of Contents
Task1: Symbolic Toolbox
Taks 2: Forward Position/Velocity Kinematics Programming
Forward Position Kinmatics.
Forward Velocity Kinematics
Direct differentiation to find the manipulator jacobians
Using the derived equation with cross product
Task 3: Forward Position/velocity Kinematics using the Robotics toolbox
Forward position kinematics
Forward velocity kinematics
Task 4: Homework
1. Forward position kinematics (5 points)
2. Forward velocity kinematics (5 points)
Task1: Symbolic Toolbox
Very useful tool to derive kinematics
clc; clear;
syms theta d a alpha pi;
theta
H = trotz(theta)*transl(0,0,d)*transl(a,0,0)*trotx(alpha)
Symbolic substitution. Suppose 
H_sub = subs(H,alpha, pi/2)
Taks 2: Forward Position/Velocity Kinematics Programming
Follow me to type in and I will leave you 5 mins each section for you to try it out.
We use a two-link robot as an example. Review your notes. Establish the frame as we did in the class.
We will use Matlab Symbolic Toolbox and derive what we have done in the class.
Forward Position Kinmatics
.
clc; clear;
Anonimous function to represent one link
H_func = @(a, d, alpha, theta) trotz(theta)*transl(0,0,d)*transl(a,0,0)*trotx(alpha)
Symobolic variables.
syms theta_1 a_1 theta_2 a_2;
T_01 = H_func(a_1, 0, 0, theta_1)
T_12 = H_func(a_2, 0, 0, theta_2)
T_02 = T_01*T_12
T_02 = simplify(T_02,Steps=10)
Compare the results with these obtained from the class.
You can evalueate this with some values for example
for 
, 
, 
, and 
, , , and T_02val = subs(T_02, [theta_1, a_1, theta_2, a_2], [deg2rad(30), 1, deg2rad(40), 1])
double(T_02val)
Forward Velocity Kinematics
Take derivative of the forward position kinematics
The Jacobian matrix of the vector function f = (f1(x1,...,xn),...,fn(x1,...,xn)) is the matrix of the derivatives of f:
Position and orientation of the end effector
where 
, 
, Direct differentiation to find the manipulator jacobians
Take drive of the position vector with respect to the variables.
Jv = jacobian([T_02(1:3, 4);0;0;theta_1+theta_2], [theta_1, theta_2])
Using the derived equation with cross product
Review the contents we have in the class.
O_0 = [0 0 0]'; %
O_1 = T_01(1:3, 4); % position vector from transformation matrix
O_2 = T_02(1:3, 4); %
Z_0 = [0 0 1]'; %
Z_1 = [0 0 1]'; % Z vector from transfromation matrix
J_1 = [cross(Z_0, (O_2-O_0)); Z_0]; % Jacobian matrix
J_2 = [cross(Z_1, (O_2-O_1)); Z_1];
J = simplify([J_1 J_2])
Task 3: Forward Position/velocity Kinematics using the Robotics toolbox
In this task, we will use the robotics toolbox to derive the forward position/velocity kinematics
Forward position kinematics
Using the toolbox:
clc; clear;
In the Toolbox we connect Link class objects in series using the SerialLink class.
A Link object holds all information related to a robot joint and link such as kinematics parameters, rigid-body inertial parameters, motor and transmission parameters.
% create the leg links based on DH parameters
% theta d a alpha joint type
links(1) = Link([ 0 0 1 0 0] );
links(2) = Link([ 0 0 1 0 0] )
robot = SerialLink(links, 'name', 'twolink')
Define two real-valued symbolic variables to represent the joint angles
syms theta_1 theta_2
The forward kinematics are
robot.fkine([theta_1, theta_2])
robot.fkine(deg2rad([30 40])) %suggested
robot.fkine([30 40], 'deg') % cause error due to degree conversion. fkine to radian
% robot.fkine([theta_1, theta_2], 'deg')% help you check why
Forward velocity kinematics
TE = robot.fkine( [theta_1 theta_2] );
TE
Compute the forward kinematics using these
The position of the end-effector p
p = TE.t; % TE homogeneous transformation matrix, t is the position column.
p = p(1:2) % take the x and y component cos(x(1)+x(2)) +cos
and we compute the derivative of p with respect to the joints variables q. Since p and
q are both vectors the derivative.
J = jacobian(p, [theta_1 theta_2])
Task 4: Homework
1. Forward position kinematics (5 points)
(a). Derive and simplify the forward kinematics model of PUMA 560 robot arm, shown in Figure 1 for the first four links (link 0 to link 3) using simbolic tool box. Using the derived equation, write a Matlab program to implement a forward kinematics model for the first four links. The program should have three joint angles (
;
; 
) as the input and the numerical value of the end-effector position (
) as the output. Your function should be able to report errors in case input data are out of joint limits. Test your program by using following three inputs (the unit for the angles are degree), turn in your codes and testing results. First input: (25, 45, -10); Second input: (-100, -260, -30); Third input: (70, 0, 50).
; ; ) as the input and the numerical value of the end-effector position () as the output. Your function should be able to report errors in case input data are out of joint limits. Test your program by using following three inputs (the unit for the angles are degree), turn in your codes and testing results. First input: (25, 45, -10); Second input: (-100, -260, -30); Third input: (70, 0, 50). (b). Use the Robotics toolbox to solve the forward kinematics for these three inputs and compare the results with those from your own code. You may need to refer to Ch7 of the reference textbook "Robotics, Vision, and Control, Edition 2" By Peter Corke or read the manual of the robotics toolbox (http://www.petercorke.com/RTB/robot.pdf) to see the detailed steps.
2. Forward velocity kinematics (5 points)
(a). Derive and simplify the Jacobian of PUMA 560 robot arm shown in Figure 1 for the first four links using simbolic tool box. Using the derived equation, write a Matlab program to implement the Jacobian for the first four links. The program should have three joint angles (; ; ) as the input and the numericl value of the Jacobian as the output. Your function should be able to report errors in case input data are out of joint limits. Test your program by using following three inputs, turn in your codes and testing results. 1. (25, 45, -10); 2. (-100, -260, -30); 3. (70, 0, 50).
; ; ) as the input and the numericl value of the Jacobian as the output. Your function should be able to report errors in case input data are out of joint limits. Test your program by using following three inputs, turn in your codes and testing results. 1. (25, 45, -10); 2. (-100, -260, -30); 3. (70, 0, 50). (b). Use the Robotics toolbox to solve the same problem, and compare the results with those from your own code
Fully document your homework solutions and you may use this live script as a template. All problem statements must be copied to the solutions. All diagrams and plots must be labeledwith units and symbols and must be captioned.
Figure 1: Problem 1 and 2.