HW3 - Body Kinematics

91/100

[-3] Q3.a) Only joint 7, 8 are involved so the dim for theta should be 61 and dim for J should be 36.

[-0.5] 3.b.i) Should mention the effect include magnitudes of the endpoint velocity vectors. Refer to the solution file for more details.

[-2] 3.b.ii) You should compute the Jacobian submatrix for joint in the IK chain to get the J_rWrist.

[-0.5] Q4(a) Inversed velocity vectors.

[-2] Q4(b). Correct procedure, but need to compute actual values.

[-1] Q4(c). Correct procedure, but need to compute actual direction vector.

1. Tree Data Structure for Joint Hierarchy

a) Draw the tree data structure representing the skeleton joint hierarchy.

The skeleton starts with the pelvis (root), and it branches out into two arms, a head, and other joints.

• Pelvis
  • lShoulder
    • lElbow
      • lWrist
  • rShoulder
    • rElbow
      • rWrist
  • Neck
    • Head

b) List the AJoint information for each joint.

For each joint, we will specify the parent-child relationships, and the names and numbers associated.

1. Pelvis
   • Parent: World (None)
   • Children:
     • 3
     • lShoulder, rShoulder, Neck
     • 4, 7, 2
2. lShoulder
   • Parent:
     • Pelvis
     • 1
   • Children:
     • 1
     • lElbow
     • 8
3. lElbow
   • Parent:
     • lShoulder
     • 4
   • Children:
     • 1
     • lWrist
     • 6
4. lWrist
   • Parent: lElbow
   • Children: None
5. rShoulder
   • Parent:
     • Pelvis
     • 1
   • Children:
     • 1
     • rElbow
     • 8
6. rElbow
   • Parent:
     • rShoulder
     • 7
   • Children:
     • 1
     • rWrist
     • 9
7. rWrist
   • Parent: rElbow
   • Children: None
8. Neck
   • Parent:
     • Pelvis
     • 1
   • Children:
     • 1
     • Head
     • 3
9. Head
   • Parent:
     • Neck
     • 2
   • Children: None

---

2. Frame of Reference and Kinematic Equations (25 points)

**a) **

i) General form of the rotation matrix for joint j:

The rotation matrix $R^i_j$ for joint j, with the given rotation order (z -> y -> x), is:

$R^i_j(\theta_j) = R_z(\theta_{jz})\cdot R_y(\theta_{jy})\cdot R_x(\theta_{jx}) $

Where:

• $R_z(\theta_{jz}) = \begin{bmatrix} \cos(\theta_{jz}) & -\sin(\theta_{jz}) & 0 \\ \sin(\theta_{jz}) & \cos(\theta_{jz}) & 0 \\ 0 & 0 & 1 \end{bmatrix}$
• $R_y(\theta_{jy}) = \begin{bmatrix} \cos(\theta_{jy}) & 0 & \sin(\theta_{jy}) \\ 0 & 1 & 0 \\ -\sin(\theta_{jy}) & 0 & \cos(\theta_{jy}) \end{bmatrix}$
• $R_x(\theta_{jx}) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(\theta_{jx}) & -\sin(\theta_{jx}) \\ 0 & \sin(\theta_{jx}) & \cos(\theta_{jx}) \end{bmatrix}$

Thus,

$$R^i_j(\theta_j) =\begin{bmatrix} \cos\theta_{jy}\cos\theta_{jz} & \sin\theta_{jx}\sin\theta_{jy}\cos\theta_{jz}-\cos\theta_{jx}\sin\theta_{jz} & \sin\theta_{jx}\sin\theta_{jz}+\cos\theta_{jx}\sin\theta_{jy}\cos\theta_{jz} \\ \cos\theta_{jy}\sin\theta_{jz} & \sin\theta_{jx}\sin\theta_{jy}\sin\theta_{jz}+\cos\theta_{jx}\cos\theta_{jz} & \sin\theta_{jz}\sin\theta_{jy}\cos\theta_{jx}-\cos\theta_{jz}\sin\theta_{jx} \\ -\sin\theta_{jy} & \cos\theta_{jy}\sin\theta_{jx} & \cos\theta_{jy}\cos\theta_{jx} \end{bmatrix}$$

ii) Joint displacement vectors $d^i_{ij}$:

Given the bone lengths of 1 unit, we have:

• $d^7_{78} = \begin{bmatrix} -1 \\ 0 \\ 0 \end{bmatrix}$
• $d^8_{89} = \begin{bmatrix} -1 \\ 0 \\ 0 \end{bmatrix}$

For the clavicle bone connecting the right shoulder:

• $d^1_{17} = \begin{bmatrix} -0.707 \\ 1.457 \\ 0\end{bmatrix}$

iii) Euler Angle rate to rotational velocity matrix $L(\theta_j)$:

The matrix $L(\theta_j)$ that relates Euler angle rates $\dot{\theta_j}$ to rotational velocity $\omega_j$ can be written as:

$L(\theta_j) = \begin{bmatrix} 1 & 0 & -\sin(\theta_{jy}) \\ 0 & \cos(\theta_{jx}) & \cos(\theta_{jy})\sin(\theta_{jx}) \\ 0 & -\sin(\theta_{jx}) & \cos(\theta_{jx})\cos(\theta_{jy}) \end{bmatrix}$

This gives the relationship between Euler angles and the angular velocity of the joint.

b)

i) Pelvis as the root (j = 1):

When the pelvis (joint 1) is the root of the hierarchy, the position and orientation of joint 9 can be expressed as a product of the transformation matrices along the chain from the pelvis to the wrist.

$F^0_9 = F^0_1 F^1_7 F^7_8 F^8_9$

where the transformation matrix from joint $i$ to $j$ is given by:
$F^i_j = \begin{bmatrix} R^i_j(\theta_j) & d^i_j \\ 0 & 1 \end{bmatrix}$
where $R^i_j(\theta_j)$ is the 3x3 rotation matrix, and $d^i_j$ is the displacement vector from joint $i$ to joint $j$.

The position and orientation of the rWrist in world coordinates (starting from the pelvis) is the product of these transformation matrices along the kinematic chain:

$$\begin{align} F^0_9 &= F^0_1 F^1_7 F^7_8 F^8_9\\ &= \begin{bmatrix} R^0_1(\theta_1) & d^0_1 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R^1_7(\theta_7) & d^1_7 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R^7_8(\theta_8) & d^7_8 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R^8_9(\theta_9) & d^8_9 \\ 0 & 1 \end{bmatrix} \end{align}$$

---

ii) lWrist as the root (j = 6):

The transformation from joint 6 to joint 9 is given by:

$F^0_9 = F_6^0 F^6_5 F^5_4 F_1^4 F^1_7 F^7_8 F^8_9$

$$\begin{align} F^0_9 &= F_6^0 F^6_5 F^5_4 F_1^4 F^1_7 F^7_8 F^8_9\\ &= \begin{bmatrix} R^0_6(\theta_6) & d^0_6 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R^6_5(\theta_5) & d^6_5 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R^5_4(\theta_4) & d^5_4 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R^4_1(\theta_1) & d^4_1 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R^1_7(\theta_7) & d^1_7 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R^7_8(\theta_8) & d^7_8 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R^8_9(\theta_9) & d^8_9 \\ 0 & 1 \end{bmatrix} \end{align}$$

---

3. Forward Kinematic Equations (25 points)

a) Joints used and dimensionality of $x$, $\Theta$, and $J$:

-3 错误的，

如果root是1，则只有7和8；如果root是6，则这样是对的

To control the position of the right wrist (joint 9) with respect to the left wrist (joint 6), the joints involved would be 1 (pelvis), 4 (lShoulder), 5 (lElbow), 6 (lWrist), 7 (rShoulder), 8 (rElbow), and 9 (rWrist). However, 1 (pelvis), 6 (lWrist) and 9 (rWrist) do not influence the position of joint 9 with respect to joint 6. So, 4 (lShoulder), 5 (lElbow), 7 (rShoulder), 8 (rElbow) are used.

• Dimensionality of $\dot x=3\times1=3$
• Dimensionality of $\dot \Theta= 3\times4=12 $ (for the involved joints)
• Dimensionality of $J=3\times \dim(\hat\Theta)=3\times12=36$

**b) **

i) What do the vectors $\mathbf{b}_{jx}, \mathbf{b}_{jy}, \mathbf{b}_{jz}$ represent for joint j?

-0.5

The vectors $\mathbf{b}_{jx}, \mathbf{b}_{jy}, \mathbf{b}_{jz}$ represent the contribution of joint $j$'s rotation to the end-effector's velocity along the x, y, and z axes, respectively. These are derived from the cross product of the unit vectors along the local axes of joint $j$ and the radius vector from joint $j$ to the end-effector $n$. More precisely:

• $\mathbf{b}_{jx} = \hat{a}_{jx} \times \mathbf{r}_{jn}$
• $\mathbf{b}_{jy} = \hat{a}_{jy} \times \mathbf{r}_{jn}$
• $\mathbf{b}_{jz} = \hat{a}_{jz} \times \mathbf{r}_{jn}$

Where $\hat{a}_{jx}, \hat{a}_{jy}, \hat{a}_{jz}$ are the unit vectors of the local axes of joint $j$, and $\mathbf{r}_{jn}$ is the vector from joint $j$ to the end-effector (right wrist, joint 9).

These vectors represent the instantaneous linear velocity directions at the end effector caused by rotation about the x, y, and z axes of joint j respectively.

ii) Minimal set of equations to compute the Jacobian matrix for the rWrist (joint 9):

-2

Steps:

1. Extract column vectors $\vec a_{ix}^0,\vec a_{iy}^0,\vec a_{iz}^0$ from $R_i^0$
2. Compute $\vec b_{ix}^0=\hat a_{ix}^0 \times\vec r_{in}^0$
3. Assemble $B=[\begin{array}{c:c}b_{ix}^0&b_{iy}^0&b_{iz}^0]\end{array}$
4. Compute L (Euler angle rates to angular velocity )
5. Compute $J=BL$

The Jacobian matrix $J$ relates the end-effector's velocity to the joint velocities. To compute the Jacobian, we need to consider how each joint in the chain contributes to the velocity of the rWrist (joint 9). For each joint $j$, the contribution is given by:

$\mathbf{v}_n = J \dot{\Theta} = \sum_{j=1}^{9} (\mathbf{b}_{jx} \dot{\theta}_{jx} + \mathbf{b}_{jy} \dot{\theta}_{jy} + \mathbf{b}_{jz} \dot{\theta}_{jz})$

Thus, the total velocity of the rWrist is the sum of the contributions from the joints in the chain. Each joint contributes based on its rotation and its distance from the end-effector.

The minimal set of equations needed to compute the Jacobian is:

1. Compute the rotation matrices $R^0_j$ for each joint in the chain (joints 1 to 9).
2. Compute the radius vectors $\mathbf{r}_{jn}$ for each joint (from joint j to joint 9).
3. Compute the cross products $\mathbf{b}_{jx}, \mathbf{b}_{jy}, \mathbf{b}_{jz}$ for each joint to form the submatrices of the Jacobian.

The final Jacobian $J$ is a 3x9 matrix that relates the joint velocities $\dot{\Theta}$ to the velocity of the rWrist in world coordinates.

---

4. Right Arm Moving in the X1-Y1 Plane (25 points)

a) Draw a picture of the right arm configuration when $\theta_1 = [0, 0, 0]^T$, $\theta_7 = [0, 0, -30^\circ]^T$, $\theta_8 = [0, 0, -30^\circ]^T$.

-0.5

b) Vectors $\mathbf{r}_{19}, \mathbf{r}_{79}, \mathbf{r}_{89}$:

-2

• $\mathbf{r}_{89}$ is the vector from the right elbow (joint 8) to the right wrist.
• $\mathbf{r}_{79} = \mathbf{r}_{78} + \mathbf{r}_{89}$
• $\mathbf{r}_{19} = \mathbf{r}_{17} + \mathbf{r}_{79}=\mathbf{r}_{17} + \mathbf{r}_{78} + \mathbf{r}_{89}$

The exact components depend on the angles of rotation.

c) Magnitude and direction of velocity vectors $\mathbf{v}_{19}, \mathbf{v}_{79}, \mathbf{v}_{89}$:

-1

$\vec v=\vec \omega\times\vec r$

Each velocity vector $\mathbf{v}_{ij}$ can be computed using the angular velocities and the displacement vectors. For example, the velocity of the wrist due to the rotation of joint 1 is:

• $\mathbf{v}_{19} = \dot{\theta}_{1z} \times \mathbf{r}_{19}$
• $\mathbf{v}_{79} = \dot{\theta}_{7z} \times \mathbf{r}_{79}$
• $\mathbf{v}_{89} = \dot{\theta}_{8z} \times \mathbf{r}_{89}$

The direction of each vector is perpendicular to both the axis of rotation and the displacement vector.

The v and direction is shown in pic.

d) Relationship between velocity vectors and the $B_j$ matrices:

$V_{end}^0=\dot x_{end}=J\dot\Theta=BL\dot\Theta$

$v_{19}+v_{79}+v_{89}=v_9$

$v_{19}+v_{79}+v_{89}=v_9=J_9\dot\Theta_9=B_9L_9\dot\Theta_9$

---

5. Inverse Kinematics (15 points)

Step 1: Compute Joint Angle $\theta_8$ (Elbow Joint)

We start by determining the angle at the elbow (joint 8) using the law of cosines. This will give us the angle needed to achieve the desired distance between the shoulder and the wrist.

Let:

• $\mathbf{p}_d$ be the desired position of the right wrist (joint 9),
• $\mathbf{p}_7$ be the position of the right shoulder (joint 7),
• $l_7$ and $l_8$ be the lengths of the upper arm (from the shoulder to the elbow) and the forearm (from the elbow to the wrist), respectively.

Desired Direction: $r_d=p_d^0-p_7^0$

Now, we can apply the law of cosines to calculate the elbow angle $\theta_8$:

$\cos(\theta_8) = \frac{l_7^2 + l_8^2 - r_d^2}{2 l_7 l_8}$

From this, we compute $\theta_8$ as:

$\theta_8 = \cos^{-1} \left( \frac{l_7^2 + l_8^2 - r_d^2}{2 l_7 l_8} \right)$

This gives us the angle needed at the elbow to reach the wrist.

**Step 2: Compute quaternion $q_7$ at joint 7 to rotate $r_d$ so it points at $p_d$ **

Desired Direction: $d=p_d^0-p_7^0$

$d\cdot r_d=||d|| ||r_d||\cos(\Delta \theta_1)$

Angle: $\Delta \theta_7=\frac{\cos^{-1}(d\cdot r_d) }{||d|| ||r_d||}$

Axis: $\hat a_7^0=\frac{r_d\times d}{||r_d\times d||}$

Local Axis: $\hat a^7_7=R_0^7\hat a_7^0$

$\Delta q_7=[\cos\frac{\Delta\theta_7}{2},\hat a_7^7\sin\frac{\Delta\theta_7}{2}]^T$

Joint 7 update: $q_7=q_7\Delta q_7$

**Step 3: Joint 3 Orientation **

$R_d^0=R_7^0R_8^7R_9^8=[\hat u_x, \hat u_y,\hat u_z]$

$R_9^8=(R_7^0R_8^7)^{-1}R_d^0$

Given $R_9^8$ can find joint angle $\theta_9=[\theta_{9x},\theta_{9y},\theta_{9z}]^T$