\documentclass[12pt]{article}
\usepackage{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.0in}
\setlength{\topmargin}{-0.51in}
\setlength{\oddsidemargin}{0.0in}
\parskip 2ex
\parindent=0pt
%%%----------------------------------------------------------------------%%%
\begin{document}

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

{\bf Problem 1:}  

{\bf Part (a):} Elements of $SU(2)$ have the form:

\[ \begin{bmatrix} {\bf z} & {\bf w} \\ -{\bf w}^* & {\bf z}^* \end{bmatrix}
  = 
 \begin{bmatrix} (a + ib) & (c + id) \\ -(c - id) & (a - ib) \end{bmatrix} \]
where $zz^{*} + ww^{*} = a^2 + b^2 + c^2 + d^2 = 1$.  Thus, the scalar
elements $a$, $b$, $c$, and $d$ are in one-to-one correspondence with
the scalar elements of unit quaternions.  That is, let a quaternion
be represented by $q=\lambda_1 + \lambda_2 i + \lambda_3 j + \lambda_4 k =
(\lambda_1, \lambda_2, \lambda_3, \lambda_4)$.  The correspondence is then:

  \begin{eqnarray}
     \lambda_1 &=& a = Re(z) = \frac{z + z^{*}}{2} \\
     \lambda_2 &=& b = Im(z) = \frac{i(z^{*} - z)}{2} \\
     \lambda_3 &=& c = Re(w) = \frac{w + w^{*}}{2} \\ 
     \lambda_4 &=& d = Im(w) = \frac{i(w^{*} - w)}{2} 
  \end{eqnarray}

\def\halfphi{\frac{\phi}{2}}
\def\halfgamma{\frac{\gamma}{2}}
\def\halfpsi{\frac{\psi}{2}}

The unit quaternion elements are in one-to-one correspondence with the Euler
parameters of a rotation: $(\lambda_1,\lambda_2, \lambda_3, \lambda_4) = 
(\cos\halfphi, s_x\sin\halfphi, s_y\sin\halfphi, s_z\sin\halfphi)$.  $\phi$ is
the rotation about an axis represented by a unit vector $\vec{\omega} =
[\omega_x\ \omega_y\  \omega_z]^T$.  
\vskip 0.2 true in
{\bf Part (b):}
A $2\times 2$ complex matrix which 
represents an arbitrary rotation 
as a function of the z-y-x Euler angles can be developed as the product of
$2\times 2$ complex matrices which represent rotations about the z, y, and x
axes.  A rotation about the x-axis of amount $\gamma$ has the $2\times 2$ 
representation (since $\lambda_1 =\cos\halfgamma$, $\lambda_2 = \sin
\halfgamma$, $\lambda_3=\lambda_4=0$):
  \[ \begin{bmatrix} (\cos\halfgamma + i\sin\halfgamma) & 0\\ 
                   0 & (\cos\halfgamma - i\sin\halfgamma) \end{bmatrix} = 
    \begin{bmatrix}e^{i\halfgamma} & 0 \\ 0 & e^{-i\halfgamma}\end{bmatrix} \]
Similarly, a rotation of amount $\phi$ about the y-axis can be represented as:
  \[ \begin{bmatrix}\cos\halfphi & \sin\halfphi \\ 
              -\sin\halfphi & \cos\halfphi \end{bmatrix} \]
while a rotation of amount $\psi$ about the z-axis can be represented as:
  \[ \begin{bmatrix}\cos\halfpsi & i\sin\halfpsi \\ 
		  i\sin\halfpsi & \cos\halfpsi \end{bmatrix} \]

\vskip 0.2 true in
{\bf Part (c):} 
\def\denom{ \sqrt{b^2 + c^2 + d^2} }
\def\cdenom{ \sqrt{ (\frac{{\bf z} -{\bf z}^*}{2})^2 + {\bf w}{\bf w}^*} }

  \begin{eqnarray}
  \phi &=& 2 \cos^{-1} (a) = 2\cos^{-1}(\frac{{\bf z} + {\bf z}^*}{2}) \\ 
  \omega_x &=& \frac{b}{\denom} = \frac{({\bf z} - {\bf z}^*)/2}{\cdenom}\\
  \omega_y &=& \frac{c}{\denom} = \frac{({\bf w} + {\bf w}^*)/2}{\cdenom}\\
  \omega_z &=& \frac{d}{\denom} = \frac{({\bf w} - {\bf w}^*)/2}{\cdenom}
  \end{eqnarray}



%%%-----------------------------------------------------------------------%%%
\vskip 0.2 true in
{\bf Problem 2:}
Start with Rodriguez' formula for a rotation matrix:
$$e^{\hat{\omega} \theta} = I + \hat\omega sin \theta + \hat{\omega}^2 (1 - cos \theta)$$
and:
\begin{eqnarray}
\lambda_1 = cos \frac{\phi}{2}\\
  \lambda_2= \omega_1 sin \frac{\phi}{2}\\  
  \lambda_3= \omega_2 sin \frac{\phi}{2}\\  
  \lambda_4= \omega_3 sin \frac{\phi}{2} \\
\end{eqnarray}

From Rodriguez' fomula we find that 
 \[   R= \begin{bmatrix} ({\omega_1}^2 (1 - cos \phi) + cos \phi) & \omega_1 \omega_2 (1 - cos \phi) - \omega_3 sin \phi & \omega_1 \omega_3 (1 - cos \phi)  + \omega_2 sin \phi\\
    \omega_1 \omega_2 (1- cos \phi) + \omega_3 sin \phi & {\omega_2}^2 (1 - cos \phi) + cos \phi & \omega_2 \omega_3 (1 - cos \phi) - \omega_1 sin \phi \\
    \omega_1 \omega_3 (1 - cos \phi) - \omega_2 sin \phi & \omega_2 \omega_3 (1 - cos \phi) + \omega_1 sin \phi & {\omega_3}^2 (1 - cos \phi) + cos \phi
         \end{bmatrix} \]

To simplify this, we need to use some trig identities: \\
\[ sin(2x) = 2 sin(x) cos(x) \]
\[   cos (2x) =  1 - 2 sin^2 (x) 
   \]
   
  By rearanging our $\lambda$ equations, we get:
   \begin{eqnarray}
\lambda_1 = cos \frac{\phi}{2}\\
\omega_1 = \frac{\lambda_2}{sin {\frac {\phi}{2}}}\\  
\omega_2 = \frac{\lambda_3}{sin {\frac {\phi}{2}}}\\  
\omega_3 = \frac{\lambda_4}{sin {\frac {\phi}{2}}}\\  
\end{eqnarray}

Using this knowledge, our original R matrix simplifies to:
\[    R= \begin{bmatrix} (\lambda_1^2+\lambda_2^2-\lambda_3^2-\lambda_4^2) & 2(\lambda_2\lambda_3-\lambda_1\lambda_4) & 2(\lambda_2\lambda_4+\lambda_1\lambda_3) \\
                          2(\lambda_2\lambda_3+\lambda_1\lambda_4) & (\lambda_1^2-\lambda_2^2+\lambda_3^2-\lambda_4^2) & 2(\lambda_3\lambda_4-\lambda_1\lambda_2) \\
			  2(\lambda_2\lambda_4-\lambda_1\lambda_3) & 2(\lambda_3d+\lambda_1\lambda_2) & (\lambda_1^2-\lambda_2^2-\lambda_3^2+\lambda_4^2) 
         \end{bmatrix} \]

%%%-----------------------------------------------------------------------%%%
\vskip 0.2 true in
{\bf Problem 3:}



\[    R= \begin{bmatrix} (\lambda_1^2+\lambda_2^2-\lambda_3^2-\lambda_4^2) & 2(\lambda_2\lambda_3-\lambda_1\lambda_4) & 2(\lambda_2\lambda_4+\lambda_1\lambda_3) \\
                          2(\lambda_2\lambda_3+\lambda_1\lambda_4) & (\lambda_1^2-\lambda_2^2+\lambda_3^2-\lambda_4^2) & 2(\lambda_3\lambda_4-\lambda_1\lambda_2) \\
			  2(\lambda_2\lambda_4-\lambda_1\lambda_3) & 2(\lambda_3d+\lambda_1\lambda_2) & (\lambda_1^2-\lambda_2^2-\lambda_3^2+\lambda_4^2) 
         \end{bmatrix} \]

To extract the quaternion parameters from the rotation matrix, note that:
  \[r_{11} + r_{22} + r_{33} = 3\lambda_1^2 -\lambda_2^2-\lambda_3^2-\lambda_4^2 \]
Using the fact that $\lambda_1^2+\lambda_2^2+\lambda_3^2+\lambda_4^2=1$, then
  \[r_{11} + r_{22} + r_{33} = 4\lambda_1^2-1 \ \rightarrow\ \lambda_1^2 = 
                \frac{r_{11} + r_{22} + r_{33} +1}{4} \]
Assuming that $\lambda_1\ne 0$, then
   \[ r_{21}-r_{12} = 2(\lambda_2\lambda_3+\lambda_1\lambda_4)-2(\lambda_2\lambda_3-\lambda_1\lambda_4) = 4 \lambda_1\lambda_4 \ \Rightarrow\ 
   \lambda_4 = \frac{r_{21}-r_{12}}{4\lambda_1} \]
Similary, one can show that 
  \[ \lambda_3=\frac{r_{13}-r_{31}}{4\lambda_1} \ \ \ \ 
     \lambda_2=\frac{r_{32}-r_{23}}{4\lambda_1} \]
If $\lambda_1=0$ (or practically, if $\lambda_2$ is very small), then one can choose
a different factorization.




%%%----------------------------------------------------------------------%%%
\vskip 0.2 true in
{\bf Problem 4:(Problem 6(a,b,d) in Chapter 2 of MLS). }\\
\begin{itemize}
\item{}{\bf Part (a):} Let $Q$ and $P$ be unit quaternions---i.e., 
$QQ^{*}= PP^{*}=1$.  The set of unit quaternions is a
group if you can show that: (i) multiplication is commutative; (ii) the product
of group elements yields a group element; (iii) the set contains an
identity element; (iv) every group element has an inverse element, and
the inverse is in the group.

\begin{description}
\item{(i)} It is easy to show that quaternion multiplication is commutative.
\item{(ii)} The product of unit quaternions, $QP$, is a unit
quaternion: $QP(QP)^{*} = QPP^{*}Q{*} = QQ^{*} =1$.
\item{(iii)} The identity quaternion is: $e=(1,0,0,0) = 1 + 0\cdot i + 0\cdot j
+ 0\cdot k$.
\item{(iv)} The inverse of any unit quaternion $Q$ is $Q^{*}$, which is 
also a unit quaternion (since $Q^{*}(Q^{*})^{*} = Q^{*}Q = (QQ^{*})^{*} = 1^{*}
=1$).
\end{description}

\def\qr{q_R}
\def\qp{q_P}

\item{}{\bf Part (b):}
If a unit quaternion, $q$, has real part $\qr$ and pure part $\qp$,
and $\vec{x}=[x_1 \ x_2\ x_3]^T$ is represented as a pure quaternion
$\tilde{x}=(0, x_1, x_2, x_3)= 0 + \vec{x}$, then:
  \begin{eqnarray*}
  \tilde{x}q^{-1} &=&
    \vec{x}\cdot \qp \tab(\leftarrow{\rm real\ part}) \\ 
    &+& \qr\vec{x} -(\vec{x}\times\qp \tab (\leftarrow {\rm pure\ part})
  \end{eqnarray*}
Similarly, the product $q\tilde{x}q^{-1}$ is:
  \begin{eqnarray*}
    q \tilde{x}q^{-1} &=&
     \qr (\vec{x}\cdot \qp) - \qp\cdot(\qr \vec{x} - \vec{x}\times\qp)
	 \tab(\leftarrow{\rm real\ part}) \\ 
    &+& \qr(\qr\vec{x} -\vec{x}\times\qp) + (\vec{x}\cdot\qp)\qp +
        \qp\times(\qr\vec{x}-\vec{x}\times\qp)
	 \tab (\leftarrow {\rm pure\ part})
  \end{eqnarray*}
The real part of $q\tilde{x}q^{-1}$ is:
 \[ (\vec{x}\cdot\qp) \qr - \qp \cdot[\qr \vec{x} - (\vec{x}\times \qp)] 
 =\qr(\vec{x}\cdot \qp) - \qr(\vec{x}\cdot \qp)+\qp\cdot(\vec{x}\times\qp)
 = 0 \]
Thus $q\tilde{x} q^{-1}$ is a pure quaternion when $\tilde{x}$ is.  

The vector part of $q\tilde{x}q^{-1}$ is:
  \begin{eqnarray*}
    \qr(\qr\vec{x} -\vec{x}\times\qp) &+& (\vec{x}\cdot\qp)\qp +
        \qp\times(\qr\vec{x}-\vec{x}\times\qp) \\ 
    &=& {\qr}^2\vec{x} - {\qr}(\vec{x}\times\qp) + (\vec{x}\cdot\qp) \qp
      + {\qr}(\qp\times\vec{x}) - {\qp}\times(\vec{x}\times{\qp}) \\ 
    &=& \qr^2\vec{x} -2\qr(\vec{x}\times\qp) + (\vec{x}\cdot\qp)\qp -
	[(\qp\cdot\qp)\vec{x} - (\vec{x}\cdot\qp)\qp]\\ 
    &=& [\qr^2 - (\qp\cdot\qp)]\vec{x} + 2[(\vec{x}\cdot\qp)\qp 
       + \qr(\qp\times\vec{x})] \end{eqnarray*}
where we have used the triple cross product identity: $\vec{a}\times (\vec{b}
\times \vec{c})=(\vec{a}\cdot\vec{c})\vec{b} - (\vec{a}\cdot\vec{b})\vec{c}$

To show that this is the same as applying a rotation to point x: \\
Let Q =  (cos$\frac\theta{2}$, $\omega$ sin $\frac{\omega}{2}$), where $||\omega||$ = 1.  
Then the vector part of  QXQ$^{*}$ becomes 
\begin{eqnarray*}
&=&({cos}^2\frac{\theta}{2} - {sin}^2\frac{\theta}{2})x + (2 \hat\omega cos\frac{\theta}{2} sin\frac{\theta}{2})x +2(\omega\omega^T {sin}^2\frac{\omega}{2}) x \\
 &=& (cos \theta I + \hat{\omega} sin \theta + \hat{\omega}^2 ( 1 - cos\theta)x\\
 &=& (exp \hat\omega \theta) x = R x \end{eqnarray*}
 \item{}{\bf Part (d):} 
\begin{description}
\item{(i)} If $A_1,A_2\in SO(3)$, then each of the 9 elements in the product
matrix $A_1 \ A_2$ requires 3 multiplications and 2 additions. Hence,
the product $A_1 \ A_2$ requires a total of 27 multiplications and 18
additions.

\item{(ii)} Let $q_1$ and $q_2$ be quaternions, with respective real and
vector parts $q_{1R}$, $q_{2R}$ and $\vec{q}_{1P}$,
$\vec{q}_{2P}$. The real part of the quaternion product, $q_{1R}
q_{2R} - \vec{q}_{1P} \cdot \vec{q}_{2P}$, requires 4 multiplications
and 3 additions (where the subtraction is counted as an addition).
The pure part, $\vec{q}_{3P}= q_{1R} \vec{q}_{2P} + q_{2R}
\vec{q}_{1P} + \vec{q}_{1P}\times \vec{q}_{2P}$, can be evaluated
in 12 multiplications and 9 additions.  Thus, the quaternion product 
requires a total of 16 multiplications and 12 additions.  It is therefore
more efficient than the equivalent matrix multiplication.

\item{(iii)} The rotation of a vector by multiplication of a $3\times 3$
rotation matrix times a $3\times 1$ vector requires only 9 multiplications
and 6 additions.

\item{(iv)} The number of multiplications and additions for the equivalent 
quaternion operation will depend upon the form which one uses for the 
quaternion vector rotation. Using the identity $1 = \qr^2 + \qp\cdot\qp$, it 
is possible to show that the vector part of $q\tilde{x}q^{-1}$ in part (b) 
above can be rearranged to the form:
  \[ \vec{x} + 2[\qp\times(\qp\times\vec{x}) + \qr(\qp\times\vec{x})] \]
Since $\qp\times\vec{x}$ need only be evaluated once, this takes only 18
multiplications and 12 additions.  However, no matter what form one tries, the
quaternion approach will always take more operations than the matrix/vector
approach for vector rotation.
\end{description}

\end{itemize}
%%%----------------------------------------------------------------------%%%
\vskip 0.2 true in
{\bf Problem 5:}  (Problem 11(a,b) in Chapter 2 of MLS). \\
\begin{itemize}
\item{}{\bf Part (a):} Recall that the matrix exponential of a twist, $\hat{\xi}$, is:
   $$ e^{\phi\hat{\xi}} = I + {{\phi}\over{1!}} \hat{\xi} 
          + {{\phi^2}\over{2!}}\hat{\xi}^2 + 
	              {{\phi^3}\over{3!}}\hat{\xi}^3 + \cdots $$

First, let's consider the case of $\xi=(v,\omega)$, with $\omega=0$. If:
  \[\hat{\xi} = \begin{bmatrix}0 & 0 & v_x \\
			      0 & 0 & v_y \\
			      0 & 0 & 0 \end{bmatrix} \]
then $\hat{\xi}^2=0$.  Thus
 \[e^{\phi\hat{\xi}} = \begin{bmatrix}1 & 0 & \phi v_x \\
			      0 & 1 & \phi v_y \\
			      0 & 0 & 1 \end{bmatrix} = 
	\begin{bmatrix} I & \vec{v}\phi\\ \vec{0}^t & 1\end{bmatrix} \]
To compute the exponential for the more general case in which $\omega\ne 0$,
let us assume that $||\omega||=1$.  In this case, note that $\hat{\omega}^2 =
- I$, where $I$ is the $2\times 2$ identity matrix.  It is easiest if we
choose a different coordinate system in which to perform the calculations.
Let
 \[ \hat{\xi}=\begin{bmatrix}0 & -\omega & v_x \\
			    \omega & 0 & v_y \\
			    0 & 0 & 0 \end{bmatrix} = 
	      \begin{bmatrix} \hat{\omega} & \vec{v} \\
			         \vec{0}^T & 0\end{bmatrix}  \]
Let
 \[ g = \begin{bmatrix}I & \hat{\omega}\vec{v} \\ \vec{0}^T & 1\end{bmatrix} \]
Let is define a new twist, $\hat{\xi}^{'}$:
  \[ \begin{array}{rl}
     \hat{\xi}^{'} &= g^{-1}\hat{\xi} g \\ 
	   &= \begin{bmatrix}I & -\hat{\omega}\vec{v} \\ 0 & 1\end{bmatrix}
	      \begin{bmatrix}\hat{\omega} & \vec{v} \\ 0 & 0\end{bmatrix}
      	      \begin{bmatrix}I & \hat{\omega}\vec{v} \\ 0 & 1\end{bmatrix} \\ 
          & = \begin{bmatrix}\hat{\omega} & (\hat{\omega}^2\vec{v} + \vec{v})\\
	                      0 & 0\end{bmatrix}
            = \begin{bmatrix}\hat{\omega} & 0 \cr 0 & 0\end{bmatrix}\end{array} \]
where we made use of the identity $\hat{\omega}^2 =-I$. That is, we have
chosen a coordinate system in which $\hat{\xi}^{'}$ corresponds to a pure
rotation.  Thus, 
  $$e^{\phi\hat{\xi}^{'}} = \begin{bmatrix}e^{\phi\hat{\omega}} & 0\\
			0 & 1\end{bmatrix}.$$
Using Eq. (2.35) on page 42 of the MLS text:
  $$e^{\phi\hat{\xi}} = ge^{\phi\hat{\xi}^{'}}g^{-1} = 
    \begin{bmatrix}e^{\phi\hat{\omega}} & 
                 (I-e^{\phi\hat{\omega}})\hat{\omega}\vec{v}\phi\\
	           0 & 1 \end{bmatrix}$$
which is clearly an element of $SE(2)$.

\item{}{\bf Part(b):} It is easy to see from part (a) that the twist
$\xi=(v_x,v_y,0)^T$ maps directly to the planar translation $(v_x,v_y)$.  

The twist corresponding to pure rotation about a point $\vec{q}=(q_x,q_y)$ can
be thought of as the Ad-transformation of a twist, $\xi^{'}=(0,0,\omega)$,
which is pure rotation, by a transformation, $g$, which is pure translation by
$\vec{q}$:
  \begin{equation}\label{eq:foo}
     \xi = {\rm Ad}_{h}\xi^{'} = (h \hat{\xi}^{'} h^{-1})^{\vee}
  \end{equation}
where 
  $$h=\begin{bmatrix}I & \vec{q}\cr 0 & 1\end{bmatrix} \tab
  {\rm and} \tab \hat{xi}^{'} = \begin{bmatrix}\hat{\omega} & 0\\
	\vec{0}^T & 0\end{bmatrix}.$$  
Expanding Eq. (\ref{eq:foo}) gives:
  \[ \xi = (h \hat{\xi}^{'} h^{-1})^{\vee} = 
     \begin{bmatrix}\hat{\omega} & -\hat{\omega}\vec{q}\cr \vec{0}^T & 0
        \end{bmatrix}^{\vee} = \begin{bmatrix}q_y\cr -q_x\cr 1\end{bmatrix} \]
assuming $\omega = 1$.
\end{itemize}
%%%----------------------------------------------------------------------%%%
\end{document}

