L4 Exercise Answers - OXBR Website

License: CC-BY-NC-SA 4.0

I found an issue¶

Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues

Latex Macros¶

$\providecommand{\myvec}[1]{{\mathbf{\boldsymbol{{#1}}}}}$ $\providecommand{\mymatrix}[1]{{\mathbf{\boldsymbol{{#1}}}}}$

\providecommand{\myvec}[1]{{\mathbf{\boldsymbol{{#1}}}}}
\providecommand{\mymatrix}[1]{{\mathbf{\boldsymbol{{#1}}}}}

Valid imports¶

from math import pi, sin, cos
import numpy as np

Exercises¶

Exercise a¶

First, we calculate FKM by hand. You’ll notice that it is given by the equation below.

\mymatrix{H}^{0}_{3} = \left[\begin{array}{ccc} c_{012} & -s_{012} & l_{0}c_0 + l_{1}c_{01} + l_{2}c_{012}\\ s_{012} & c_{012} & l_{0}s_0 + l_{1}s_{01} + l_{2}s_{012}\\ 0 & 0 & 1 \end{array}\right].

(1)

We don’t need to compute it explicitly, we just need the task-space values.

\begin{align} p_{x}&=l_{0}c_0 + l_{1}c_{01} + l_{2}c_{012} \\ p_{y}&=l_{0}s_0 + l_{1}s_{01} + l_{2}s_{012} \\ \phi&=q_0 + q_1 + q_2.\\ \end{align}

(2)

Then, take the derivative to find the Jacobian.

\mymatrix{J} = \left[\begin{array}{ccc} -l_{0}s_0 - l_{1}s_{01} - l_{2}s_{012} & - l_{1}s_{01} - l_{2}s_{012} & - l_{2}s_{012} \\ l_{0}c_0 + l_{1}c_{01} + l_{2}c_{012} & l_{1}c_{01} + l_{2}c_{012} & l_{2}c_{012}\\ 1 & 1 & 1 \end{array}\right].

(3)

q_0 = pi/4.0
q_1 = -pi/8.0
q_2 = pi/12.0

l_0 = 1
l_1 = 1
l_2 = 1

c0 = cos(q_0)
c01 = cos(q_0 + q_1)
c012 = cos(q_0 + q_1 + q_2)

s0 = sin(q_0)
s01 = sin(q_0 + q_1)
s012 = sin(q_0 + q_1 + q_2)

# Task space
p_x = l_0 * c0 + l_1 * c01 + l_2 * c012
p_y = l_0 * s0 + l_1 * s01 + l_2 * s012
phi = q_0 + q_1 + q_2

# Jacobian
J_1_1 = - l_0 * s0 - l_1 * s01 - l_2 * s012
J_1_2 = - l_1 * s01 - l_2 * s012
J_1_3 = - l_2 * s012

J_2_1 = l_0 * c0 + l_1 * c01 + l_2 * c012
J_2_2 = l_1 * c01 + l_2 * c012
J_2_3 = l_2 * c012

J_3_1 = 1
J_3_2 = 1
J_3_3 = 1

J = np.array(
        [[J_1_1, J_1_2, J_1_3],
         [J_2_1, J_2_2, J_2_3],
         [J_3_1, J_3_2, J_3_3]]
)

print(f"The analytical Jacobian at q_0={q_0}, q_1={q_1}, and q_2={q_2} is \n J={J}")

The analytical Jacobian at q_0=0.7853981633974483, q_1=-0.39269908169872414, and q_2=0.2617993877991494 is 
 J=[[-1.69855164 -0.99144486 -0.60876143]
 [ 2.42433965  1.71723287  0.79335334]
 [ 1.          1.          1.        ]]

Challenge 1¶

To solve this challenge, we notice the patterns in the computation. You can change to the same link values of exercise a to test that this is correct.

# Consider manipulator DoFs as the length of the following lists.
# Consider it as the configuration space of the RRR...RRR robot
q = [pi/4.0, -pi/8.0, pi/12.0, -pi/3.0] # Increase length of q if you'd like to check
l = [1, 1, 1, 1] # l must be same size of q

if len(q) != len(l):
    raise Exception("q and l are not the same length")

def c_n(q, n_frame):
    """
    Get cos(q_0 + q_1 + ... + q_n).
    """
    q_sum = 0
    c_result = 0
    for i in range(n_frame+1):
        qi = q[i]
        q_sum += qi # First will be q_0, then q_0 + q_1, then...
        c_result = cos(q_sum) # First will be cos(q_0), then cos(q_0 + q_1), then...
    return c_result

def s_n(q, n_frame):
    """
    Get sin(q_0 + q_1 + ... + q_n).
    """
    q_sum = 0
    s_result = 0
    for i in range(n_frame+1):
        qi = q[i]
        q_sum += qi # First will be q_0, then q_0 + q_1, then...
        s_result = sin(q_sum) # First will be sin(q_0), then sin(q_0 + q_1), then...
    return s_result

def px_n(q, l, n_frame):
    """
    Get l_0*cos(q_0) + l_1*cos(q_0 + q_1) + ... + l_n*cos(q_0 + q_1 + ... + q_n).
    """
    px = 0
    for i in range(n_frame + 1):
        li = l[i]
        px += li * c_n(q, i)
    return px

def py_n(q, l, n_frame):
    """
    Get l_0*sin(q_0) + l_1*sin(q_0 + q_1) + ... + l_n*sin(q_0 + q_1 + ... + q_n).
    """
    py = 0
    for i in range(n_frame + 1):
        li = l[i]
        py += li * s_n(q, i)
    return py


def j_n(q, l, n_frame):
    """
    Construct the n-th column of the Jacobian matrix.
    """
    N = len(q)

    pnx = px_n(q, l, n_frame-1) # starts at 0
    pny = py_n(q, l, n_frame-1) # starts at 0
    pNx = px_n(q, l, N-1)
    pNy = py_n(q, l, N-1)

    px = pNx - pnx
    py = pNy - pny

    jn = np.array(
        [[-py],
          [px],
          [1]]
    )
    return jn

# Jacobian
J = j_n(q, l, 0)
for i in range(1,len(q)):
    J = np.hstack((J, j_n(q, l, i))) # We stack the columns horizontally
print(J)

[[-1.31586821 -0.60876143 -0.226078    0.38268343]
 [ 3.34821919  2.64111241  1.71723287  0.92387953]
 [ 1.          1.          1.          1.        ]]