Question 1

Resolve the position-level forward and inverse kinematics of the Furuta pendulum with the help of the coordinate systems shown as shown in the figure. Once you have the kinematic equations, answer the following questions:

(a) If the joint angles $ \theta $ are given by

$$ \theta = \begin{bmatrix} \theta_1 \\ \theta_2 \end{bmatrix} = \begin{bmatrix} 5\pi/6 \\ -3\pi/7 \end{bmatrix} $$

measured from zero position, what is the pose of the end-effector, i.e., the frame $\{C\}$ with respect to the fixed frame $\{0\}$? Provide the orientation using both the rotation matrix and the corresponding quaternion.

(b) At the same angles given in part (a), what is the pose of frame $\{2\}$ with respect to the fixed frame $\{0\}$? Provide the orientation using both the rotation matrix and the corresponding quaternion.

(c) Provide the axis-angle representation of the orientation for parts (a) and (b). Use rodrigues’s formula to recover the rotation matrices you found in parts (a) and (b).

(d) Suppose a camera is installed at the end-effector of the Furuta pendulum. Let $\{C\}$ denote the camera frame, as shown. What is the value of $ \theta_1 $ and $ \theta_2 $ if the $ z $-axis of the camera points in the direction $ k $, where

$$ k = -\frac{1}{2} x_0 + \frac{1}{2} y_0 - \frac{\sqrt{2}}{2} z_0. $$

(e) Repeat part (d) to find the value of $ \theta_1 $ and $ \theta_2 $ if the origin of frame $\{C\}$ is located at a point $ P $, whose coordinate vector is given in frame $\{0\}$ by

$$ p = 1.075x_0 + 0.303y_0 + 1.022z_0. $$

import sympy as sp
import numpy as np
from spatialmath import SO3
from spatialmath.base import r2q, qprint, tr2angvec, skew, vexa, tr2adjoint
from scipy.linalg import expm # used only for the matrix exponential part of rodrigues formula

Instruction on How to Read Variables in this Report

In this report, we will be making use of some shorthand notation within the code.

"t01" is intended to be read as ${}^0t_1$, the position of frame {1} in respect to frame {2}.

"R01 is intended to be read as ${}^0R_1$, the rotation of frame {1} in respect to frame {2}.

"T01" is intended to be read as ${}^{0}T_{1}$, the pose of frame {1} in respect to frame {2}. Note the difference between "t01" and "T01"

"twist_0c_spatial" is intended to be read as ${}^0V_c^0$, the twist of frame {c} in respect to frame {0} represented in frame {0}.

"twist_0c_body" is intended to be read as ${}^cV_c^0$, the twist of frame {c} in respect to frame {0} represented in frame {c}.

Solve Forward Kinematics - Q1

Resolve the position-level forward kinematics of the Furuta pendulum with the help of the coordinate systems shown as shown in the figure.

Find:
$^{0}T_{c} = {}^{0}T_{1} \, {}^{1}T_{2} \, {}^{2}T_{c}$

d1, d2, d3 = sp.symbols('d1 d2 d3')

# Find the position matrixes
t01 = sp.Matrix([0,0,d1]) #read as the position of frame 1 in respect to frame 0
t12 = sp.Matrix([0,0,d2])
t2c = sp.Matrix([0,0,d3])

theta1, theta2 = sp.symbols('theta1 theta2')
# - Find the Rotation Matrixes
# Find R01 (read as the rotation matrix of 1 in respect to 0)

Rx90 = SO3.Rx(sp.pi/2) #90 degrees
RyTheta1 = SO3.Ry(theta1)
R01 = Rx90 * RyTheta1

sp.Matrix(R01.A)

$\displaystyle \left[\begin{matrix}\cos{\left(\theta_{1} \right)} & 0 & \sin{\left(\theta_{1} \right)}\\\sin{\left(\theta_{1} \right)} & 0 & - \cos{\left(\theta_{1} \right)}\\0 & 1 & 0\end{matrix}\right]$

# Find R12

RyTheta2 = SO3.Ry(theta2)
R12 = Rx90 * RyTheta2

sp.Matrix(R12.A)

$\displaystyle \left[\begin{matrix}\cos{\left(\theta_{2} \right)} & 0 & \sin{\left(\theta_{2} \right)}\\\sin{\left(\theta_{2} \right)} & 0 & - \cos{\left(\theta_{2} \right)}\\0 & 1 & 0\end{matrix}\right]$

# Find R2c

Rz270 = SO3.Rz(-sp.pi/2)
R2c = Rz270

sp.Matrix(R2c.A)

$\displaystyle \left[\begin{matrix}0 & 1 & 0\\-1 & 0 & 0\\0 & 0 & 1\end{matrix}\right]$

Using the position and rotation matrixes, construct coorponding pose.

def make_pose(R, t): # this function is needed since we are using sympy
    """
    Creates a 4x4 homogeneous transformation matrix T from a rotation matrix R and a translation vector t. 
    AI generated.
    
    Parameters:
      R : A rotation matrix. This can be an object with a .A attribute (like an SO3 object)
          or a standard Sympy Matrix.
      t : A 3x1 position vector. It should be a column vector.
      
    Returns:
      T : A 4x4 homogeneous transformation matrix as a Sympy Matrix.
    """
    # Convert the rotation matrix to a standard Sympy Matrix if necessary. 
    try:
        R_mat = sp.Matrix(R.A)
    except AttributeError:
        R_mat = sp.Matrix(R)
    
    # Ensure t is a column vector.
    t_vec = sp.Matrix(t)
    if t_vec.shape[1] != 1:
        t_vec = t_vec.T if t_vec.shape[0] == 1 else t_vec
    
    # Build the transformation matrix by concatenating R and t, and appending the bottom row.
    T = sp.Matrix.vstack(
        sp.Matrix.hstack(R_mat, t_vec),
        sp.Matrix([[0, 0, 0, 1]])
    )
    return T

T01 = make_pose(R01, t01)
T01

$\displaystyle \left[\begin{matrix}\cos{\left(\theta_{1} \right)} & 0 & \sin{\left(\theta_{1} \right)} & 0\\\sin{\left(\theta_{1} \right)} & 0 & - \cos{\left(\theta_{1} \right)} & 0\\0 & 1 & 0 & d_{1}\\0 & 0 & 0 & 1\end{matrix}\right]$

T12 = make_pose(R12, t12)
T12

$\displaystyle \left[\begin{matrix}\cos{\left(\theta_{2} \right)} & 0 & \sin{\left(\theta_{2} \right)} & 0\\\sin{\left(\theta_{2} \right)} & 0 & - \cos{\left(\theta_{2} \right)} & 0\\0 & 1 & 0 & d_{2}\\0 & 0 & 0 & 1\end{matrix}\right]$

T2c = make_pose(R2c, t2c)
T2c

$\displaystyle \left[\begin{matrix}0 & 1 & 0 & 0\\-1 & 0 & 0 & 0\\0 & 0 & 1 & d_{3}\\0 & 0 & 0 & 1\end{matrix}\right]$

# Construct final pose

T0c = T01 * T12 * T2c
print("The pose of frame c in respect to frame 0 is:")
T0c

The pose of frame c in respect to frame 0 is:

$\displaystyle \left[\begin{matrix}- \sin{\left(\theta_{1} \right)} & \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} & \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)} & d_{2} \sin{\left(\theta_{1} \right)} + d_{3} \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)}\\\cos{\left(\theta_{1} \right)} & \sin{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} & \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} \right)} & - d_{2} \cos{\left(\theta_{1} \right)} + d_{3} \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} \right)}\\0 & \sin{\left(\theta_{2} \right)} & - \cos{\left(\theta_{2} \right)} & d_{1} - d_{3} \cos{\left(\theta_{2} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

# extract final rotation matrix
R0c = T0c[:3,:3]
print("The extracted rotation matrix:")
R0c

The extracted rotation matrix:

$\displaystyle \left[\begin{matrix}- \sin{\left(\theta_{1} \right)} & \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} & \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)}\\\cos{\left(\theta_{1} \right)} & \sin{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} & \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} \right)}\\0 & \sin{\left(\theta_{2} \right)} & - \cos{\left(\theta_{2} \right)}\end{matrix}\right]$

# extract final position matrix
t0c = T0c[:3, 3]
print("The extracted position matrix:")
t0c

The extracted position matrix:

$\displaystyle \left[\begin{matrix}d_{2} \sin{\left(\theta_{1} \right)} + d_{3} \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)}\\- d_{2} \cos{\left(\theta_{1} \right)} + d_{3} \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} \right)}\\d_{1} - d_{3} \cos{\left(\theta_{2} \right)}\end{matrix}\right]$

Solve Inverse Kinematics for Position - Q1

Resolve the position-level inverse kinematics of the Furuta pendulum with the help of the coordinate systems shown as shown in the figure.

Determine $ \theta_1 $ and $ \theta_2 $ required to achieve the desired position $ t $.

Position Equations:

The desired position is given by:

$ {}^0t_c = [{}^0x_c, {}^0y_c, {}^0z_c] $

where:

$ {}^0x_c = f(\theta_1, \theta_2) $

$ {}^0y_c = f(\theta_1, \theta_2) $

$ {}^0z_c = f(\theta_1, \theta_2) $

From these equations, $ \theta_1 $ and $ \theta_2 $ can be solved to achieve the desired position.

xc = sp.symbols(r'{}^0x_c')
yc = sp.symbols(r'{}^0y_c')
zc = sp.symbols(r'{}^0z_c')

# Relate t0c with xc, yc, and zc
eq1 = sp.Eq(xc, t0c[0]) #toc[0] extracts the x component of the position of frame c in respect to frame 0
eq1

$\displaystyle {}^0x_c = d_{2} \sin{\left(\theta_{1} \right)} + d_{3} \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)}$

eq2 = sp.Eq(yc, t0c[1])
eq2

$\displaystyle {}^0y_c = - d_{2} \cos{\left(\theta_{1} \right)} + d_{3} \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} \right)}$

eq3 = sp.Eq(zc, t0c[2])
eq3

$\displaystyle {}^0z_c = d_{1} - d_{3} \cos{\left(\theta_{2} \right)}$

# Now solve
solutions_for_theta = sp.solve([eq1, eq2, eq3], (theta1, theta2), check=False)
# List of 4 possible (theta1, theta2) solutions (see nicely formatted version just below)

Inverse Kinematics Solutions for a Given Position

Keep in mind, it is unlikely that all of these equations result in a real solution. Solutions will have to be verified after plugging in numbers.

---

Solution 1:

$ \theta_1 = - 2 \operatorname{atan}{\left(\frac{d_{3} \sqrt{\frac{\left(- d_{1} + d_{3} + {}^0z_c\right) \left(d_{1} + d_{3} - {}^0z_c\right)}{d_{3}^{2}}} - \sqrt{- d_{1}^{2} + 2 d_{1} {}^0z_c + d_{2}^{2} + d_{3}^{2} - \left({}^0y_c\right)^{2} - \left({}^0z_c\right)^{2}}}{d_{2} - {}^0y_c} \right)} $

$ \theta_2 = \operatorname{acos}{\left(\frac{d_{1} - {}^0z_c}{d_{3}} \right)} $

---

Solution 2:

$ \theta_1 = 2 \operatorname{atan}{\left(\frac{d_{3} \sqrt{\frac{\left(- d_{1} + d_{3} + {}^0z_c\right) \left(d_{1} + d_{3} - {}^0z_c\right)}{d_{3}^{2}}} - \sqrt{- d_{1}^{2} + 2 d_{1} {}^0z_c + d_{2}^{2} + d_{3}^{2} - \left({}^0y_c\right)^{2} - \left({}^0z_c\right)^{2}}}{d_{2} - {}^0y_c} \right)} $

$ \theta_2 = - \operatorname{acos}{\left(\frac{d_{1} - {}^0z_c}{d_{3}} \right)} + 2 \pi $

---

Solution 3:

$ \theta_1 = - 2 \operatorname{atan}{\left(\frac{d_{3} \sqrt{\frac{\left(- d_{1} + d_{3} + {}^0z_c\right) \left(d_{1} + d_{3} - {}^0z_c\right)}{d_{3}^{2}}} + \sqrt{- d_{1}^{2} + 2 d_{1} {}^0z_c + d_{2}^{2} + d_{3}^{2} - \left({}^0y_c\right)^{2} - \left({}^0z_c\right)^{2}}}{d_{2} - {}^0y_c} \right)} $

$ \theta_2 = \operatorname{acos}{\left(\frac{d_{1} - {}^0z_c}{d_{3}} \right)} $

---

Solution 4:

$ \theta_1 = 2 \operatorname{atan}{\left(\frac{d_{3} \sqrt{\frac{\left(- d_{1} + d_{3} + {}^0z_c\right) \left(d_{1} + d_{3} - {}^0z_c\right)}{d_{3}^{2}}} + \sqrt{- d_{1}^{2} + 2 d_{1} {}^0z_c + d_{2}^{2} + d_{3}^{2} - \left({}^0y_c\right)^{2} - \left({}^0z_c\right)^{2}}}{d_{2} - {}^0y_c} \right)} $

$ \theta_2 = - \operatorname{acos}{\left(\frac{d_{1} - {}^0z_c}{d_{3}} \right)} + 2 \pi $

---

Q1 Part A

(a) If the joint angles $ \theta $ are given by

$$ \theta = \begin{bmatrix} \theta_1 \\ \theta_2 \end{bmatrix} = \begin{bmatrix} \pi/3 \\ -3\pi/7 \end{bmatrix} $$

from home position, what is the pose of the end-effector, i.e., the frame $\{C\}$ with respect to the fixed frame $\{0\}$, ${}^0T_c$? Provide the orientation using both the rotation matrix and the corresponding quaternion.

# With the given angles, the pose is now:
subsForA = {d1: 1.2, d2: 0.8, d3: 0.8, theta1: 5 * sp.pi /6, theta2: -3 * sp.pi / 7}
T0c_partA = T0c.subs(subsForA)
print("The pose of frame c in respect to frame 0:")
T0c_partA.evalf()

The pose of frame c in respect to frame 0:

$\displaystyle \left[\begin{matrix}-0.5 & -0.192708781680008 & 0.844312338807984 & 1.07544987104639\\-0.866025403784439 & 0.111260466978157 & -0.487463956090912 & 0.302849158154821\\0 & -0.974927912181824 & -0.222520933956314 & 1.02198325283495\\0 & 0 & 0 & 1.0\end{matrix}\right]$

# Extract the rotation matrix
R0c_partA = T0c_partA[:3,:3]
print("The rotation matrix of frame c in respect to frame 0: ")

R0c_partA.evalf()

The rotation matrix of frame c in respect to frame 0: 

$\displaystyle \left[\begin{matrix}-0.5 & -0.192708781680008 & 0.844312338807984\\-0.866025403784439 & 0.111260466978157 & -0.487463956090912\\0 & -0.974927912181824 & -0.222520933956314\end{matrix}\right]$

# Quaternion representation of the Rotation matrix
R0c_partA = np.array(R0c_partA, dtype=float) #convert R to a np array
q_parta = r2q(R0c_partA)
print("The quaternian representation of the rotation of frame c in respect to frame 0:\n")

qprint(q_parta)

The quaternian representation of the rotation of frame c in respect to frame 0:

 0.3117 < -0.3909,  0.6771, -0.5400 >

Q1 Part B

(b) At the same angles given in part (a), what is the pose of frame $\{2\}$ with respect to the fixed frame $\{0\}$? Provide the orientation using both the rotation matrix and the corresponding quaternion.

# the pose is
T02 = T01 * T12
T02_partb = T02.subs(subsForA)
print("The pose of frame 2 in respect to frame 0:")
T02_partb.evalf()

The pose of frame 2 in respect to frame 0:

$\displaystyle \left[\begin{matrix}-0.192708781680008 & 0.5 & 0.844312338807984 & 0.4\\0.111260466978157 & 0.866025403784439 & -0.487463956090912 & 0.692820323027551\\-0.974927912181824 & 0 & -0.222520933956314 & 1.2\\0 & 0 & 0 & 1.0\end{matrix}\right]$

# so the rotation matrix is
R02_partb = T02_partb[:3,:3]
print("The rotation matrix of frame 2 in respect to frame 0:")

R02_partb.evalf()

The rotation matrix of frame 2 in respect to frame 0:

$\displaystyle \left[\begin{matrix}-0.192708781680008 & 0.5 & 0.844312338807984\\0.111260466978157 & 0.866025403784439 & -0.487463956090912\\-0.974927912181824 & 0 & -0.222520933956314\end{matrix}\right]$

#and the quaternian representation of the rotation matrix is
R02_partb = np.array(R02_partb, dtype=float) #convert R to a np array
q_partb = r2q(R02_partb)
print("The quaternion representation of the rotation of frame 2 in respect to frame 0:\n")
qprint(q_partb)

The quaternion representation of the rotation of frame 2 in respect to frame 0:

 0.6022 <  0.2024,  0.7552, -0.1614 >

Q1 Part C

(c) Provide the axis-angle representation of the orientation for parts (a) and (b). Use Rodrigues’s formula to recover the rotation matrices you found in parts (a) and (b).

# Convert the rotation matrix to its axis angle representation
ang0c, axis0c = tr2angvec(R0c_partA, unit='rad')

print("The angle axis representation of frame c in respect to frame 0:\n")
print("\tAngle: %.3f rad" % (ang0c))
print("\t    or %.3f degrees" % np.rad2deg(ang0c))
print("\t Axis: " + str(axis0c))

The angle axis representation of frame c in respect to frame 0:

	Angle: 2.508 rad
	    or 143.671 degrees
	 Axis: [-0.41141843  0.71259763 -0.56827765]

ang02, axis02 = tr2angvec(R02_partb, unit='rad')

print("The angle axis representation of frame 2 in respect to frame 0:\n")
print("\tAngle: %.3f rad" % (ang02))
print("\t    or %.3f degrees" % np.rad2deg(ang02))

print("\t Axis: " + str(axis02))

The angle axis representation of frame 2 in respect to frame 0:

	Angle: 1.849 rad
	    or 105.938 degrees
	 Axis: [ 0.25347612  0.94598578 -0.20214046]

This equation represents the matrix exponential of a skew-symmetric matrix, which is used in 3D rotation transformations based on the axis-angle representation.

$ R = e^{\widehat{\omega} \theta} = \mathbf{I} + \sin(\theta) \widehat{\omega} + (1 - \cos(\theta)) \widehat{\omega}^2 $

Where:

• $\omega$ is the axis of rotation, a unit vector that defines the direction of rotation.
• $\theta$ is the scalar part of the axis-angle rotation, representing the rotation magnitude in radians.
• $\widehat{\omega}$ is the skew-symmetric matrix of $\omega$, used to compute the rotation matrix.
• $\mathbf{I}$ is the identity matrix.

# this solves rodrigues formula with matrix exponential for C in frame 0
R0c_parta_matrixExponential = expm(skew(axis0c) * ang0c)

print("Recovered rotation matrix using matrix exponential: \n(frame c in respect to frame 0) ")
sp.Matrix(R0c_parta_matrixExponential.round(3)) # sympy for nice printing

Recovered rotation matrix using matrix exponential: 
(frame c in respect to frame 0) 

$\displaystyle \left[\begin{matrix}-0.5 & -0.193 & 0.844\\-0.866 & 0.111 & -0.487\\0.0 & -0.975 & -0.223\end{matrix}\right]$

# this solves it using rodrigues formula for C in 0
R0c_parta_rodrigues = SO3() + np.sin(ang0c) * skew(axis0c) + (1 - np.cos(ang0c)) * (np.dot(skew(axis0c), skew(axis0c)))

print("Recovered rotation matrix using Rodrigues' formula: \n(frame c in respect to frame 0) ")
sp.Matrix(R0c_parta_rodrigues.round(3)) # sympy for nice printing

Recovered rotation matrix using Rodrigues' formula: 
(frame c in respect to frame 0) 

$\displaystyle \left[\begin{matrix}-0.5 & -0.193 & 0.844\\-0.866 & 0.111 & -0.487\\0.0 & -0.975 & -0.223\end{matrix}\right]$

# This is our original R0c matrix for comparing
print("The original rotation matrix was: \n(frame c in respect to frame 0) \n")
sp.pprint(sp.Matrix(R0c_partA.round(3)))

print("\nAs can be seen, Rodrigues' formula correctly recovered the rotation matrix from the angle axis representation.\n")

The original rotation matrix was: 
(frame c in respect to frame 0) 

⎡ -0.5   -0.193  0.844 ⎤
⎢                      ⎥
⎢-0.866  0.111   -0.487⎥
⎢                      ⎥
⎣ 0.0    -0.975  -0.223⎦

As can be seen, Rodrigues' formula correctly recovered the rotation matrix from the angle axis representation.

#this solves it with matrix exponential for 2 in 0
R02_parta_matrixExponential = expm(skew(axis02) * ang02)

print("Recovered rotation matrix using matrix exponential: \n(frame 2 in respect to frame 0) ")
sp.Matrix(R02_parta_matrixExponential.round(3)) # sympy for nice printing

Recovered rotation matrix using matrix exponential: 
(frame 2 in respect to frame 0) 

$\displaystyle \left[\begin{matrix}-0.193 & 0.5 & 0.844\\0.111 & 0.866 & -0.487\\-0.975 & 0.0 & -0.223\end{matrix}\right]$

# this solves it using rodrigues formula for 2 in 0
R02_parta_rodrigues = SO3() + np.sin(ang02) * skew(axis02) + (1 - np.cos(ang02)) * (np.dot(skew(axis02), skew(axis02)))

print("Recovered rotation matrix using Rodrigues' formula: \n(frame 2 in respect to frame 0) ")
sp.Matrix(R02_parta_rodrigues.round(3)) # sympy for nice printing

Recovered rotation matrix using Rodrigues' formula: 
(frame 2 in respect to frame 0) 

$\displaystyle \left[\begin{matrix}-0.193 & 0.5 & 0.844\\0.111 & 0.866 & -0.487\\-0.975 & 0.0 & -0.223\end{matrix}\right]$

# This is our original R0c matrix for comparing
print("The original rotation matrix was: \n(frame 2 in respect to frame 0) \n")
sp.pprint(sp.Matrix(R02_partb.round(3)))

print("\nAs can be seen, Rodrigues' formula correctly recovered the rotation matrix from the angle axis representation.\n")

The original rotation matrix was: 
(frame 2 in respect to frame 0) 

⎡-0.193   0.5   0.844 ⎤
⎢                     ⎥
⎢0.111   0.866  -0.487⎥
⎢                     ⎥
⎣-0.975   0.0   -0.223⎦

As can be seen, Rodrigues' formula correctly recovered the rotation matrix from the angle axis representation.

Q1 Part D

(d) Suppose a camera is installed at the end-effector of the Furuta pendulum. Let $\{C\}$ denote the camera frame, as shown. What is the value of $ \theta_1 $ and $ \theta_2 $ if the $ z $-axis of the camera points in the direction $ k $, where

$$ k = -\frac{1}{2} x_0 + \frac{1}{2} y_0 - \frac{\sqrt{2}}{2} z_0. $$

We need to find the solutions for $\theta_1$ and $\theta_2$ in this equation:

$$ \begin{bmatrix} \ {}^0z_{c,1} \\ \ {}^0z_{c,2} \\ \ {}^0z_{c,3} \end{bmatrix} = \begin{bmatrix} k_1 \\ k_2 \\ k_3 \end{bmatrix} \\ or \\ \begin{bmatrix} \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)} \\ \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} \right)} \\ - \cos{\left(\theta_{2} \right)} \end{bmatrix} = \begin{bmatrix} -0.5 \\ 0.5 \\ - \frac{\sqrt{2}}{2} \end{bmatrix} $$

# Our understanding of this question is that k is the same thing as the z axis part of R0c
# or in other words
print("k is the same thing as the z part of the rotation matrix: ")
R0c[:,2] # the z part of the rotation matrix

k is the same thing as the z part of the rotation matrix: 

$\displaystyle \left[\begin{matrix}\sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)}\\\sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} \right)}\\- \cos{\left(\theta_{2} \right)}\end{matrix}\right]$

# define k
k = sp.Matrix([-1/2, 1/2, -sp.sqrt(2)/2])
# set up equations relating the z part of the rotation matrix with k
eq1_partd = sp.Eq(R0c[0,2], k[0])
eq2_partd = sp.Eq(R0c[1,2], k[1])
eq3_partd = sp.Eq(R0c[2,2], k[2])

# This function takes in the three equations and solves them for theta1 and theta2
solutionPartD = sp.solve([eq1_partd, eq2_partd, eq3_partd], (theta1, theta2), check=False)

#The 4 different solutions for theta1 and theta2 in rad
print("The solutions for theta1 and theta2 to satisfy k are given here in radians.")
print("Each row is one of four possible solutions. Columns 1 and 2 represent theta1 and theta2 respectively.")
solutionPartD

The solutions for theta1 and theta2 to satisfy k are given here in radians.
Each row is one of four possible solutions. Columns 1 and 2 represent theta1 and theta2 respectively.

[(-0.785398163397448, 5.49778714378214),
 (0.785398163397448, 0.785398163397448),
 (2.35619449019234, 0.785398163397448),
 (3.92699081698724, 5.49778714378214)]

#convert theta1 and theta2 into degrees
solutionPartD_list = []
for a in solutionPartD:
    solutionPartD_list.append(a)
solutionPartD_deg = np.rad2deg(np.array(solutionPartD_list, dtype=float))

print("The same solutions from above in degrees:")
print("Each row is one of four possible solutions. Columns 1 and 2 represent theta1 and theta2 respectively.")
sp.Matrix(solutionPartD_deg)

The same solutions from above in degrees:
Each row is one of four possible solutions. Columns 1 and 2 represent theta1 and theta2 respectively.

$\displaystyle \left[\begin{matrix}-45.0 & 315.0\\45.0 & 45.0\\135.0 & 45.0\\225.0 & 315.0\end{matrix}\right]$

We are given 4 solutions here, lets check them all to see which ones make sense.

We will do this by plugging theta into the pose of frame c in respect to frame 0 and then comparing it to the orientation of k. If they are the same, then we have correctly found angles to make the end effector have that orientation.

correct_solutions = []
corresponding_theta = []
# Plug in solution to pose, check to see if the z part of the rotation matrix matches up with k
for theta_iterate in solutionPartD:
    solution_on_trial = T0c.subs({theta1: theta_iterate[0], theta2: theta_iterate[1], d1: 1.2, d2: 0.8, d3: 0.8})
    dif = solution_on_trial[:3,2] - k # solution_on_trial is the pose, index [:3,2] extracts z part of the rotation matrix
    # sp.pprint(solution_on_trial)
    # print()
    if dif[0] < 0.00001 and dif[1] < 0.00001 and dif[2] < 0.00001:
        correct_solutions.append(solution_on_trial) 
        corresponding_theta.append(theta_iterate)

print("\nCorrect solutions found:\n")
for a in range(len(correct_solutions)):
    print("Solution %d:" % (a+1))
    print("Theta1 and theta2:", end="\n\t")
    sp.pprint(corresponding_theta[a])
    print("Result in this pose:\n")
    sp.pprint(correct_solutions[a])
    print()

Correct solutions found:

Solution 1:
Theta1 and theta2:
	(-0.785398163397448, 5.49778714378214)
Result in this pose:

⎡0.707106781186547         0.5                 -0.5         -0.965685424949238⎤
⎢                                                                             ⎥
⎢0.707106781186548         -0.5                0.5          -0.165685424949238⎥
⎢                                                                             ⎥
⎢        0          -0.707106781186548  -0.707106781186547  0.634314575050762 ⎥
⎢                                                                             ⎥
⎣        0                  0                   0                   1         ⎦

Solution 2:
Theta1 and theta2:
	(2.35619449019234, 0.785398163397448)
Result in this pose:

⎡-0.707106781186548        -0.5                -0.5         0.165685424949238⎤
⎢                                                                            ⎥
⎢-0.707106781186547         0.5                0.5          0.965685424949238⎥
⎢                                                                            ⎥
⎢        0           0.707106781186547  -0.707106781186548  0.634314575050762⎥
⎢                                                                            ⎥
⎣        0                   0                  0                   1        ⎦

We can see from the 4 solutions, only 2 correctly represent a pose that matches k. To narrow down only one solution, we would need position information.

*It is worth noting that if we were to set "check=True" instead of false in sympy's solve function, the solve function would have found the correct 2 solutions for us. However, due to issues we were having with the solve function when solving the inverse kinematics earlier, we opted to check the solutions ourselves.

Q1 Part E

(e) Repeat part (d) to find the value of $ \theta_1 $ and $ \theta_2 $ if the origin of frame $\{C\}$ is located at a point $ P $, whose coordinate vector is given in frame $\{0\}$ by

$$ p = 1.075x_0 + 0.303y_0 + 1.022z_0. $$

Remember from the Inverse Kinematics section that we found these functions:

$ \theta_1 = f({}^0x_c, {}^0y_c, {}^0z_c) $

$ \theta_2 = f({}^0x_c, {}^0y_c, {}^0z_c) $

In the following code, we will be plugging $ p $ into $ {}^0x_c, {}^0y_c, $ and $ {}^0z_c $ and figuring out what $ \theta_1 $ and $ \theta_2 $ could be.

It should be noted that most of the work for this question has been done in the "Inverse Kinematics Section for Position" earlier.

theta_on_trial_dict = {
    d1: 1.2, 
    d2: 0.8, 
    d3: 0.8,
    xc: 1.075,
    yc: 0.303,
    zc: 1.022
}

# Plug p into solutions_for_theta to get possible outputs of theta
# Remember solutions_for_theta was found in an earlier section.
theta_options_partE = []
for a in range(len(solutions_for_theta)):
    temp_list = []
    for b in range(len(solutions_for_theta[0])):
        theta_current_value = solutions_for_theta[a][b].subs(theta_on_trial_dict)
        temp_list.append(theta_current_value)
    theta_options_partE.append(tuple(temp_list))

print("All possible solution pairs for theta that correspond to point p in radians.")
print("Each row is one of four possible solutions. Columns 1 and 2 represent theta1 and theta2 respectively.")

theta_options_partE

All possible solution pairs for theta that correspond to point p in radians.
Each row is one of four possible solutions. Columns 1 and 2 represent theta1 and theta2 respectively.

[(1.07272370972679, 1.34641832379787),
 (-1.07272370972679, -1.34641832379787 + 2*pi),
 (-2.61813591673788, 1.34641832379787),
 (2.61813591673788, -1.34641832379787 + 2*pi)]

Now we need to check which solutions make sense. Lets plug in these theta values into the pose of frame c in resepect to frame 0 and see if we get the correct position after.

p = sp.Matrix([1.075, 0.303, 1.022])

# Plug in theta to pose, check to see if the position of the pose matches up with p
correct_solutions = []
corresponding_theta = []
for theta_iterate in theta_options_partE:
    pose_on_trial = T0c.subs({theta1: theta_iterate[0], theta2: theta_iterate[1], d1: 1.2, d2: 0.8, d3: 0.8})
    dif = p - pose_on_trial[:3,3] # pose_on_trial is the pose, index [:3,3] extracts the position
    if dif[0] < 0.00001 and dif[1] < 0.00001 and dif[2] < 0.00001:
        correct_solutions.append(pose_on_trial) 
        corresponding_theta.append(theta_iterate)

print("\n-- Correct solutions found: --\n")
for a in range(len(correct_solutions)):
    print("Solution %d:" % (a+1))
    print("Theta1 and theta2 in rad:", end="\n\t")
    sp.pprint(corresponding_theta[a])
    print("Result in this pose:\n")
    sp.pprint(correct_solutions[a])
    print()

-- Correct solutions found: --

Solution 1:
Theta1 and theta2 in rad:
	(1.07272370972679, 1.34641832379787)
Result in this pose:

⎡-0.878504967894967  0.106295639561954  0.465757724996567  1.07541015431323⎤
⎢                                                                          ⎥
⎢0.477733211514401   0.19546735535663   0.856483211514401       0.303      ⎥
⎢                                                                          ⎥
⎢        0           0.974932689984288       -0.2225            1.022      ⎥
⎢                                                                          ⎥
⎣        0                   0                  0                 1        ⎦

Solution 2:
Theta1 and theta2 in rad:
	(2.61813591673788, -1.34641832379787 + 2⋅π)
Result in this pose:

⎡-0.499876985793974  -0.192706452208752   0.84438570709756   1.07541015431323⎤
⎢                                                                            ⎥
⎢-0.866096414421357  0.111222629339159   -0.487346414421357       0.303      ⎥
⎢                                                                            ⎥
⎢        0           -0.974932689984288       -0.2225             1.022      ⎥
⎢                                                                            ⎥
⎣        0                   0                   0                  1        ⎦

We can see from the 4 solutions, only 2 correctly represent a pose that matches p. To narrow down only one solution, we would need orientation information.

Question 2 (20 points)

Resolve the velocity-level forward and inverse kinematics of the Furuta pendulum and answer the following questions:

(a) [5 points]

At the pose given in part (a) of Question 1, if the joint velocities are given by

$$ \dot{\theta} = \begin{bmatrix} 1 \\ 2 \end{bmatrix}^{\top} $$

what are the body and spatial twists (angular and linear) velocity of the end-effector?

(b) [5 points]

At the same pose in part (a), what are the body and spatial twists of frame {2}?

(c) [5 points]

At the same pose in part (a), if the end-effector frame {C} is moving with a body twist given by

$$ {}^{c}V_{c} = (\omega_c, v_c) = \begin{bmatrix} -1.2 \\ 0.487 \\ 0.111 \\ 0.39 \\ 0.871 \\ 0.39 \end{bmatrix} $$

what are the joint rates?

(d) [5 points]

Repeat part (c) for the case where the spatial twist of the end-effector is given by

$$ {}^{0}V_{c} = (\omega_c, v_c) = \begin{bmatrix} -0.35 \\ -0.606 \\ 0.5 \\ -0.044 \\ 0.475 \\ 0.546 \end{bmatrix} $$

Forward Velocity Kinematics - Q2

We will be solving the forward velocity kinematics for frame c in respect to frame 0

Compute the spatial twist first

# Compute Spatial Twist of Frame c in frame 0

theta1_dot = sp.symbols(r"\dot{\theta_1}")
theta2_dot = sp.symbols(r"\dot{\theta_2}")
time = sp.symbols(r"t")

# Given angular velocities
w01_0 = sp.Matrix([0,0,theta1_dot]) # read as omega of frame 1 in respect to frame 0 expressed in 0
w12_1 = sp.Matrix([0,0,theta2_dot])
w2c_2 = sp.Matrix([0,0,0])

# Construct spatial angular velocity in order to find R0c_dot_spatial
# Each angular velocity needs to be expressed in frame 0
w12_0 = sp.Matrix(R01.A) @ w12_1
w2c_0 = sp.Matrix(R01.A) @ sp.Matrix(R12.A) @ w2c_2
w0c_0 = w01_0 + w12_0 + w2c_0                 # This is the spatial angular velocity of frame c 
w0c_0_skew = sp.Matrix(skew(np.array(w0c_0))) # this is the skew of that angular velocity

# FIND R0c_dot_spatial using angular velocity spatial and R0c
R0c_dot_spatial = sp.simplify(w0c_0_skew @ R0c)

# this is the scuffed code to make the derivative work, without this all the thetas will calculate as theta dot. 
theta1_function_of_time = theta1_dot * time
theta2_function_of_time = theta2_dot * time

# Find t0c_dot by taking the derivative of t assuming the thetas are functions of time
t0c_dot_spatial = sp.diff(t0c.subs({theta1: theta1_function_of_time, theta2: theta2_function_of_time}), time).subs({theta1_function_of_time: theta1, theta2_function_of_time: theta2})

# Construct the transformation dot by plugging it all in
T0c_dot_spatial = sp.Matrix.hstack(sp.Matrix.vstack(R0c_dot_spatial, sp.Matrix([0,0,0]).transpose()), sp.Matrix.vstack(t0c_dot_spatial, sp.Matrix([0])))#smush the r dot matrix and the t dot matrix together into the tranformation matrix derivative

# Construct the spatial twist
twist_0c_skew_spatial = sp.simplify(T0c_dot_spatial @ (T0c.inv()))
twist_0c_spatial = sp.Matrix(vexa(np.array(twist_0c_skew_spatial)))
print("This is the spatial twist of frame c with respect of frame 0.\nv is on top, omega is on bottom")  
twist_0c_spatial

This is the spatial twist of frame c with respect of frame 0.
v is on top, omega is on bottom

$\displaystyle \left[\begin{matrix}\dot{\theta_2} d_{1} \cos{\left(\theta_{1} \right)}\\\dot{\theta_2} d_{1} \sin{\left(\theta_{1} \right)}\\0\\\dot{\theta_2} \sin{\left(\theta_{1} \right)}\\- \dot{\theta_2} \cos{\left(\theta_{1} \right)}\\\dot{\theta_1}\end{matrix}\right]$

Now compute the body twist the same way

# Construct body angular velocity in order to find R0c_dot_body
# Each angular velocity needs to be expressed in frame c
w01_c = R0c.transpose() @ w01_0
w12_c = sp.Matrix(R2c.A).transpose() @ sp.Matrix((R12.A)).transpose() @ w12_1
w2c_c = sp.Matrix(R2c.A).transpose() @ w2c_2

w0c_c = w01_c + w12_c + w2c_c                 # This is the body angular velocity of frame c 
w0c_c_skew = sp.Matrix(skew(np.array(w0c_c))) # this is the skew of that angular velocity

# FIND R0c_dot_body using angular velocity body and R0c
R0c_dot_body = sp.simplify(R0c @ w0c_c_skew)

# this is the scuffed code to make the derivative work, without this all the thetas will calculate as theta dot. 
theta1_function_of_time = theta1_dot * time
theta2_function_of_time = theta2_dot * time

# Find t0c_dot by taking the derivative of t assuming the thetas are functions of time
t0c_dot_body = sp.diff(t0c.subs({theta1: theta1_function_of_time, theta2: theta2_function_of_time}), time).subs({theta1_function_of_time: theta1, theta2_function_of_time: theta2})

# Construct the transformation dot by plugging it all in
T0c_dot_body = sp.Matrix.hstack(sp.Matrix.vstack(R0c_dot_body, sp.Matrix([0,0,0]).transpose()), sp.Matrix.vstack(t0c_dot_body, sp.Matrix([0])))#smush the r dot matrix and the t dot matrix together into the tranformation matrix derivative

# Construct the body twist
twist_0c_skew_body = sp.simplify((T0c.inv()) @ T0c_dot_body )
twist_0c_body = sp.Matrix(vexa(np.array(twist_0c_skew_body)))
print("This is the body twist of frame c with respect of frame 0.\nv is on top, omega is on bottom")  
twist_0c_body

This is the body twist of frame c with respect of frame 0.
v is on top, omega is on bottom

$\displaystyle \left[\begin{matrix}\dot{\theta_1} d_{3} \sin{\left(\theta_{2} \right)}\\\dot{\theta_1} d_{2} \cos{\left(\theta_{2} \right)} + \dot{\theta_2} d_{3}\\\dot{\theta_1} d_{2} \sin{\left(\theta_{2} \right)}\\- \dot{\theta_2}\\\dot{\theta_1} \sin{\left(\theta_{2} \right)}\\- \dot{\theta_1} \cos{\left(\theta_{2} \right)}\end{matrix}\right]$

Verify with ad-joint

# find body twist using the spatial twist and an adjoint transformation
print("Body twist constructed from ad-joint:")
sp.simplify(sp.Matrix(tr2adjoint(np.array(T0c.inv()))) @ twist_0c_spatial)

Body twist constructed from ad-joint:

$\displaystyle \left[\begin{matrix}\dot{\theta_1} d_{3} \sin{\left(\theta_{2} \right)}\\\dot{\theta_1} d_{2} \cos{\left(\theta_{2} \right)} + \dot{\theta_2} d_{3}\\\dot{\theta_1} d_{2} \sin{\left(\theta_{2} \right)}\\- \dot{\theta_2}\\\dot{\theta_1} \sin{\left(\theta_{2} \right)}\\- \dot{\theta_1} \cos{\left(\theta_{2} \right)}\end{matrix}\right]$

As can be seen, the body twist calculated is the same as the body twist constructed from the ad-joint

Solve Inverse Kinematics Given a Twist - Q2

In this section, we will be solving for $\dot{\theta}_1$ and $\dot{\theta}_2$, the joint velocities of the Furuta pendulum, given a desired end-effector twist. We will solve for the spatial and body.

Solve Spatial Twist Inverse Kinematics

Solve for $ \dot{\theta}_1 $ and $ \dot{\theta}_2 $

$$ {}^0V_c^0 = \left[ \begin{array}{c} \dot{\theta_2} d_{1} \cos{\left(\theta_{1} \right)} \\ \dot{\theta_2} d_{1} \sin{\left(\theta_{1} \right)} \\ 0 \\ \dot{\theta_2} \sin{\left(\theta_{1} \right)} \\ - \dot{\theta_2} \cos{\left(\theta_{1} \right)} \\ \dot{\theta_1} \end{array} \right] = \left[ \begin{array}{c} {}^0_xv_c^0 \\ {}^0_yv_c^0 \\ {}^0_zv_c^0 \\ {}^0_xw_c^0 \\ {}^0_yw_c^0 \\ {}^0_zw_c^0 \end{array} \right] $$

Note that ${}^0_xv_c^0$ represents the x part of the linear velocity of frame c in respect to frame 0 represented in frame 0 and ${}^0_xw_c^0$ represents the same thing for angular velocity.

v_x_spatial, v_y_spatial, v_z_spatial, w_x_spatial, w_y_spatial, w_z_spatial = sp.symbols(
    '{}^0_xv_c^0 {}^0_yv_c^0 {}^0_zv_c^0 {}^0_xw_c^0 {}^0_yw_c^0 {}^0_zw_c^0', real=True
)
twist_spatial_components = sp.Matrix([
    v_x_spatial,
    v_y_spatial,
    v_z_spatial,
    w_x_spatial,
    w_y_spatial,
    w_z_spatial
])

# Set up each equality individually.
eq1 = sp.Eq(twist_0c_spatial[0], twist_spatial_components[0])
eq2 = sp.Eq(twist_0c_spatial[1], twist_spatial_components[1])
eq3 = sp.Eq(twist_0c_spatial[2], twist_spatial_components[2])
eq4 = sp.Eq(twist_0c_spatial[3], twist_spatial_components[3])
eq5 = sp.Eq(twist_0c_spatial[4], twist_spatial_components[4])
eq6 = sp.Eq(twist_0c_spatial[5], twist_spatial_components[5])

# - Put solutions into a list 
# - This has to be done because sympy's solve function does like 6 equations and 2 unknowns
# look at the 6th equation to solve for theta1_dot since it is the only equation with theta1_dot
theta1_dot_spatial_solutions = []
theta1_dot_spatial_solutions.append(sp.solve(eq6, theta1_dot)[0])

# look at the equations 1,2,4, and 5 to solve for theta2_dot since they are the only equations with theta2_dot
# Store the solutions in a list. Will need to be carful about how each one is called depending on what is given later. 
theta2_dot_spatial_solutions = []
theta2_dot_spatial_solutions.append(sp.solve(eq1, theta2_dot)[0])
theta2_dot_spatial_solutions.append(sp.solve(eq2, theta2_dot)[0])
theta2_dot_spatial_solutions.append(sp.solve(eq4, theta2_dot)[0])
theta2_dot_spatial_solutions.append(sp.solve(eq5, theta2_dot)[0])

Joint Velocities $ \dot{\theta}_1 $ and $ \dot{\theta}_2 $ as functions of the spatial twist

From the symbolic solution, we find:

• The joint velocity $ \dot{\theta}_1 $ is uniquely determined by the spatial twist’s $ z $-angular component:

$$ \dot{\theta}_1 = {}^0_z \omega_c^0. $$

• The joint velocity $ \dot{\theta}_2 $ can take multiple equivalent forms:

$$ \dot{\theta}_2 = \begin{cases} \displaystyle \frac{{}^0_x v_c^0}{d_1 \cos(\theta_1)}, & \text{(from equation 1)} \\[8pt] \displaystyle \frac{{}^0_y v_c^0}{d_1 \sin(\theta_1)}, & \text{(from equation 2)} \\[8pt] \displaystyle \frac{{}^0_x \omega_c^0}{\sin(\theta_1)}, & \text{(from equation 4)} \\[8pt] \displaystyle -\,\frac{{}^0_y \omega_c^0}{\cos(\theta_1)}. & \text{(from equation 5)} \end{cases} $$

In summary, $\dot{\theta}_1$ is directly determined by the $z$ -angular component of the spatial twist, whereas $\dot{\theta}_2$ has multiple possible expressions depending on different parts of the spatial twist.

Solve Body Twist Inverse Kinematics

Solve for $ \dot{\theta}_1 $ and $ \dot{\theta}_2 $ $$ {}^cV_c^0 = \left[ \begin{array}{c} \dot{\theta_1} d_{3} \sin{\left(\theta_{2} \right)} \\ \dot{\theta_1} d_{2} \cos{\left(\theta_{2} \right)} + \dot{\theta_2} d_{3} \\ \dot{\theta_1} d_{2} \sin{\left(\theta_{2} \right)} \\ - \dot{\theta_2} \\ \dot{\theta_1} \sin{\left(\theta_{2} \right)} \\ - \dot{\theta_1} \cos{\left(\theta_{2} \right)} \end{array} \right] = \left[ \begin{array}{c} {}^c_xv_c^0 \\ {}^c_yv_c^0 \\ {}^c_zv_c^0 \\ {}^c_xw_c^0 \\ {}^c_yw_c^0 \\ {}^c_zw_c^0 \end{array} \right] $$

v_x_body, v_y_body, v_z_body, w_x_body, w_y_body, w_z_body = sp.symbols(
    '{}^c_xv_c^0 {}^c_yv_c^0 {}^c_zv_c^0 {}^c_xw_c^0 {}^c_yw_c^0 {}^c_zw_c^0', real=True
)

twist_body_components = sp.Matrix([
    v_x_body,
    v_y_body,
    v_z_body,
    w_x_body,
    w_y_body,
    w_z_body
])


eq1 = sp.Eq(twist_0c_body[0], twist_body_components[0])
eq2 = sp.Eq(twist_0c_body[1], twist_body_components[1])
eq3 = sp.Eq(twist_0c_body[2], twist_body_components[2])
eq4 = sp.Eq(twist_0c_body[3], twist_body_components[3])
eq5 = sp.Eq(twist_0c_body[4], twist_body_components[4])
eq6 = sp.Eq(twist_0c_body[5], twist_body_components[5])

# look at the 4th equation to solve for theta2_dot since it is the only equation with theta2_dot by itself
theta2_dot_body_solutions = []
theta2_dot_body_solutions.append(sp.solve(eq4, theta2_dot)[0])

# look at the equations 1,3,5, and 6 to solve for theta1_dot since they are the only equations with theta1_dot by themselves
# Store the solutions in a list. Will need to be carful about how each one is called depending on what is given later. 
theta1_dot_body_solutions = []
theta1_dot_body_solutions.append(sp.solve(eq1, theta1_dot)[0])
theta1_dot_body_solutions.append(sp.solve(eq3, theta1_dot)[0])
theta1_dot_body_solutions.append(sp.solve(eq5, theta1_dot)[0])
theta1_dot_body_solutions.append(sp.solve(eq6, theta1_dot)[0])

# solve equation 2 by plugging in the solution for theta2_dot
theta1_dot_body_solutions.append((sp.solve(eq2, theta1_dot)[0]).subs({theta2_dot: theta2_dot_body_solutions[0]}))

Joint Velocities $ \dot{\theta}_1 $ and $ \dot{\theta}_2 $ as functions of the body twist

From the symbolic solution, we find:

• The joint velocity $ \dot{\theta}_2 $ is uniquely determined by the body twist’s $ x $-angular component:

$$ \dot{\theta}_2 = -{}^c_x \omega_c^0. $$

• The joint velocity $ \dot{\theta}_1 $ can take multiple equivalent forms:

$$ \dot{\theta}_1 = \begin{cases} \displaystyle \frac{{}^c_x v_c^0}{d_3 \sin(\theta_2)}, & \text{(from equation 1)} \\[8pt] \displaystyle \frac{{}^c_z v_c^0}{d_2 \sin(\theta_2)}, & \text{(from equation 3)} \\[8pt] \displaystyle \frac{{}^c_y \omega_c^0}{\sin(\theta_2)}, & \text{(from equation 5)} \\[8pt] \displaystyle -\,\frac{{}^c_z \omega_c^0}{\cos(\theta_2)}, & \text{(from equation 6)} \\[8pt] \displaystyle \frac{d_3 {}^c_x \omega_c^0 + {}^c_y v_c^0}{d_2 \cos(\theta_2)}. & \text{(from equation 2, after substituting $ \dot{\theta}_2 $)} \end{cases} $$

In summary, $ \dot{\theta}_2 $ is directly determined by the body twist’s $ x $-angular component, whereas $ \dot{\theta}_1 $ has multiple possible expressions.

Q2 Part A

(a) At the pose given in part (a) of Question 1, if the joint velocities are given by

$$ \dot{\boldsymbol{\theta}} = \begin{bmatrix} 1 & 2 \end{bmatrix}^{\top} $$

what are the body and spatial twists (angular and linear velocity) of the end-effector?

# plug in given values
subs_Q2PA= {d1: 1.2, d2: 0.8, d3: 0.8, theta1: 5 * sp.pi /6, theta2: -3 * sp.pi / 7, theta1_dot: 1, theta2_dot: 2}

print("The Spatial Twist of frame c in respect to frame 0 (v,w): ")
twist_0c_spatial.subs(subs_Q2PA).evalf()

The Spatial Twist of frame c in respect to frame 0 (v,w): 

$\displaystyle \left[\begin{matrix}-2.07846096908265\\1.2\\0\\1.0\\1.73205080756888\\1.0\end{matrix}\right]$

print("The Body Twist of frame c in respect to frame 0 (v,w): ")
twist_0c_body.subs(subs_Q2PA).evalf()

The Body Twist of frame c in respect to frame 0 (v,w): 

$\displaystyle \left[\begin{matrix}-0.779942329745459\\1.77801674716505\\-0.779942329745459\\-2.0\\-0.974927912181824\\-0.222520933956314\end{matrix}\right]$

Q2 Part B

(b) At the same pose in part (a), what are the body and spatial twists of frame {2}?

We will start by solving the forward kinematics of frame 2 in respect to frame 0 Then we will plug in the pose and find the twist!

# Find the spatial twist from frame 0 to 2 

# Construct spatial angular velocity in order to find R02_dot_spatial
w02_0 = w01_0 + w12_0                         # This is the spatial angular velocity of frame 2 in respect to 0
w02_0_skew = sp.Matrix(skew(np.array(w02_0))) # this is the skew of that angular velocity

# Extract rotation matrix of frame 2 in respect to frame 0
R02 = T02[:3,:3]
R02 = sp.Matrix(R02)

# FIND R02_dot_spatial using angular velocity spatial and R02
R02_dot_spatial = sp.simplify(w02_0_skew @ sp.Matrix(R02))

# Extract position matrix of frame 2 in respect to frame 0
t02 = T02[:3,3]

# Find t02_dot by taking the derivative of t assuming the thetas are functions of time
t02_dot_spatial = sp.diff(t02.subs({theta1: theta1_function_of_time, theta2: theta2_function_of_time}), time).subs({theta1_function_of_time: theta1, theta2_function_of_time: theta2})

# Construct the transformation dot by plugging it all in
T02_dot_spatial = sp.Matrix.hstack(sp.Matrix.vstack(R02_dot_spatial, sp.Matrix([0,0,0]).transpose()), sp.Matrix.vstack(t02_dot_spatial, sp.Matrix([0]))) #smush the r dot matrix and the t dot matrix together into the transformation matrix derivative

# Construct the spatial twist
twist_02_skew_spatial = sp.simplify(T02_dot_spatial @ (T02.inv()))
twist_02_spatial = sp.Matrix(vexa(np.array(twist_02_skew_spatial)))
print("This is the spatial twist of frame 2 with respect of frame 0. (v,w)")  
twist_02_spatial

This is the spatial twist of frame 2 with respect of frame 0. (v,w)

$\displaystyle \left[\begin{matrix}\dot{\theta_2} d_{1} \cos{\left(\theta_{1} \right)}\\\dot{\theta_2} d_{1} \sin{\left(\theta_{1} \right)}\\0\\\dot{\theta_2} \sin{\left(\theta_{1} \right)}\\- \dot{\theta_2} \cos{\left(\theta_{1} \right)}\\\dot{\theta_1}\end{matrix}\right]$

# Find the body twist from frame 0 to 2 

# Construct body angular velocity in order to find R02_dot_body
# Each angular velocity needs to be expressed in frame 2
w01_2 = R02.transpose() @ w01_0
w12_2 = sp.Matrix(R12.A).transpose() @ w12_1

w02_2 = w01_2 + w12_2                         # This is the body angular velocity of frame 2 
w02_2_skew = sp.Matrix(skew(np.array(w02_2))) # this is the skew of that angular velocity

# FIND R0c_dot_body using angular velocity body and R0c
R02_dot_body = sp.simplify(R02 @ w02_2_skew)

# Find t02_dot by taking the derivative of t assuming the thetas are functions of time
t02_dot_body = sp.diff(t02.subs({theta1: theta1_function_of_time, theta2: theta2_function_of_time}), time).subs({theta1_function_of_time: theta1, theta2_function_of_time: theta2})

# Construct the transformation dot by plugging it all in
T02_dot_body = sp.Matrix.hstack(sp.Matrix.vstack(R02_dot_body, sp.Matrix([0,0,0]).transpose()), sp.Matrix.vstack(t02_dot_body, sp.Matrix([0]))) #smush the r dot matrix and the t dot matrix together into the tranformation matrix derivative

# Construct the body twist
twist_02_skew_body = sp.simplify((T02.inv()) @ T02_dot_body )
twist_02_body = sp.Matrix(vexa(np.array(twist_02_skew_body)))
print("This is the body twist of frame 2 with respect of frame 0. (v,w)")  
twist_02_body

This is the body twist of frame 2 with respect of frame 0. (v,w)

$\displaystyle \left[\begin{matrix}\dot{\theta_1} d_{2} \cos{\left(\theta_{2} \right)}\\0\\\dot{\theta_1} d_{2} \sin{\left(\theta_{2} \right)}\\\dot{\theta_1} \sin{\left(\theta_{2} \right)}\\\dot{\theta_2}\\- \dot{\theta_1} \cos{\left(\theta_{2} \right)}\end{matrix}\right]$

# Verify these with the Adjoint
print("The body velocity according to the adjoint (v,w)")
sp.simplify(sp.Matrix(tr2adjoint(np.array(T02.inv()))) @ twist_02_spatial)

The body velocity according to the adjoint (v,w)

$\displaystyle \left[\begin{matrix}\dot{\theta_1} d_{2} \cos{\left(\theta_{2} \right)}\\0\\\dot{\theta_1} d_{2} \sin{\left(\theta_{2} \right)}\\\dot{\theta_1} \sin{\left(\theta_{2} \right)}\\\dot{\theta_2}\\- \dot{\theta_1} \cos{\left(\theta_{2} \right)}\end{matrix}\right]$

subs_Q2Pb= {d1: 1.2, d2: 0.8, d3: 0.8, theta1: 5 * sp.pi /6, theta2: -3 * sp.pi / 7, theta1_dot: 1, theta2_dot: 2}

print("The Spatial Twist of frame 2 in respect to frame 0 with the given values (v,w): ")
twist_02_spatial.subs(subs_Q2Pb).evalf()

The Spatial Twist of frame 2 in respect to frame 0 with the given values (v,w): 

$\displaystyle \left[\begin{matrix}-2.07846096908265\\1.2\\0\\1.0\\1.73205080756888\\1.0\end{matrix}\right]$

print("The Body Twist of frame 2 in respect to frame 0 with the given values (v,w): ")
twist_02_body.subs(subs_Q2Pb).evalf()

The Body Twist of frame 2 in respect to frame 0 with the given values (v,w): 

$\displaystyle \left[\begin{matrix}0.178016747165052\\0\\-0.779942329745459\\-0.974927912181824\\2.0\\-0.222520933956314\end{matrix}\right]$

Q2 Part C

(c) At the same pose in part (a), if the end-effector frame $ \{C\} $ is moving with a body twist given by

$$ {}^cV_c^0 = (\mathbf{v}_c, \boldsymbol{\omega}_c) = \begin{bmatrix} 0.39 & 0.871 & 0.39 & -1.2 & 0.487 & 0.111 \end{bmatrix}^{\top} $$

what are the joint rates?

Note that v and ${\omega}$ have been swapped from the instructions in order to fit our notation

body_subs = {
    v_x_body: 0.39,
    v_y_body: 0.871,
    v_z_body: 0.39,
    w_x_body: -1.2,
    w_y_body: 0.487,
    w_z_body: 0.111,
    d1: 1.2,
    d2: 0.8,
    d3: 0.8,
    theta1: 5 * sp.pi / 6,
    theta2: -3*sp.pi / 7
}

print("Theta1_dot calculated from all formulas found (rad/s):")
print(sp.N(theta1_dot_body_solutions[0].subs(body_subs)))
print(sp.N(theta1_dot_body_solutions[1].subs(body_subs)))
print(sp.N(theta1_dot_body_solutions[2].subs(body_subs)))
print(sp.N(theta1_dot_body_solutions[3].subs(body_subs)))
print(sp.N(theta1_dot_body_solutions[4].subs(body_subs)))

print("\nTheta2_dot calculated from the 1 formula found (rad/s):")
print(sp.N(theta2_dot_body_solutions[0].subs(body_subs)))

Theta1_dot calculated from all formulas found (rad/s):
-0.500036970845370
-0.500036970845370
-0.499524112413734
-0.498829472025278
-0.499952961827136

Theta2_dot calculated from the 1 formula found (rad/s):
1.20000000000000

Part c Answer:

The joint rates found are:

$$ \dot{\theta}_1 = -0.5 \text{ rad/s}, \quad \dot{\theta}_2 = 1.2 \text{ rad/s} $$

Q2 Part D

(d) Repeat part (c) for the case where the spatial twist of the end-effector is given by

$$ {}^0V_c^0 = (\mathbf{v}_c, \boldsymbol{\omega}_c) = \begin{bmatrix} -1.247 & 0.72 & 0 & 0.6 & 1.039 & -0.5 \end{bmatrix}^{\top} $$ Note that v and ${\omega}$ have been swapped from the instructions in order to fit our notation

spatial_subs = {
    v_x_spatial: -1.247,
    v_y_spatial: 0.72,
    v_z_spatial: 0.0,
    w_x_spatial: 0.6,
    w_y_spatial: 1.039,
    w_z_spatial: -0.5,
    d1: 1.2,
    d2: 0.8,
    d3: 0.8,
    theta1: 5 * sp.pi / 6,
    theta2: -3*sp.pi / 7
}

print("Theta1_dot calculated from the 1 formula found (rad/s):")
print(sp.N(theta1_dot_spatial_solutions[0].subs(spatial_subs)))

print("\nTheta2_dot calculated from all the formulas found (rad/s):")
print(sp.N(theta2_dot_spatial_solutions[0].subs(spatial_subs)))
print(sp.N(theta2_dot_spatial_solutions[1].subs(spatial_subs)))
print(sp.N(theta2_dot_spatial_solutions[3].subs(spatial_subs)))

Theta1_dot calculated from the 1 formula found (rad/s):
-0.500000000000000

Theta2_dot calculated from all the formulas found (rad/s):
1.19992630946577
1.20000000000000
1.19973385937604

Part D Answer:

The joint rates found are:

$$ \dot{\theta}_1 = -0.5 \text{ rad/s}, \quad \dot{\theta}_2 = 1.2 \text{ rad/s} $$

Question 2 Part C and D Conclusion

Both given twists in both questions resulted in the same angular rates $$ \dot{\theta}_1 = -0.5 \text{ rad/s}, \quad \dot{\theta}_2 = 1.2 \text{ rad/s} $$