\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
\def\tab{\ \ \ \ \ }
%%%----------------------------------------------------------------------%%%
\begin{document}
\input{/home/jwb/tex/reals.tex}

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

%%%----------------------------------------------------------------------%%%
{\bf Problem $\# 1$:} (Problem 5(c) in Chapter 2 of MLS). 

There are two ways to solve this problem.  The simplest way is to use
the result of part 5(b) quoted in the text:

\begin{equation}\label{eq:cayley}
   R= \frac{1}{1+||a||^2}\begin{bmatrix} 
     1 + a_1^2-a_2^2-a_3^2 & 2(a_1 a_2 - a_3)      & 2(a_1 a_3 + a_2)\\
        2(a_1 a_2 + a_3)   & 1 - a_1^2+a_2^2-a_3^2 & 2(a_2 a_3 - a_1)\\
        2(a_1 a_3 - a_2)   & 2(a_2 a_3 + a_1)      & 1 - a_1^2-a_2^2+a_3^2
    \end{bmatrix} \end{equation}
where $||a||^2$ is shorthand notation for $||a||^2=a_1^2+a_2^2+a_3^2$.
Noting that 
   \[ trace(R)= \frac{3-||a||^2}{1+||a||^2} \Rightarrow 
     ||a||^2 = \frac{3-trace(R)}{1+trace(R)} 
             = \frac{3-r_{11}-r_{22}-r_{33}}{1+r_{11}+r_{22}+r_{33}} \]
so that an expression for $||a||^2$ is known, 
simple algebraic manipulation of the off-diagonal term of $R$ yield
  \[ \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix}
     = \frac{1+||a||^2}{4} \begin{bmatrix} r_{32}-r_{23} \\ r_{13}-r_{31} \\
       r_{21} - r_{12} \end{bmatrix} \]
     
If you didn't use the results of 5(b) in the text, then you would have
started with Cayley's formula $R=(I-\hat{a})^{-1}(I+\hat{a})$ and derived 
Equation (\ref{eq:cayley}).

%%%----------------------------------------------------------------------%%%
\vskip 0.2 true in 
{\bf Problem $\# 2$:} (Problem 6(a,b,d) in Chapter 2 of MLS). 

{\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}

{\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}$

{\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}

%%%----------------------------------------------------------------------%%%
\vskip 0.2 true in
{\bf Problem $\#$ 3:} (Problem 8(b) in Chapter 2 of the MLS text)

Oops, this was mistakenly assigned, as you already completed this problem
in Homework $\#2$!

%%%----------------------------------------------------------------------%%%
\vskip 0.2 true in
{\bf Problem $\#$ 4:} (Problem 10(b,c) in Chapter 2 of the MLS text)

{\bf Part (b):}
If $\omega\in\real$, then let:
  \[ \hat{\omega} = \begin{bmatrix} 0 & -\omega\\ \omega & 0\end{bmatrix}
             = \omega J \tab {\rm where}\ 
     J =  \begin{bmatrix} 0 & -1\\ 1 & 0\end{bmatrix} \]
Note that:
  \[ \hat{\omega}^2 = -\omega^2 I; \tab \hat{\omega}^3 = \omega^3 J \]
Hence:
  \[ \begin{array}{rl}
    e^{\hat{\omega}\theta} 
     &= I + \omega\theta J + \frac{(\omega\theta)^2}{2!} J^2 
          + \frac{(\omega\theta)^3}{3!} J^3 + \cdots \\
     &= I + (\omega\theta) J - \frac{(\omega\theta)^2}{2!} I 
         - \frac{(\omega\theta)^3}{3!} J + \cdots \\
     &= (1 + \frac{(\omega\theta)^2}{2!} + \frac{(\omega\theta)}{4!}
           + \cdots )I
       +(\omega\theta - \frac{(\omega\theta)^3}{3!} + \cdots) J \\
     &= \cos(\omega\theta) I + \sin(\omega\theta) J \\
     &\quad \\ 
     &= \begin{bmatrix}\cos(\omega\theta) & -\sin(\omega\theta)\\ 
                      \sin(\omega\theta) &  \cos(\omega\theta)\end{bmatrix}
      \end{array} \]


{\bf Part (c):} This can be shown by brute-force calculation.  If $R\in SO(2)$
then:
\def\stheta{\sin\theta}
\def\ctheta{\cos\theta}

\[ \begin{array}{rl}
    R\hat{\omega}R^T &= 
   \begin{bmatrix}\ctheta & -\stheta\\ \stheta & \ctheta\end{bmatrix}
   \begin{bmatrix}0 & -\omega \\ \omega & 0\end{bmatrix}
   \begin{bmatrix} \ctheta & \stheta\\ -\stheta & \ctheta\end{bmatrix} \\
&= \omega \begin{bmatrix} \ctheta & -\stheta\\ \stheta & \ctheta\end{bmatrix}
         \begin{bmatrix}\stheta & -\ctheta\\ \ctheta & \stheta\end{bmatrix}
   \\
&= \omega \begin{bmatrix} 0 & -(\cos^2\theta + \sin^2\theta)\\ 
                 (\sin^2\theta + \cos^2\theta) & 0\end{bmatrix}
   \omega \begin{bmatrix}0 & -1\\ 1 & 0\end{bmatrix}
   = \hat{\omega} \end{array}\]

%%%----------------------------------------------------------------------%%%
\vskip 0.2 true in
{\bf Problem $\#$ 5:} (Problem 11(a,b) in Chapter 2 of the MLS text)

{\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)$.

{\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{document}

