FORWARD AND INVERSE KINEMATICS OF

VIPER S-650

ME-5337

Introduction to Robotics

Report by:

Patel, Dhruvik

Student No: 10

UTA ID: 1001276070

FORWARD KINEMATICS VIPER S-650
MDH TABLE OF VIPER S650

CURRENT NEXT JOINT JOINT JOINT a α d θ

FRAME FRAME NUMBER TYPE VARIABLE
0 1 1 R 0 0 0 d1 θ1
1 A - - - 0 π/2 0 π
A B - - - ab 0 0 - π/2
B 2 2 R θ a3 0 0 θ2
2 3 3 R θ a4 0 0 θ3
3 4 4 R θ a5 - π/2 d5 θ4
4 C - - - 0 π/2 0 π/2
C 5 5 R θ a6 0 0 - π/2+
θ5

𝑑1 = 203; 𝑎𝑏 = 75; 𝑎3 = 132; 𝑎4 = 270; 𝑑5 = 108; 𝑎5 = 90; 𝑎6 = 187;

𝑎5 = 90; 𝑎6 = 187
Individual homogeneous Transformation
Verification of frame assignment

For value of this θ:

𝜃1 = 0; 𝜃5 = 0; 𝜃2 = 0; 𝜃4 = 0; 𝜃3 = 0;

I got Homogeneous transformation of frame 5 w.r.t to frame 1 is

 Inverse Kinematics of Viper S-650
Modules

I have used 2 modules for inverse kinematics.

1) backtrans

This module take first frame and last frame for matrix multiplication. Give
output of matrix multiplication.

2) Invtrans

This module take last frame and first frame and calculate their inverse and
premultiply them with the help of mdhinv module desire matrix
For θ1 :

To find θ1 I have compared homogeneous transformation of frame 5 w.r.t to frame

1 (1T5 ) and premultiply desire transformation matrix 0T1 inverse. The pz element of
both matrix is compared for θ1.

−pyCos[𝜃1 ] + pxSin[𝜃1 ] = 0

Code to find θ1 :

The for loop in the code store the value of θ1 in solmat matrix.
For θ4:

To find θ4 I have compared homogeneous transformation of frame 5 w.r.t to frame 1

(1T5 ) and premultiply desire transformation matrix 0T1 inverse. The az element of
both matrix is compared for θ4.

−ayCos[𝜃1 ] + axSin[𝜃1 ] = Cos[𝜃4 ]

Code for finding θ4

For loop stores the value of θ4 in solmat matrix.

For θ3:

To find θ3 I have compared homogeneous transformation of frame 5 w.r.t to frame

3(3T5 ) and premultiply desire transformation matrix with 0T2 inverse. The sum of
the square of px and py element is compare for θ3.

2
((−335 + pz)Cos[𝜃2 ] − (75 + pxCos[𝜃1 ] + pySin[𝜃1 ])Sin[𝜃2 ]) + (5(54 +
18Cos[𝜃3 ] − 59Sin[𝜃3 ]))^2=(−Cos[𝜃2 ](75 + pxCos[𝜃1 ] + pySin[𝜃1 ]) − (−335 +
pz)Sin[𝜃2 ])^2 + (295Cos[𝜃3 ] + 90Sin[𝜃3 ])^2
Further expansion of above equation gives,

1
(−100350 + px 2 + py 2 − 1340pz + 2pz 2 + 300pxCos[𝜃1 ]
2
+ (px 2 − py 2 )Cos[2𝜃1 ] − 97200Cos[𝜃3 ] + 300pySin[𝜃1 ]
+ 2pxpySin[2𝜃1 ] + 318600Sin[𝜃3 ]) = 0

Which is in terms of the 𝜃1 and𝜃3 . So from that we can find the value of 𝜃3 .

Code for finding the value of𝜃3 .

For loop stores the value of 𝜃3 in solmat matrix.

For θ2:

To find θ2 I have compared homogeneous transformation of frame 5 w.r.t to frame

3(3T5 ) and premultiply desire transformation matrix with 0T2 inverse. The px
element is compare for θ2.

(−335 + pz)Cos[𝜃2 ] − (75 + pxCos[𝜃1 ] + pySin[𝜃1 ])Sin[𝜃2 ]= 5(54 + 18Cos[𝜃3 ] −

59Sin[𝜃3 ])

This equation consist of the value of 𝜃3 and𝜃1 . we know the value of that so we can
find the 𝜃2 .

Code to find θ2 with help of θ11 :

This for loop updates the value of θ2 in solmat matrix.

Code to find θ2 with help of θ12:

This for loop updates the value of θ2.
For θ5:

To find θ5 I have compared homogeneous transformation of frame 5 w.r.t to frame

4(4T5 ) and premultiply desire transformation matrix with 0T4 inverse. The ox and oy
element of two matrix is to be compare for θ5.

1
(−2qxCos[𝜃1 ]Cos[𝜃2 + 𝜃3 ] − qySin[𝜃1 − 𝜃2 − 𝜃3 ] − 2qzSin[𝜃2 + 𝜃3 ] −
2

qySin[𝜃1 + 𝜃2 + 𝜃3 ]) = Cos[𝜃5 ]

And

1
(−2nxCos[𝜃1 ]Cos[𝜃2 + 𝜃3 ] − nySin[𝜃1 − 𝜃2 − 𝜃3 ] − 2nzSin[𝜃2 + 𝜃3 ]
2
− nySin[𝜃1 + 𝜃2 + 𝜃3 ]) = Sin[𝜃5 ]

From the above equation we can find the value of θ5 .

Code to find θ5 :
Final set of solution

I have stored the all solution of θ in solmat matrix.

This for loop make the first column of the matrix zero for that solution which are
imaginary and not in the range of joint value and make others column 1.

This for loop prints the final set of solution form the solmat matrix.
Optimum solution:

This for loop calculates the optimum value of θ for pictured configuration.