Lab report(robotics) 2018-BME-114

Biomedical Robotics Lab (BME-411L)

Sr List of Experiments CLOs

No. Covered
0 Introduction to Robotics CL01

1 Simulate the forward Kinematics of 2R robotic arm using geometric method in CL01
MATLAB.

2 Simulate the forward Kinematics of 3R robotic arm using geometric method in CL01
MATLAB.

3 Simulate the forward Kinematics of 2R robotic arm using DH parameters in CL01

MATLAB.

4 Simulate the forward Kinematics of 3R robotic arm using DH parameters in CL01

MATLAB.

5 Simulate the Inverse Kinematics of 2R in MATLAB. CL01

6 Simulate the Inverse Kinematics of 3R in MATLAB. CL01

7 Plot the position and velocity as a function of time in case of single joint trajectory CL01
planning

8 Plot the position and velocity as a function of time in case of two joints trajectory CL01
planning

9 Use two link manipulators to draw a straight-line trajectory. CL01

10 Use two link manipulators to draw a circular trajectory. CL01

11 Simulate forward kinematics of 2R, 3R manipulator using Roboanalyzer CL01

12 Introduction to six axis collaboration robots (Mycobot) CL01

1
Lab report(robotics) 2018-BME-114

INTRODUCTION TO ROBOTICS
Objectives
➢ Array forming
➢ Matrix forming
➢ Example 2.2 solved

1) Array forming in MATLAB:

[1 4 5 6 7]

Ans =

1 4 5 6 7

2) matric formation:
x=[1 2 3 ; 4 5 6; 7 8 9]

x=

1 2 3
4 5 6
7 8 9

3) example 2.2:
x=[1 2 3 ; 4 5 6; 7 8 9]
T=[0.866 -0.500 0 10; 0.500 0.866 0 5; 0 0 1 0; 0 0 0 1]
Bp=[3 7 0 1]'

2
Lab report(robotics) 2018-BME-114

x=

1 2 3
4 5 6
7 8 9
T=

0.8660 -0.5000 0 10.0000

0.5000 0.8660 0 5.0000
0 0 1.0000 0
0 0 0 1.0000

>> Bp=[3 7 0 1]'

Bp = 3
7
0
1

Ap= T*Bp

Ap = 9.0980
12.5620
0
1.0000

3
Lab report(robotics) 2018-BME-114

4
Lab report(robotics) 2018-BME-114

Simulate the forward Kinematics of 2R robotic arm using geometric method

in MATLAB.

Objective:
2R robotic arm by geometric method
Code:
l1=1;
l2=0.5;
time=0;
dt=0.2;
for i=1:1000
time=time+dt;
theta=pi/2;
theta1=pi/3;
x0= l1*cos(theta);
y0=l1*sin(theta);

x1=l2*cos(theta+theta1);
y1=l2*sin(theta+theta1);

x2= l1*cos(theta)+l2*cos(theta+theta1);
y2=l1*sin(theta)+l2*sin(theta+theta1);
%plotting equations
linka=line([0,x0],[0,y0],'linewidth',1.5);
linkb=line([x0,x2],[y0,y2],'linewidth',1.5);

pause(0.1)
delete(linka)
delete(linkb)
grid on
xlim([-3, 4])
ylim([-1, 4])
end

Explanation:
This is the code for ploting the forward kinematics of robot in matlab . l1 and l2 are the 2 link
lengths .i have used theta 90 and 60 which is equivalent to pi/2 and pi/3 for usage in matlab
expressions. Moving on we have formula turned to code for finding x2,y2 and x1,y2. Delete
command is used to remove the redundancy of the lines in the final plot.

5
Lab report(robotics) 2018-BME-114

Plot:

6
Lab report(robotics) 2018-BME-114

Simulate the forward Kinematics of 3R robotic arm using geometric method

in Matlab
Objective:
3R robotic arm by geometric method
Code:
l1=1;
l2=0.5;
l3=0.25;
time=0;
dt=0.2;
for i=1:1000
time=time+dt;
theta=pi/2;
theta1=pi/3;
theta2=pi/6;
x0= l1*cos(theta);
y0=l1*sin(theta);

x1=l2*cos(theta+theta1);
y1=l2*sin(theta+theta1);

x2= l1*cos(theta)+l2*cos(theta+theta1);
y2=l1*sin(theta)+l2*sin(theta+theta1);
%plotting equations
x3= l1*cos(theta)+l2*cos(theta+theta1);
y3=l1*sin(theta)+l2*sin(theta+theta1);

x4=l1*cos(theta)+l2*cos(theta+theta1)+l3*cos(theta+thet
a1+theta2);
y4=l1*sin(theta)+l2*sin(theta+theta1)+l3*sin(theta+thet
a1+theta2);
%plotting the links
linka=line([0,x0],[0,y0],'linewidth',1.5);
linkb=line([x0,x3],[y0,y3],'linewidth',1.5);
linkc=line([x3,x4],[y3,y4],'linewidth',1.5);
pause(0.2)
delete(linka)
delete(linkb)
delete(linkc)
grid on

xlim([-4, 5])
ylim([-2, 4])
end

Explanation:
Thetas used are 90, 60 and 30 which are pi/2,pi/3 and pi/6 respectively.

7
Lab report(robotics) 2018-BME-114

L1, l2 and l3 are 1, 0.5 and 0.25 respectively. Plot through geometric method and
DH method will be the same.

Plot:

8
Lab report(robotics) 2018-BME-114

Simulate the forward Kinematics of 2R robotic arm using DH parameters in

Matlab.
Objective:
Using DH parameters 2R robotic arm position
Function:
function [A]=DH (a,alpha,d,theta)
A=[cos(theta) -sin(theta)*round(cos(alpha))
sin(theta)*round(sin(alpha)), a*cos(theta)
sin(theta) cos(theta)*round(cos(alpha)) -
cos(theta)*round(sin(alpha)), a*sin(theta)
0 round(sin(alpha))
round(cos(alpha)) , d
0 , 0 , 0
, 1];
end
code:

l1=1;
l2=0.5;
t=0;
dt=0.2;
for i=1:100
t=t+dt;
theta1=pi/2
theta2=pi/3
%DH prameters
%link 1
a1=l1; alpha1=0; theta1=theta1; d1=0;
a2=l2; alpha2=0; theta2=theta2; d2=0;

%coordinates of first frame

Z0_1 = DH(a1,alpha1,d1,theta1);
%coordinates of second frame
Z0_2 = DH(a2,alpha2,d2,theta2);

U0_2=Z0_1* Z0_2;

x1=Z0_1(1,4);
y1=Z0_1(2,4);

x2=(U0_2(1,4));
y2=(U0_2(2,4));

linka=line([0,x1],[0,y1],'linewidth',1.0);

linkb=line([x1,x2],[y1,y2],'linewidth',1.0);
pause(0.1)
delete(linka)
delete(linkb)
grid on
xlim([-2, 4])
ylim([-2, 4])

9
Lab report(robotics) 2018-BME-114

end

explanation:
I have kept the parameters of link length and theta similar
to that of geometric method inputs. L1 and l2 are 1 ,0.5
respectively while thetas are pi/2,pi/4 respectively. We will
observe the graphs through DH method are also similar to
that of geometric method.

Plot:

10
Lab report(robotics) 2018-BME-114

Simulate the forward Kinematics of 3R robotic arm using DH parameters in

Matlab
Objective:
3R robotic arm using DH parameters
Function:
function [A]=DH (a,alpha,d,theta)
A=[cos(theta) -sin(theta)*round(cos(alpha))
sin(theta)*round(sin(alpha)), a*cos(theta)
sin(theta) cos(theta)*round(cos(alpha)) -
cos(theta)*round(sin(alpha)), a*sin(theta)
0 round(sin(alpha))
round(cos(alpha)) , d
0 , 0 , 0
, 1];
end
code:
l1=1;
l2=0.5;
l3=0.25;
time=0;
dt=0.2;
for i=1:1000
time=time+dt;
theta=pi/2;
theta1=pi/3;
theta2=pi/6;

%DH prameters
%link 1
a1=l1; alpha1=0; theta=theta; d1=0;
a2=l2; alpha2=0; theta1=theta1; d2=0;
a3=l3; alpha3=0; theta2=theta2; d3=0;
%coordinates of first frame
Z0_1 = DH(a1,alpha1,d1,theta);
%coordinates of second frame
Z0_2 = DH(a2,alpha2,d2,theta1);
%coordinates of thired frame
Z0_3 = DH(a3,alpha3,d3,theta2);
U0_2=Z0_1* Z0_2;
U0_3=U0_2*Z0_3;
x1=Z0_1(1,4);
y1=Z0_1(2,4);
x2=(U0_2(1,4));
y2=(U0_2(2,4));
x3=(U0_3(1,4));
y3=(U0_3(2,4));

%plotting
linka=line([0,x1],[0,y1],'linewidth',1.5);
linkb=line([x1,x2],[y1,y2],'linewidth',1.5);
linkc=line([x2,x3],[y2,y3],'linewidth',1.5);
pause(0.1)
delete(linka)
delete(linkb)

11
Lab report(robotics) 2018-BME-114

delete(linkc)
grid on
xlim([-4, 5])
ylim([-2, 4])
end
plot:

Explanation:
The only thing to notice is the same graph of 3R through
geometric as well as DH parameters.

12
Lab report(robotics) 2018-BME-114

Simulate the Inverse Kinematics of 2R in Matlab.

Objective:
lK of 2R in matlab
code:
l1=1;
l2=0.5;
x=-0.433013;
y=1.25;
ct2=((x^2+y^2-l1^2-l2^2)/(2*l1*l2))
st2=sqrt(1-(ct2)^2)
theta21=acosd((x^2+y^2-l1^2-l2^2)/(2*l1*l2))
theta22=atan2d(st2,ct2)
m1=[x (-l2*sind(theta22));y l1+l2*cosd(theta22)];
m2=[l1+l2*cosd(theta22) x;l2*sind(theta22) y];
m3=[l1+l2*cosd(theta22) (-l2*sind(theta22));l2*sind(theta22)
l1+l2*cosd(theta22)];
c1=det(m1)/det(m3);
s1=det(m2)/det(m3);
theta1=atan2d(s1,c1)
x0=0;
y0=0;
x1=l1*cosd(theta1);
y1=l1*sind(theta1);
x2=x1+l2*cosd(theta1+theta22);
y2=y1+l2*sind(theta1+theta22);
plot([x0,x1],[y0,y1],'b',[x1,x2],[y1,y2],'r','linewidth',3)
grid on
axis([-1 10 -1 7])

plot:

13
Lab report(robotics) 2018-BME-114

Explanation:
Same graph is obtained after inverse and forward kinematics.

14
Lab report(robotics) 2018-BME-114

Simulate the Inverse Kinematics of 3R in MATLAB.

Objective:
• Inverse kinematics of 3R
Code:
l1= 1;
l2= 0.5;
l3= 0.25;
u=-0.433013;
v=1.25;
x=-0.683013;
y=1.25;
ct2=((u^2+v^2-l1^2-l2^2)/(2*l1*l2));
st2=sqrt(1-(ct2)^2);
theta21=acosd((x^2+y^2-l1^2-l2^2)/(2*l1*l2));
theta2=atan2d(st2,ct2)
m1=[u (-l2*sind(theta2));v l1+l2*cosd(theta2)];
m2=[l1+l2*cosd(theta2) u;l2*sind(theta2) v];
m3=[l1+l2*cosd(theta2) (-l2*sind(theta2));l2*sind(theta2)
l1+l2*cosd(theta2)];
c1=det(m1)/det(m3);
s1=det(m2)/det(m3);
theta1=atan2d(s1,c1)
cphi=(u-x)/(-l3);
sphi=(v-y)/(-l3);
phi=atan2d(sphi,cphi);
theta3=phi-theta1-theta2
x0=0;
y0=0;
x1=l1*cosd(theta1);
y1=l1*sind(theta1);
x2=x1+l2*cosd(theta1+theta2);
y2=y1+l2*sind(theta1+theta2);
x3=x2+l3*cosd(theta1+theta2+theta3);
y3=y2+l3*sind(theta1+theta2+theta3);
%output
plot([x0,x1],[y0,y1],'c',[x1,x2],[y1,y2],'m',[x2,x3],[y2,y3],'b','linewidth',
3)
grid on
axis([-1 13 -1 11])

15
Lab report(robotics) 2018-BME-114

Explanation:
Inverse kinematics and forward kinematics should be same for forward and
inverse both. From graph of this lab and forward kinematics of 3R WE have
similar graphs.

16
Lab report(robotics) 2018-BME-114

Plot the position and velocity as a function of time in case of single joint
trajectory planning.
Objective:
Trajectory of single joint.
Code:
to=0;
tf=3;
d10=15;
d1f=75;

i=0;
for t=linspace(to,tf,10)
i=i+1;
tseconds(i)=t;
%ao=Qo=15
a1o=15;
%according to boundary conditions Qo derivative=a1=0;
a11=0;
%to find a2 a2=3(Qf-Qo)/tf.^2
a12=(3*(d1f-d10))/(tf.^2);
a13=(-2*(d1f-d10))/(tf.^3);
p1(i)=a1o+a11*t+a12*t*t+a13*t*t*t;
v1(i)=2*a12*t+3*a13*t*t;
%ao=Qo=15
end
figure
plot(tseconds,v1,'b','linewidth',3)
title(' velocity of link 1')
xlabel('time')
ylabel('v1')
grid on

figure
plot(tseconds,p1,'b','linewidth',3)
title(' position of link 1')
xlabel('time')
ylabel('p1')
grid on
figure

17
Lab report(robotics) 2018-BME-114

Plots:
Velocity plot:

18
Lab report(robotics) 2018-BME-114

Position plot:

19
Lab report(robotics) 2018-BME-114

20
Lab report(robotics) 2018-BME-114

Plot the position and velocity as a function of time in case of two joints
trajectory planning
Objective:
• Velocity and position graph
• 2 joints trajectory planning
Code:
to=0;
tf=5;
d10=20;
d1f=50;
d20=25;
d2f=60;
i=0;
for t=linspace(to,tf,10)
i=i+1;
tseconds(i)=t;
%ao=Qo=15
a10=20;
%according to boundary conditions Qo derivative=a1=0;
a11=0;
%to find a2 a2=3(Qf-Qo)/tf.^2
a12=(3*(d2f-d10))/(tf.^2);
a13=(-2*(d2f-d10))/(tf.^3);
p1(i)=a10+a11*t+a12*t*t+a13*t*t*t;
v1(i)=2*a12*t+3*a13*t*t;
%ao=Qo=15
a2o=25;
%according to boundary conditions Qo derivative=a1=0;
a21=0;
%to find a2 a2=3(Qf-Qo)/tf.^2
a22=(3*(d2f-d20))/(tf.^2);
a23=(-2*(d2f-d20))/(tf.^3);
p2(i)=a2o+a21*t+a22*t*t+a23*t*t*t;
v2(i)=2*a22*t+3*a23*t*t;
end

figure;
plot(tseconds,v1,'b','linewidth',3)
title(' velocity of link 1')
xlabel('time')
ylabel('v1')
grid on

figure;
plot(tseconds,p1,'b','linewidth',3)
title(' position of link 1')
xlabel('time')
ylabel('p1')
grid on

figure;
plot(tseconds,v2,'b','linewidth',3)

21
Lab report(robotics) 2018-BME-114

title(' velocity of link 2')

xlabel('time')
ylabel('v2')
grid on

figure;
plot(tseconds,p2,'b','linewidth',3)
title(' position of link 2')
xlabel('time')
ylabel('p2')
grid on
code screenshots:

Plots:

Link 1

22
Lab report(robotics) 2018-BME-114

23
Lab report(robotics) 2018-BME-114

Link 2:

24
Lab report(robotics) 2018-BME-114

Use two link manipulators to draw a straight-line trajectory.

Objective:
• Straight line motion observation
• 2 links trajectory.
Function:
function
[theta1_1,theta2_1,theta1_2,theta2_2]=RRmanipulator(l1,
l2,x0,y0)
c2=(x0*x0+y0*y0-l1*l1-l2*l2)/(2*l1*l2);
if c2<=1
s2_1=sqrt(1-c2*c2);
s2_2=-sqrt(1-c2*c2);
theta2_1=atan2(s2_1,c2);%first solution of theta2
theta2_2=atan2(s2_2,c2);%second solution of theta2

s1_1=y0*(l1+l2*cos(theta2_1))-x0*l2*cos(theta2_1);
s1_2=y0*(l1+l2*cos(theta2_2))-x0*l2*cos(theta2_2);
c1_1=x0*(l1+l2*cos(theta2_1))-y0*l2*cos(theta2_1);
c1_2=x0*(l1+l2*cos(theta2_2))-y0*l2*cos(theta2_2);
theta1_1=atan2(s1_1,c1_1);%first solution
theta1_2=atan2(s1_2,c1_2);%seond solution

end

Code:
l1=2;
l2=1;
n=100;%no of intermediate steps
xarray=zeros(n,1);
yarray=zeros(n,1);
theta1Array=zeros(n,1); axarray=zeros(n,1);
theta2Arrray=zeros(n,1); ayarray=zeros(n,1);
%parametric line or circle
p1=[1.5;0]; p2=[0;2];
%endeffector trajectory generation
delP=p2-p1;
tarray=linspace(0,1,n);
for i=1:n
t=tarray(i);
p=p1+t*delP;
xarray(i)=p(1);
yarray(i)=p(2);
end
%calling 2r inverse kinematics functions
[theta1_1,theta2_1,theta1_2,theta2_2]=RRmanipulator(l1,
l2,xarray(1),yarray(1));
thetaprevious=theta1_1; theta2previous=theta2_1;
for i=1:n
x=xarray(i); y= yarray(i);
[theta1_1,theta2_1,theta1_2,theta2_2]=RRmanipulator(l1,
l2,x,y);

25
Lab report(robotics) 2018-BME-114

if(((theta1_1-thetaprevious)^2)-((theta2_1-
theta2previous)^2)) <(((theta1_2-thetaprevious)^2)-
((theta2_2-theta2previous)^2))
theta1=theta1_1;
theta2=theta2_2;
else
theta1=theta1_2;
theta2=theta2_2;
end
axarray(i)= l1*cos(theta1); ayarray(i)=l1*sin(theta1);
theta1array(i)=theta1; theta2array(i)=theta2;
end
t=0;
dt=0.01;
tend=1;
%Animation
for i=1:n
t=t+dt;
px=axarray(i); py=ayarray(i); qx=xarray(i);
qy=yarray(i);
tracex(i)=qx; tracey(i)=qy;

%plotting the links

link1=line([0,px],[0,py],'linewidth',2);
link2=line([px,qx],[py,qy],'linewidth',2);
hold on;
plot(tracex,tracey,'k','linewidth',2);
hold off;
pause(0.5)
delete(link1)
delete(link2)
grid on
xlim([-4, 6])
ylim([-4, 6])
end

plots:

26
Lab report(robotics) 2018-BME-114

Explanation:
This code helps generating a line trajectory by following different sequences.
It keeps on ploting until a single line on graph remains. This is how a line
trajectory can be made.

27
Lab report(robotics) 2018-BME-114

Use two link manipulators to draw a circular trajectory.

Objective:
• Circular trajectory observation
• 2 link manipulator
Code:
L1=1;
L2=1;
r=0.4;
phi=0:1:360;
xx=1+r*cosd(phi);
X=[xx xx xx xx xx]
yy=1+r*sind(phi);
Y=[yy yy yy yy yy];
for i= 1:length(X)
x=X(i)
y=Y(i)
theta2=acosd((x^2+y^2-L1^2-L2^2)/(2*L1*L2));

theta1=asind((L2*sind(theta2))/sqrt(x^2+y^2))+atand(y/x
);
x1=L1*cosd(theta1);
y1=L2*sind(theta1);
figure(1)
plot(X,Y, 'b')
grid on
hold on
H=plot([0 x1],[0 y1], 'r', 'Linewidth', 5);
hold on
H1=plot([x1 x],[y1,y], 'r' , 'Linewidth' ,5);
axis([-2 2 -2 3])
pause(0.1)
delete(H)
delete(H1)
end
plots:

28
Lab report(robotics) 2018-BME-114

Explanation:

From this code we have obtained cicle trajectory and the

link is moving through it.

29
Lab report(robotics) 2018-BME-114

Introduction to six axis collaboration robots (Mycobot)

Mycobot:
myCobot is the world's smallest and lightest six-axis collaborative robot, jointly
produced by Elephant Robotics and M5STACK. It is more than a productivity tool
full of imaginations, can carry on the secondary development according to the
demands of users to achieve personalized customization.
With a weight of 850g, a payload of 250g and an arm length of 350mm, myCobot
is compact but powerful, can not only be matched with a variety of end effectors
to adapt to different kinds of application scenarios also support the secondary
development of multi-platforms software to meet the needs of various scenarios
such as scientific research and education, smart home, and commercial
applications.

Parameters

Parameter Data

Model myCobot-280

Working radius 280mm

Payload 250g

Arm Span 350mm

Repeatability ±0.5mm

Weight 850g

Power input 8V,5A

Working Conditon -5°~45°

Communication USB Type-C

30
Lab report(robotics) 2018-BME-114

Accessories

• myCobot
• End Effectors
o Parallel Gripper
o Adaptive Gripper
o Angled Gripper
o Suction Pump
• Base
o G Base
o Flap Base
• Others
o Rocker
o Battery Case

Features

• Unique Industrial Design & Extremely Compact

myCobot is an integrated modular design and only weighs 850g which is

31
Lab report(robotics) 2018-BME-114

very easy to carry. Its overall body structure is compact with less spare
parts and can be quickly disassembled and replaced to realize plug and
play.
• High configuration & Equipped with 2 Screens
myCobot contains 6 high-performance servo motors with fast response,
small inertia and smooth rotation, and carries 2 screens supporting
fastLED library to show the expanded application scene more easily and
clearly.
• Lego Connector & Thousands of M5STACK Ecological Application
The base and end of myCobot are equipped with Lego Connector, which is
suitable for the development of various miniature embedded equipment.
Its base is controlled by M5STACK Basic, and thousands of application
cases can be use directly.
• Bloky Programming & Supporting Industrial ROS
Using UIFlow visual programming software, programming myCobot is
simple and easy for everyone. You can also Arduino, ROS, or other
multiple functional modules of open source system, even RoboFlow,
software of industrial robots from Elephant Robotics.
• Track Recording & Learn by hand
Get rid of the traditional point saving mode, myCobot supports drag
teaching to record the saved track and can save up to 60mins different
tracks making it easy and fun for new players to learn.

32