Question 3¶

Implement the Furuta pendulum in Mujoco and use its utility functions to verify all the kinematic equations you derived in Questions 1 and 2.

In [47]:
# Setup
import mujoco as mj
import numpy as np
import sympy as sp

# Load the XML model
model = mj.MjModel.from_xml_path(r"FurturaPendulum.xml")
data = mj.MjData(model)

joint1_name = "joint1"
joint2_name = "joint2"

joint1_id = model.joint(joint1_name).id
joint2_id = model.joint(joint2_name).id

frame1_name = "frame1"
frame1_id = model.body(name=frame1_name).id

frame2_name = "frame2"
frame2_id = model.body(name=frame2_name).id

end_effector_body_name="end-effector"
end_effector_body_id = model.body(name=end_effector_body_name).id

Helper Functions¶

In [48]:
def get_pose(body_id):
    #Extract the positions
    rot = data.xmat[body_id].reshape(3,3)
    pos = data.xpos[body_id]


    #Convert to transformation matrix
    transformation = np.eye(4) 
    transformation[:3, :3] = rot  
    transformation[:3, 3] = pos 
    return transformation

Verification of Answers to Problems 1¶

Q1 Part A, B, and C¶

We were given joint angles $\theta$:

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

We calculated these poses given those angles: $$ {}^0T_C = \begin{bmatrix} -0.5 & -0.1927 & 0.8443 & 1.0754 \\ -0.8660& 0.1113 & -0.4875& 0.3028 \\ 0 & -0.9749 & -0.2225& 1.0220 \\ 0 & 0 & 0 & 1.0 \end{bmatrix} $$ and $$ {}^0T_2 = \begin{bmatrix} -0.1927 & 0.5 & 0.8443 & 0.4 \\ 0.1113 & 0.8660 & -0.4875 & 0.6928 \\ -0.9749 & 0 & -0.2225 & 1.2 \\ 0 & 0 & 0 & 1.0 \end{bmatrix} $$

Now, using the Mujoco we can plug in these $\theta$ values and see if our simulated poses will be the same.

In [49]:
theta1_parta = 5 * np.pi / 6
theta2_parta = -3 * np.pi / 7

#set model to the given theta values
data.qpos[joint1_id] = theta1_parta
data.qpos[joint2_id] = theta2_parta

mj.mj_forward(model, data)

#Display
print("Resulting Pose of Frame c in respect to Frame 0:\n")
sp.pprint(sp.Matrix(np.round(get_pose(end_effector_body_id), 4)))

print("\nAs can be seen, this simulated result is the same as our calculated result.")

Resulting Pose of Frame c in respect to Frame 0:

⎡ -0.5   -0.1927  0.8443   1.0754⎤
⎢                                ⎥
⎢-0.866  0.1113   -0.4875  0.3028⎥
⎢                                ⎥
⎢ 0.0    -0.9749  -0.2225  1.022 ⎥
⎢                                ⎥
⎣ 0.0      0.0      0.0     1.0  ⎦

As can be seen, this simulated result is the same as our calculated result.
In [50]:
print("Resulting Pose of Frame 2 in respect to Frame 0:\n")
sp.pprint(sp.Matrix(np.round(get_pose(frame2_id), 4)))

print("\nAs can be seen, this simulated result is the same as our calculated result.")

Resulting Pose of Frame 2 in respect to Frame 0:

⎡-0.1927   0.5   0.8443    0.4  ⎤
⎢                               ⎥
⎢0.1113   0.866  -0.4875  0.6928⎥
⎢                               ⎥
⎢-0.9749   0.0   -0.2225   1.2  ⎥
⎢                               ⎥
⎣  0.0     0.0     0.0     1.0  ⎦

As can be seen, this simulated result is the same as our calculated result.

Having seen that our simulated pose $ {}^0T_c $ and $ {}^0T_2 $ is the same as our calculated pose in question 1, the resulting quaternion and axis angle representations of the rotation matrixes are obviously the same as well.

Q1 Part D¶

We were asked to find the values of $\theta$ corresponding to a given orientation of the camera frame.

We were given the following orientation which is the z part of the rotation matrix: $$ k = -\frac{1}{2} x_0 + \frac{1}{2} y_0 - \frac{\sqrt{2}}{2} z_0. $$

We found two sets of solutions for $\theta$ that result in this orientation:

Solution 1: $$ (\theta_1, \theta_2) = (-0.7854, 5.4978) $$

Resulted in this pose: $$ \begin{bmatrix} 0.7071 & 0.5 & -0.5 & -0.9657 \\ 0.7071 & -0.5 & 0.5 & -0.1657 \\ 0 & -0.7071 & -0.7071 & 0.6343 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$

Solution 2: $$ (\theta_1, \theta_2) = (2.356, 0.7854) $$

Resulted in this pose: $$ \begin{bmatrix} -0.7071 & -0.5 & -0.5 & 0.1657 \\ -0.7071 & 0.5 & 0.5 & 0.9657 \\ 0 & 0.7071 & -0.7071 & 0.6343 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$

To verify this calculation we can plug in the values of theta that we found and see if the resulting simulated pose matches the calculated pose.

In [51]:
# Theta1 and theta2 solution optinos
solutions = [(-0.785398163397448, 5.49778714378214),
             (2.35619449019234, 0.785398163397448),]

a = 1
for solution in solutions:
    #set model to solution
    data.qpos[joint1_id] = solution[0]
    data.qpos[joint2_id] = solution[1]
    mj.mj_forward(model, data)
    
    print("Resulting Pose from Solution " + str(a) + ":\n")
    sp.pprint(sp.Matrix(np.round(get_pose(end_effector_body_id), 4)))
    print()

    k = data.xmat[end_effector_body_id].reshape(3,3)[:,2]
    print("Z Orientation extracted: \n")
    print(k)
    print("--------------")
    a+=1

print("As can be seen, this simulated result is the same as our calculated result.")

Resulting Pose from Solution 1:

⎡0.7071    0.5     -0.5    -0.9657⎤
⎢                                 ⎥
⎢0.7071   -0.5      0.5    -0.1657⎥
⎢                                 ⎥
⎢ 0.0    -0.7071  -0.7071  0.6343 ⎥
⎢                                 ⎥
⎣ 0.0      0.0      0.0      1.0  ⎦

Z Orientation extracted: 

[-0.5         0.5        -0.70710678]
--------------
Resulting Pose from Solution 2:

⎡-0.7071   -0.5    -0.5    0.1657⎤
⎢                                ⎥
⎢-0.7071   0.5      0.5    0.9657⎥
⎢                                ⎥
⎢  0.0    0.7071  -0.7071  0.6343⎥
⎢                                ⎥
⎣  0.0     0.0      0.0     1.0  ⎦

Z Orientation extracted: 

[-0.5         0.5        -0.70710678]
--------------
As can be seen, this simulated result is the same as our calculated result.

Q1 Part E¶

We were asked to find the values of theta corresponding to a given position of the camera frame.

We were given the following position which is the position part of the pose: $$ p = 1.075x_0 + 0.303y_0 + 1.022z_0. $$

We found two sets of solutions for $\theta$ that result in this position:

Solution 1: $$ (\theta_1, \theta_2) = (1.0727, 1.3464) $$

Resulted in this pose: $$ \begin{bmatrix} -0.8785 & 0.1063 & 0.4658 & 1.0754 \\ 0.4777 & 0.1955 & 0.8565 & 0.303 \\ 0 & 0.9749 & -0.2225 & 1.022 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$

Solution 2: $$ (\theta_1, \theta_2) = (2.6181, 4.9368) $$

Resulted in this pose: $$ \begin{bmatrix} -0.4999 & -0.1927 & 0.8444 & 1.0754 \\ -0.8661 & 0.1112 & -0.4873 & 0.303 \\ 0 & -0.9749 & -0.2225 & 1.022 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$

To verify this calculation we can plug in the values of theta that we found and see if the resulting simulated pose matches the calculated pose.

In [52]:
solutions = [
(1.07272370972679, 1.34641832379787),
(2.61813591673788, -1.34641832379787)]

a=1
for solution in solutions:
    #set model to solution
    data.qpos[joint1_id] = solution[0]
    data.qpos[joint2_id] = solution[1]
    mj.mj_forward(model, data)
    print("Resulting Pose from Solution " + str(a) + ":\n")
    sp.pprint(sp.Matrix(np.round(get_pose(end_effector_body_id), 4)))
    print()

    p = data.xpos[end_effector_body_id]
    print("Position extracted: \n")
    print(p)
    print("--------------")
    a+=1


print("As can be seen, this simulated result is the same as our calculated result.")

Resulting Pose from Solution 1:

⎡-0.8785  0.1063  0.4658   1.0754⎤
⎢                                ⎥
⎢0.4777   0.1955  0.8565   0.303 ⎥
⎢                                ⎥
⎢  0.0    0.9749  -0.2225  1.022 ⎥
⎢                                ⎥
⎣  0.0     0.0      0.0     1.0  ⎦

Position extracted: 

[1.07541015 0.303      1.022     ]
--------------
Resulting Pose from Solution 2:

⎡-0.4999  -0.1927  0.8444   1.0754⎤
⎢                                 ⎥
⎢-0.8661  0.1112   -0.4873  0.303 ⎥
⎢                                 ⎥
⎢  0.0    -0.9749  -0.2225  1.022 ⎥
⎢                                 ⎥
⎣  0.0      0.0      0.0     1.0  ⎦

Position extracted: 

[1.07541015 0.303      1.022     ]
--------------
As can be seen, this simulated result is the same as our calculated result.

Verification of Answers to Problem 2¶

Q2 Parts A and B¶

We were given some joint angles and velocities and asked to produce body and spacial twists for frame c and frame 2.

The joint angles $\theta$ were given by

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

The angular rates of change were also given:

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

The resulting twists were calculated:

$$ {}^0V_c^0 = \left[\begin{matrix}v\\\omega\end{matrix}\right] = \left[\begin{matrix}-2.0785\\1.2\\0\\1.0\\1.7321\\1.0\end{matrix}\right] , {}^cV_c^0 = \left[\begin{matrix}v\\\omega\end{matrix}\right] = \left[\begin{matrix}-0.7799\\1.7780\\-0.7799\\-2.0\\-0.9749\\-0.2225\end{matrix}\right] , {}^0V_2^0 = \left[\begin{matrix}v\\\omega\end{matrix}\right] = \left[\begin{matrix}-2.0785\\1.2\\0\\1.0\\1.7321\\1.0\end{matrix}\right] , {}^2V_2^0 = \left[\begin{matrix}v\\\omega\end{matrix}\right] = \left[\begin{matrix}0.1780\\0\\-0.7799\\-0.9749\\2.0\\-0.2225\end{matrix}\right] $$

We can verify this in Mujoco by plugging in the joint angles and velocities and seeing if the resulting simulated twist matches with our calculated twist.

In [53]:
mj.mj_resetData(model, data)

data.qpos[joint1_id] = theta1_parta
data.qpos[joint2_id] = theta2_parta

data.qvel[joint1_id] = 1.0
data.qvel[joint2_id] = 2.0

mj.mj_forward(model, data)


# Body twist: measured in the end-effector's (body) frame [linear; angular]
body_lin_vel = data.sensordata[0 : 3]
body_ang_vel = data.sensordata[3 : 6]
body_twist = np.concatenate([body_lin_vel, body_ang_vel])
print("\nSimulated Body Twist of frame c in respect to frame 0 (v,w):")
print(body_twist.round(4))


# find spatial twist using the body twist and an adjoint transformation
from spatialmath.base import tr2adjoint
spatial_twist = tr2adjoint(np.array(get_pose(end_effector_body_id))) @ body_twist

print("\nResulting Spatial Twist of frame c in respect to frame 0 (v,w):")
print(spatial_twist.round(4))

print("\nAs can be seen, the simulated twist is the same as the calculated twist.")

Simulated Body Twist of frame c in respect to frame 0 (v,w):
[-0.7799  1.778  -0.7799 -2.     -0.9749 -0.2225]

Resulting Spatial Twist of frame c in respect to frame 0 (v,w):
[-2.0785  1.2     0.      1.      1.7321  1.    ]

As can be seen, the simulated twist is the same as the calculated twist.
In [54]:
mj.mj_resetData(model, data)

data.qpos[joint1_id] = theta1_parta
data.qpos[joint2_id] = theta2_parta

data.qvel[joint1_id] = 1.0
data.qvel[joint2_id] = 2.0

mj.mj_forward(model, data)


# Body twist: measured in frame 2 body frame [linear; angular]
body_lin_vel = data.sensordata[9 : 12]
body_ang_vel = data.sensordata[12 : 15]
body_twist = np.concatenate([body_lin_vel, body_ang_vel])
print("\nSimulated Body Twist of frame 2 in respect to frame 0 (v,w):")
print(body_twist.round(4))


# find spatial twist using the body twist and an adjoint transformation
from spatialmath.base import tr2adjoint
spatial_twist = tr2adjoint(np.array(get_pose(frame2_id))) @ body_twist

print("\nResulting Spatial Twist of frame 2 in respect to frame 0 (v,w):")
print(spatial_twist.round(4))

print("\nAs can be seen, the simulated twist is the same as the calculated twist.")

Simulated Body Twist of frame 2 in respect to frame 0 (v,w):
[ 0.178   0.     -0.7799 -0.9749  2.     -0.2225]

Resulting Spatial Twist of frame 2 in respect to frame 0 (v,w):
[-2.0785  1.2     0.      1.      1.7321  1.    ]

As can be seen, the simulated twist is the same as the calculated twist.

Q2 Part C and D¶

We were given the following twists and asked to find the joint rates.

Given body twist: $$ {}^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} $$ Given spatial twist: $$ {}^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} $$

The joint rates we found for each twist ended up being the same: $$ \dot{\theta}_1 = -0.5 \text{ rad/s}, \quad \dot{\theta}_2 = 1.2 \text{ rad/s} $$

We can verify this in Mujoco by plugging in the joint rates we found and seeing if the resulting simulated twist matches with the given twist.

In [55]:
mj.mj_resetData(model, data)

data.qpos[joint1_id] = theta1_parta
data.qpos[joint2_id] = theta2_parta

# Plug in our calculated joint velocities
data.qvel[joint1_id] = -.5
data.qvel[joint2_id] = 1.2

mj.mj_forward(model, data)


# Body twist: measured in the end-effector's (body) frame [linear; angular]
body_lin_vel = data.sensordata[0 : 3]
body_ang_vel = data.sensordata[3 : 6]
body_twist = np.concatenate([body_lin_vel, body_ang_vel])
print("\nSimulated Body Twist of frame c in respect to frame 0 (v,w):")
print(body_twist.round(3))

# find spatial twist using the body twist and an adjoint transformation
from spatialmath.base import tr2adjoint
spatial_twist = tr2adjoint(np.array(get_pose(end_effector_body_id))) @ body_twist

print("\nResulting Spatial Twist of frame c in respect to frame 0 (v,w):")
print(spatial_twist.round(3))

print("\nAs can be seen, the simulated twist is the same as the expected twist given these joint angle velocities.")

Simulated Body Twist of frame c in respect to frame 0 (v,w):
[ 0.39   0.871  0.39  -1.2    0.487  0.111]

Resulting Spatial Twist of frame c in respect to frame 0 (v,w):
[-1.247  0.72   0.     0.6    1.039 -0.5  ]

As can be seen, the simulated twist is the same as the expected twist given these joint angle velocities.

Conclusion¶

All the kinematic equations calculated in question 1 and question 2 have been confirmed by simulating the Furuta Pendulum in Mujoco.