Robot Mechanics and Control Robot Mechanics and Control

Example 4.03:: 2R Planar: Alternate Geometric Solution (MATLAB)

2R Planar Manipulator

This example computes the inverse kinematics for the 2R planar manipulator via a geometric solution.

Contents

Clear All Workspace Objects and Reset All Assumptions

clear all

DH Parameters

a1 = 30; % cm
a2 = 30; % cm

Position for the Origin $O_{2}$

px = 49.5; % cm
py = -10.0; % cm

Computation for $r$

r = sqrt(px*px + py*py); % cm

Computation for $\phi_{1}$ and $\phi_{2}$

cos_phi1 = (a1*a1 + r*r - a2*a2)/(2*a1*r);
cos_phi2 = (a1*a1 + a2*a2 - r*r)/(2*a1*a2);
sin_phi1 = sqrt(1 - cos_phi1*cos_phi1);
sin_phi2 = sqrt(1 - cos_phi2*cos_phi2);
phi1 = atan2d(sin_phi1, cos_phi1); % deg
phi2 = atan2d(sin_phi2, cos_phi2); % deg

Computation for $\beta$

beta = atan2d(py, px); % deg

The Elbow-Left Solution

theta1L = beta + phi1 % deg
theta1 = theta1L; % deg
theta2L = atan2d(py - a1*sind(theta1), px - a1*cosd(theta1)) - theta1 % deg
theta1L =

   21.2623


theta2L =

  -65.3669

The Elbow-Right Solution

theta1R = beta - phi1 % deg
theta1 = theta1R; % deg
theta2R = atan2d(py - a1*sind(theta1), px - a1*cosd(theta1)) - theta1 % deg
theta1R =

  -44.1047


theta2R =

   65.3669

This MATLAB example illustrates a computation from the textbook Fundamentals of Robot Mechanics by G. L. Long, Quintus-Hyperion Press, 2015. See http://www.RobotMechanicsControl.info for additional relevant files.