 \documentclass[12pt]{article}
\usepackage{epsfig,amsmath}
%%%--------- Set up page size and formatting information ----------%%%
\def\newpage{\par\vfill\eject}	%new page macro
\def\tab{\ \ \ \ \ }		%tab in five spaces
\def\dtab{{\tab}{\tab}}         %double tab

\setlength{\textwidth}{6.5in}
\setlength{\textheight}{9.2in}
\setlength{\topmargin}{-0.31in}
\setlength{\oddsidemargin}{0.0in}
\parskip 2ex
\parindent=0pt
\def\tab{\ \ \ \ \ }
%%%----------------------------------------------------------------------%%%
\begin{document}
\input{reals.tex}

\centerline{\bf ME 115(a):  Solution to Homework $\#6$}
\vskip 0.5 true in

%%%----------------------------------------------------------------------%%%
{\bf Problem $\# 1$:} 

{\bf Part (a):}  Using the Denavit-Hartenberg approach,
the forward kinematics of this manipulator are:
  \[ g_{ST} = g_{S,1} g_{1,2} g_{2,3} g_{3,T}\]
where the matrices $g_{i,i+1}$ define the transformations between
adjacent link frames.  To simplify things, we choose the tool frame to
be parallel with the link frame of the third link, but displaced a distance
$d_4$ along the link frame $z$-axis.  In this case:
   \[ \begin{array}{rl}
      g_{ST}
           &=\begin{bmatrix}\cos\theta_1 & -\sin\theta_1 & 0 & 0\\
		            \sin\theta_1 &  \cos\theta_1 & 0 & 0\\
		            0 & 0 & 1 & 0\\
                            0 & 0 & 0 & 1\end{bmatrix}
             \begin{bmatrix}\cos\theta_2 & -\sin\theta_2 & 0 & 0\\
	 		     0 & 0 & 1 & 0 \\
		           -\sin\theta_2 & -\cos\theta_2 & 0 & 0\\\
                           0 & 0 & 0 &    1 \end{bmatrix}
             \begin{bmatrix}1 & 0 & 0 &    0 \\
	 		    0 & 0 &-1 & -d_3 \\
		            0 & 1 & 0 &    0\\
                            0 & 0 & 0 & 1\end{bmatrix} 
             \begin{bmatrix}1 & 0 & 0 &    0 \\
	 		    0 & 1 & 0 &    0 \\
		            0 & 0 & 1 &    d_4\\
                            0 & 0 & 0 & 1\end{bmatrix} \\\\
      &=\begin{bmatrix} c_1 c_2 & -s_1 & c_1 s_2 & (d_3+d_4) c_1 s_2 \\
		      s_1 c_2 &  c_1 & s_1 s_2 & (d_3+d_4) s_1 s_2 \\
		         -s_2 &    0 &     c_2 & (d_3+d_4) c_2 \\
			0  & 0 & 0 & 1\end{bmatrix} \ =\ 
	\begin{bmatrix} R_{ST} & \vec{p}_{ST} \\
			\vec{0}^T & 1\end{bmatrix}
	\end{array} \]
     
{\bf Inverse Kinematics:} Since $d_3$ is variable, this manipulator is capable
of reaching any desired point $\vec{p}_D=[x_D\ y_D\ z_D]^T$ within the
workspace dictated by the mechanical limits of the joints.  Let $\vec{p}_{ST}$
denote the portion of the forward kineatics that describes the position of the
tool frame origin with respect to the stationary frame origin.  Hence, we can
find the inverse kinematics by equating terms of $\vec{p}_{ST}$ with 
$\vec{p}_D$.  First notice that:
	\[ ||\vec{p}_{ST}||^2 = (d_3+d_4)^2 = (x_D^2 + y_D^2 + z_D^2) \]
Hence, 
  \begin{equation}\label{eq:dsoln}
    d_3 = -d_4 \pm (x_D^2 + y_D^2 + z_D^2)^{1/2} \end{equation} We
will denote the two possible solutions by $d_3^{+}$ and $d_3^{-}$.
Next notice that $(d_3+d_4) \cos\theta_2 = z_D$ and
$(d_3+d_4)^2\sin^2\theta_2 = x_D^2 + y_D^2$.  Hence there are two
solutions in $\theta_2$ for a given choice of $d_3$:
   \[ \begin{array}{rl} \label{eq:t2soln}
     \theta_2^a &= \cos^{-1}[z_D/(d_3+d_4)] \\ 
     \theta_2^b &= -\theta_2^a \end{array} \]
However, there at not four possible solutions, since two of the solutions are
the same.  Hence, there are two combinations of $\theta_2$ and $d_3$.  
Finally, we can determine the value of $\theta_1$ from the $x$ and $y$ 
components of $\vec{p}_{ST}$:
    \[ \begin{array}{rl} \label{eq:t1soln}
       \cos\theta_1 &= { {x_D}\over{(d_3+d_4)\sin\theta_2}} \\
       \sin\theta_1 &= { {y_D}\over{(d_3+ d_4)\sin\theta_2}} \\
       \theta_1 &= Atan2[\cos\theta_1,\sin\theta_1] \end{array} \]
There will be one $\theta_1$ solution for each of the two $(\theta_2,d_3)$
pairs.\\
\bigskip
{\bf Part (b):}

First, find the D-H parameters.
\[ 
   \begin{array}{llll}
    a_0 = 0 & \alpha_0=0              & d_1 = 0 & \theta_1=\ \rm{variable} \\
    a_1 = 0 & \alpha_1=-\frac{\pi}{2} & d_2 = 0 & \theta_2=\ \rm{variable} \\
    a_2 = 0 & \alpha_2= \frac{\pi}{2} & d_3 = \theta_3 & \theta_3= 0 \\
    a_3 = 0 & \alpha_3= \frac{\pi}{2}    & d_4 = 0 & \theta_4=\ \rm{variable} \\
    a_4 = 0 & \alpha_4= \frac{\pi}{2} & d_5 = 0 & \theta_5= \ \rm{variable} \\
    a_5 = 0 & \alpha_5= \frac{\pi}{2} & d_6 =l_6 & \theta_5= \ \rm{variable} 
   \end{array}
\]     
   
   To find the forward kinematics using these parameters, one must use the formula
   
\begin{equation}
g_{ST}(\theta) = g_{ST}(\theta_1) ... g_{56}(\theta_6) g_{6T}
\end{equation}
   
   where each g is given by:
   \[ g_{i,i+1}=\begin{bmatrix} 
   cos\theta_{i+1} & -sin\theta_{i+1} & 0 & \alpha_i \\
   sin\theta_{i+1}cos\alpha_{i} & cos\theta_{i+1}cos\alpha_i & -sin\alpha_i & -d_{i+1}sin\alpha_i\\
   sin\theta_{i+1}sin\alpha_{i} &  cos\theta_{i+1}sin\alpha_i  & cos\alpha_i & d_{i+1}cos\alpha_i\\
    0 & 0& 0& 1
    \end{bmatrix} \]
Thus,
   \[ g_{ST}=\begin{bmatrix} 
cos\theta_1 & -sin\theta_1 & 0 & 0\\ 
sin\theta_1  & cos\theta_1 & 0 & 0 \\ 
0 & 0 & 1 & l_1\\
0 & 0 & 0& 1 \end{bmatrix} 
\begin{bmatrix} cos\theta_2 & -sin\theta_2 & 0 & 0\\ 0 & 0 & 1 & 0\\ -sin\theta_2 & -cos\theta_2 & 0 & 0\\ 0 & 0& 0& 1\end{bmatrix} 
\begin{bmatrix} 1& 0 & 0& 0\\ 0 & 0 & -1 & \theta_3\\ 0 & 1 & 0 & 0\\ 0 & 0& 0& 1\end{bmatrix} 
\begin{bmatrix} cos\theta_4 & -sin\theta_4 & 0 & 0\\ 0 & 0 & -1 & 0 \\ sin\theta_4 & cos\theta_4 & 0 & 0\\
0 & 0& 0& 1   \end{bmatrix} \]
\[ \begin{bmatrix} cos\theta_5 & -sin\theta_5 & 0 & 0\\ 0 & 0 & -1 & 0\\ sin\theta_5 & cos\theta_5 & 0 & 0\\ 0 & 0 & 0 & 1\end{bmatrix}
\begin{bmatrix}cos\theta_6 & -sin\theta_6 & 0 & 0\\ 0 & 0 & -1 & -l_6\\ sin\theta_6 & cos\theta_6 & 0 & 0\\ 0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix} \end{bmatrix} \]
   	
$\theta_1$, $\theta_2$, and $\theta_3$ will be the same as in part (a).
We can also easily calculate $R_{S3}$ (which we got in part (a).)

\[R_{S3}=R_{S1}R_{12}R_{23}=\begin{bmatrix}cos\theta_1cos\theta_2 & -sin\theta_1 &cos\theta_1sin\theta_2\\
sin\theta_1cos\theta_2 & cos\theta_1 & sin\theta_1sin\theta_2 \\
-sin\theta_2 & 0 &cos\theta_2\end{bmatrix} \]

Similarly,
\small{
\begin{equation}\label{r3t}
R_{3T}=R_{34}R_{45}R_{56}=\begin{bmatrix}
cos\theta_4cos\theta_5cos\theta_6+sin\theta_4sin\theta_6 & -cos\theta_4cos\theta_5sin\theta_6 + sin\theta_4cos\theta_6 & cos\theta_4sin\theta_5\\
-sin\theta_5cos\theta_6 & sin\theta_5sin\theta_6 & cos\theta_5 \\
sin\theta_4cos\theta_5cos\theta_6-cos\theta_4sin\theta_6 & -sin\theta_4cos\theta_5sin\theta_6-cos\theta_4cos\theta_6 & sin\theta_4sin\theta_5  \end{bmatrix} 
\end{equation}}
We're interested in the inverse kinematics, so we want to know what combinations of angles will give $R_D$ (R desired) and $d_D$ (d desired). The inverse kinematics for position depend only on 
$\theta_1$, $\theta_2$ and $\theta_3$ and thus are the same as in part (a). (This is because the "regional" part of this manipulator is exactly the same as in part (a).)

   \[ g_{S6}=\begin{bmatrix} R_{S6} & d_{S6} \\  0 & 1  \end{bmatrix}
      =\begin{bmatrix} R_D & d_D \\ 0& 1\end{bmatrix} \]



We know that
$R_{S6}$=$R_{S1}$ $R_{12}$ $R_{23}$ $R_{34}$ $R_{45}$ $R_{56}$
and thus $R_{S6}$ = $R_{S3}$ $R_{46}$.

$R_{S3}$ is invertible,so  $R_{S3}^{-1}$ =$ R_{S3}^T$

\begin{equation}
R_{46}={R_{13}^T}R_D.
\end{equation}

   \[R_D=\begin{bmatrix} r_{11}  & r_{12} &  r_{13}\\ r_{21} & r_{22} & r_{23}\\  r_{31} & r_{32} & r_{33} \end{bmatrix} \]

\[ R_{46}={\begin{bmatrix} cos\theta_1cos\theta_2 & -sin\theta_1 & cos\theta_1sin\theta_2\\ sin\theta_1cos\theta_2 & cos\theta_1 &sin\theta_1sin\theta_2\\-l_1 sin\theta_2 & 0 & l_1 cos\theta_2 \end{bmatrix}}^{-1} \begin{bmatrix}D\end{bmatrix} \]

\footnotesize{\[= \begin{bmatrix} r_{11}cos\theta_1cos\theta_2-sin\theta_1r_{12}+r_{13}cos\theta_1sin\theta_2 & r_{11}sin\theta_1cos\theta_2+r_{12}cos\theta_1+r_{13}sin\theta_1sin\theta_2 & r_{13}l_1cos\theta_2 - r_{11}l_1sin\theta_2\\
r_{21}cos\theta_1cos\theta_2-r_{22}sin\theta_1+r_{23}cos\theta_1sin\theta_2 & r_{21}sin\theta_1cos\theta_2+cos\theta_1r_{22}+r_{23}sin\theta_1sin\theta_2 & r_{23}l_1cos\theta_2-r_{21}l_1sin\theta_2\\
-r_{32}cos\theta_1cos\theta_2-r_{32}sin\theta_1+r_{33}cos\theta_1sin\theta_2 & r_{31}sin\theta_1cos\theta_2 + r_{32}cos\theta_1 + r_{33}sin\theta_1sin\theta_2 & r_{33}l_1cos\theta_2-r_{31}l_1sin\theta_2 \end{bmatrix}
\]}
Setting this $R_{46}$ to the one in Equation 3 will give us expressions for $\theta_4$, $\theta_5$, and 
$\theta_6$.


%%%----------------------------------------------------------------------%%%
\vskip 0.25 true in
{\bf Problem 2:} 
The hybrid Jacobian can be computed by first calculating the forward kinematics.

   \[ g_{ST}=\begin{bmatrix} R(\theta) & p(\theta) \\  0 & 1  \end{bmatrix}
      =\begin{bmatrix}cos\theta_1 cos (\theta_2 + \theta_3) & -sin\theta_1 & cos\theta_1 sin(\theta_2 + \theta_3) & cos\theta_1 [l_2 cos\theta_2 + l_3 cos(\theta_2 + \theta_3)]\\
sin\theta_1 cos (\theta_2 + \theta_3) & cos\theta_1 &sin\theta_1 sin(\theta_2 + \theta_3) & sin\theta_1 [l_2 cos\theta_2 + l_3 cos(\theta_2 + \theta_3)]\\
-sin(\theta_2 + \theta_3) & 0 & cos(\theta_2 + \theta_3) & -l_{2}sin\theta_2 - l_{3}sin(\theta_2 + \theta_3)\\
0 & 0 & 0 & 1 \end{bmatrix} \]

The Hybrid Jacobian is defined as \[J^{H} = \begin{bmatrix} \frac{dp}{d\theta} \\ {(\frac{dR}{d\theta}R^(-1))^V} \end{bmatrix} \] 

   \[ J^{H}=\begin{bmatrix}
    -sin\theta_1 [l_2 cos\theta_2 + l_3 cos(\theta_2 + \theta_3)] &     -cos\theta_1 [l_2 sin\theta_2 + l_3 sin(\theta_2 + \theta_3)] & -l_3 cos\theta_1 sin(\theta_2 + \theta_3)\\
      cos\theta_1 [l_2 cos\theta_2 + l_3 cos(\theta_2 + \theta_3)] &     -sin\theta_1 [l_2 sin\theta_2 + l_3 sin(\theta_2 + \theta_3)] & -l_3 sin\theta_1 sin(\theta_2 + \theta_3)\\
      0 & -l_2 cos\theta_2 - l_3 cos(\theta_2 + \theta_3) & -l_3 cos(\theta_2 + \theta_3)\\
      0 & -sin\theta_1 & -sin\theta_1 \\
      0 & cos\theta_1 & cos\theta_1 \\
      1 & 0 & 0  \end{bmatrix} \]

There exist singularities when the determinant of J equals zero.  
\begin{equation}
0 = det[{J^H}^T J^H] = -l_2 l_3 (l_2 cos\theta_2 + l_3 cos(\theta_2 + \theta_3)) sin\theta_3
\end{equation}

This corresponds to two cases.  The first is when $\theta_3 = 0$  and $\theta_2$ is anything.  The other case is when  $\theta_2 = \frac{\pi}{2}$ and$\theta_3$=$\pi$.

    
\end{document}