\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}
\input{/home/jwb/tex/reals.tex}

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

{\bf Problem 1:} (Problem 11(a,b,d,e) in Chapter 2 of MLS).

{\bf Part (a):} Recall that the matrix exponential of a twist, $\hat{\xi}$, is:
   \[ e^{\phi\hat{\xi}} = I + \frac{\phi}{1!} \hat{\xi} 
          + \frac{\phi^2}{2!}\hat{\xi}^2 + 
	              \frac{\phi^3}{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} \cr \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 \\ 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}\\ 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}\\ \vec{0}^T & 0\end{bmatrix}}^{\vee} 
    = \begin{bmatrix} q_y\\ -q_x\\ 1\end{bmatrix} \]
assuming $\omega = 1$.

{\bf Part (d):} Let 
  $$g=\begin{bmatrix} A & \vec{p}\\ \vec{0}^T & 1\end{bmatrix}$$
where $A\in SO(2)$ and $\vec{p}\in\real^2$.  Then direct calculation shows
that $\dot{g}g^{-1}$ and $g^{-1}\dot{g}$ are twists.  The spatial and
body velocities have definitions analogous to those for 3-dimensional
rigid bodies

{\bf Part (e):} Let $\hat{V}^b$ denote the planar body velocity:
   \[ \hat{V}^b = \begin{bmatrix} \hat{\omega}^b & \vec{v}^b\\
			        \vec{0}^T & 0\end{bmatrix} \]
where $\hat{\omega}^b\in so(2)$, $\vec{v}^b\in\real^2$. Then the planar
spatial velocity is:
  \[\begin{array}{rl} 
    \hat{V}^s &= Ad_g \hat{V}^b = g\hat{V}^b g^{-1} \\ 
      &= \begin{bmatrix} R & \vec{p}\\ \vec{0}^T & 1\end{bmatrix}
         \begin{bmatrix} \hat{\omega}^b & \vec{v}^b\\ \vec{0}^T & 0\end{bmatrix} 
         \begin{bmatrix} R^T & -R^T\vec{p}\\ \vec{0}^T & 0\end{bmatrix}\\ 
      &= \begin{bmatrix} R\hat{\omega}^bR^T & -R\hat{\omega}^b R^T\vec{p}
	       + R \vec{v}^b\\ \vec{0}^T & 0\end{bmatrix} \end{array} \]
Therefore:
  \[ \hat{\omega}^s = R \hat{\omega}^b R^T \tab 
     \vec{v}^s = R\vec{v}^b - R\hat{\omega}^b R^T \vec{p}
               = R\vec{v}^b - \hat{\omega}^s\vec{p} \]
The spatial angular velocity can be simplified as follows:
  \[ \hat{\omega}^s = R \hat{\omega}^b R^T = 
     \begin{bmatrix} r_{11} & r_{12}\\ r_{21} & r_{22}\end{bmatrix}
     \begin{bmatrix} 0 & -\omega\\ \omega & 0\end{bmatrix}
     \begin{bmatrix} r_{11} & r_{21}\\ r_{12} & r_{22}\end{bmatrix}
     = \omega \begin{bmatrix} 0 & -\det(R)\\ \det(R) & 0\end{bmatrix}
     = \omega \begin{bmatrix} 0 & -1\\ 1 & 0\end{bmatrix} = \hat{\omega}^b \]

Using this result:
  \begin{eqnarray}
    \vec{v}^s &=& R\vec{v}^b - \hat{\omega}^s \vec{p}
               = R\vec{v}^b + \omega^b \begin{bmatrix} p_y\\ -p_x\end{bmatrix} 
		 \end{eqnarray}
Therefore:
  \[ V^s = \begin{bmatrix} \vec{v}^s\\ \omega^s\end{bmatrix}
         = \begin{bmatrix} R & \begin{bmatrix} p_y \\ -p_x \end{bmatrix} \\
		         \vec{0}^T & 1\end{bmatrix} V^b \]


%%%----------------------------------------------------------------------%%%
{\bf Problem 2:} (Problem 14 in Chapter 2 of MLS).

{\bf Part (a):} Let $g\in SE(3)$ denote a homogeneous transformation matrix:
  \[ g= \begin{bmatrix} R & \vec{p}\\ \vec{0}^T & 1\end{bmatrix} \tab
    Ad_g = \begin{bmatrix} R & \hat{p}R\\ 0 & R\end{bmatrix} \]
Then:
  \[ g^{-1}=\begin{bmatrix} R^T & -R^T\vec{p}\\ \vec{0}^T & 1\end{bmatrix} \tab
     Ad_{g^{-1}} = \begin{bmatrix} R^T & -\widehat{(R^T\vec{p})} R^T\\
       		                \vec{0}^T & R^T\end{bmatrix}
        = \begin{bmatrix} R^T & -R^T\hat{p}\\ 0 & R^T\end{bmatrix} \]
where we have made use of the identity $\widehat{(R^T\vec{p})} = R^T \hat{p} 
R$.  Let's now compute $Ad_g Ad_{g^{-1}}$:
  \[ Ad_g Ad_{g^{-1}} = 
     \begin{bmatrix} R & \hat{p}R\\ 0 & R\end{bmatrix}
     \begin{bmatrix} R^T & -R^T\hat{p}\\ 0 & R^T\end{bmatrix}
   = \begin{bmatrix} I & 0\\ 0 & I\end{bmatrix} \]
Hence, $Ad_{g^{-1}}$ must equal $(Ad_g)^{-1}$ since $Ad_g Ad_{g^{-1}} = I$.

{\bf Part (b):} If 
  \[   g_1=\begin{bmatrix} R_1 & \vec{p}_1\\ \vec{0}^T & 1\end{bmatrix} \tab
       g_2=\begin{bmatrix} R_2 & \vec{p}_2\\ \vec{0}^T & 1\end{bmatrix} \]
Then
  \[ g_1 g_2=\begin{bmatrix} R_1 R_2 & \vec{p}_1 + R_1 \vec{p}_2\\
			\vec{0}^T & 1\end{bmatrix} \]
Hence:
  \[ \begin{array}{rl}
    Ad_{g_1 g_2} &= 
     \begin{bmatrix} R_1 R_2 & (\vec{p}_1 + R_1 \vec{p}_2)^{\hat{}} R_1 R_2\\ 
	  0 & R_1 R_2 \end{bmatrix} \\
    &= \begin{bmatrix} R_1 R_2 & \hat{p}_1 R_1 R_2 +R_1\hat{p}_2 R_1^T R_1 R_2\\ 
		0 & R_1 R_2  \end{bmatrix} \\ 
    &= \begin{bmatrix} R_1 R_2 & \hat{p}_1 R_1 R_2 +R_1\hat{p}_2 R_2\\ 
		0 & R_1 R_2  \end{bmatrix} \\ 
    &= \begin{bmatrix} R_1 & \hat{p}_1 R_1\\  0 & R_1 \end{bmatrix}
       \begin{bmatrix} R_2 & \hat{p}_2 R_2\\  0 & R_2 \end{bmatrix} = Ad_{g_1}
	Ad_{g_2} \end{array}\]


%%%----------------------------------------------------------------------%%%
{\bf Problem 3:} (Problem 15 in Chapter 2 of MLS).

Since $g_{ac}=g_{ab}g_{bc}$, using the product rule for differentiation
yields:
 \begin{eqnarray}
   \hat{V}^b_{ac} &=& g^{-1}_{ac}\dot{g}_{ac} \nonumber\\
  &=& g^{-1}_{bc}g^{-1}{ab}(\dot{g}_{ab}g_{bc}+g_{ab}\dot{g}_{bc}) \nonumber\\ 
  &=& g^{-1}_{bc}\hat{V}^b_{ab} g_{bc} + \hat{V}^b_{bc} \end{eqnarray}
Converting to vector form via the $\vee$ operator yields:
   \[ \vec{V}^b_{ac} = (\hat{V}_{ac}^b)^{\vee} = 
       Ad_{g_{bc}^{-1}} \vec{V}_{ab}^c + \vec{V}_{bc}^b \]

%%%----------------------------------------------------------------------%%%
{\bf Problem 4:} (Problem 18(e) in Chapter 2 of MLS.)

{\bf Part (e):} Let the position and orientation of a moving rigid body be
given by $R(t)$ and $\vec{p}(t)$.  Let $V^b$ be the body velocity of the rigid
body, and let $F^b$ be a wrench applied to the body, expressed in body
coordinates.  The power applied to the body due to this wrench is given by: 
   \begin{equation}\label{eq:power}
     V^b \cdot F^b = (V^b)^T F^b \end{equation}
Let $V^h$ denote the velocity of the body in hybrid coordinates.  Similarly,
define the hybrid wrench to be $F^h$.  We will define $F^h$ to be the wrench
that preserves the amount of power in Eq. (\ref{eq:power}):
  \[ \begin{array}{rl}
    V^b \cdot F^b = (V^b)^T F^b &= V^h \cdot F^h \\ 
                                &= (V^h)^T F^h \\
     &= (\begin{bmatrix}R & 0\\ 0& R\end{bmatrix} V^b)^T F^h \\ 
     &=(V^b)^T \begin{bmatrix}R^T & 0\\ 0 & R^T\end{bmatrix} F^h\end{array} \]
Hence, it must be true that:
   \[ F^b = \begin{bmatrix}R^T & 0\\ 0 & R^T\end{bmatrix} F^h \tab {\rm or}\tab
      F^h = \begin{bmatrix}R & 0\\ 0 & R\end{bmatrix} F^b \]

%%%----------------------------------------------------------------------%%%
{\bf Problem 5:}
Let points $P$, $Q$, and $R$ be located at positions
$\vec{r}_1$, $\vec{r}_2$, and $\vec{r}_3$ with respect to a fixed observer.
Let the velocities of each of these points be $\dot{\vec{r}}_1$, 
$\dot{\vec{r}}_2$, and $\dot{\vec{r}}_3$.  Recall that a spatial velocity
has the form 
   \[ \hat{V}^s = \begin{bmatrix}\hat{\omega}^s & \vec{v}^s \\ 
		\vec{0}^T & 0\end{bmatrix} \]

In general, let $\vec{p}$ denote the location of a particle in the moving
body, with respect to the observing frame.  Recall that the velocity of this
particle, $\dot{\vec{p}}$, as seen in the observing frame, is related to the
location of the particle (as seen in the stationary observing frame) by
$\dot{\vec{p}} = \hat{V}^s \vec{p}$---where we have used the same notation
for homogeneous coordinates.  Thus, for each of the particles the following
relationship holds:
    \begin{eqnarray}
      \dot{\vec{r}}_1 &=& \vec{\omega}^s \times \vec{r}_1 + \vec{v}^s 
		\label{eq:one} \\
        \dot{\vec{r}}_2 &=& \vec{\omega}^s \times \vec{r}_2 + \vec{v}^s 
		\label{eq:two} \\
        \dot{\vec{r}}_3 &=& \vec{\omega}^s \times \vec{r}_3 + \vec{v}^s 
		\label{eq:three} \end{eqnarray}
Hence, we have three simultaneous equations in the unknowns $\vec{\omega}^s$
and $\vec{v}^s$.  To solve these equations, we follow closely the technique
that was used in the handout entitled `` Computing the Screw Parameters of a
Rigid Body Displacement.''

{\bf Step 1:} subtract Equation \ref{eq:three} from Equations (\ref{eq:one})
and (\ref{eq:two}).  
  \begin{eqnarray}
      \dot{\vec{r}}_1 - \dot{\vec{r}}_3 &=& \vec{\omega}^s \times(\vec{r}_1
	  -\vec{r}_3) \label{eq:onethree}\\
      \dot{\vec{r}}_2 - \dot{\vec{r}}_3 &=& \vec{\omega}^s \times(\vec{r}_2
	  -\vec{r}_3) \label{eq:twothree}\end{eqnarray}

{\bf Step 2:} Take the cross product of the vector $(\dot{\vec{r}}_2 -
\dot{\vec{r}}_3)$ with Equation (\ref{eq:onethree}).  

  \begin{equation}\label{eq:temp}
    (\dot{\vec{r}}_2 - \dot{\vec{r}}_3) \times 
   (\dot{\vec{r}}_1 - \dot{\vec{r}}_3) = 
   (\dot{\vec{r}}_2 - \dot{\vec{r}}_3)\times (\vec{\omega}^s \times(\vec{r}_1
	  -\vec{r}_3)). \end{equation}

{\bf Step 3:} We now use two facts.  First, we use the triple vector identity 
$\vec{a}\times(\vec{b}\times\vec{c}) = (\vec{a}\cdot\vec{c})\vec{b} - 
(\vec{a}\cdot\vec{b})\vec{c}$.  Second, from Equation (\ref{eq:twothree})
we know that $\dot{\vec{r}}_2 - \dot{\vec{r}}_3$ is orthogonal to 
$\vec{\omega}$.  Hence, Equation (\ref{eq:temp}) reduces to 
  \[  (\dot{\vec{r}}_2 - \dot{\vec{r}}_3) \times  
    (\dot{\vec{r}}_1 - \dot{\vec{r}}_3) = \vec{\omega} 
   [(\dot{\vec{r}}_2 - \dot{\vec{r}}_3)\cdot (\vec{r}_1 -\vec{r}_3)] . \]
The quantity $\vec{\omega}$ is then easily found as 
   \[ \vec{\omega} = 
   [(\dot{\vec{r}}_2 - \dot{\vec{r}}_3)\cdot (\vec{r}_1 -\vec{r}_3)]^{-1}
 (\dot{\vec{r}}_2 - \dot{\vec{r}}_3) \times  
    (\dot{\vec{r}}_1 - \dot{\vec{r}}_3). \]

{\bf Step 4:} Any one of Equations (\ref{eq:one}), (\ref{eq:two}), or
(\ref{eq:three}) could be used to solve for $\vec{v}^s$.



\end{document}