Skip to content

Instantly share code, notes, and snippets.

@a-nooj

a-nooj/panda_jacobian.py

Last active October 6, 2024 08:38
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save a-nooj/72ba6c31d22e82799e2cff120c6ac136 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save a-nooj/72ba6c31d22e82799e2cff120c6ac136 to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. a-nooj revised this gist Feb 8, 2023. 1 changed file with 13 additions and 0 deletions.
    13 changes: 13 additions & 0 deletions panda_jacobian.py
    View file
    Original file line number Diff line number Diff line change
    @@ -1,6 +1,19 @@
    import numpy as np


    def get_tf_mat(i, dh):
    a = dh[i][0]
    d = dh[i][1]
    alpha = dh[i][2]
    theta = dh[i][3]
    q = theta

    return np.array([[np.cos(q), -np.sin(q), 0, a],
    [np.sin(q) * np.cos(alpha), np.cos(q) * np.cos(alpha), -np.sin(alpha), -np.sin(alpha) * d],
    [np.sin(q) * np.sin(alpha), np.cos(q) * np.sin(alpha), np.cos(alpha), np.cos(alpha) * d],
    [0, 0, 0, 1]])


    def get_jacobian(joint_angles):
    dh_params = np.array([[0, 0.333, 0, joint_angles[0]],
    [0, 0, -np.pi / 2, joint_angles[1]],
  2. a-nooj created this gist Feb 8, 2023.
    31 changes: 31 additions & 0 deletions panda_jacobian.py
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,31 @@
    import numpy as np


    def get_jacobian(joint_angles):
    dh_params = np.array([[0, 0.333, 0, joint_angles[0]],
    [0, 0, -np.pi / 2, joint_angles[1]],
    [0, 0.316, np.pi / 2, joint_angles[2]],
    [0.0825, 0, np.pi / 2, joint_angles[3]],
    [-0.0825, 0.384, -np.pi / 2, joint_angles[4]],
    [0, 0, np.pi / 2, joint_angles[5]],
    [0.088, 0, np.pi / 2, joint_angles[6]],
    [0, 0.107, 0, 0],
    [0, 0, 0, -np.pi / 4],
    [0.0, 0.1034, 0, 0]], dtype=np.float64)

    T_EE = np.identity(4)
    for i in range(7 + 3):
    T_EE = T_EE @ get_tf_mat(i, dh_params)

    J = np.zeros((6, 10))
    T = np.identity(4)
    for i in range(7 + 3):
    T = T @ get_tf_mat(i, dh_params)

    p = T_EE[:3, 3] - T[:3, 3]
    z = T[:3, 2]

    J[:3, i] = np.cross(z, p)
    J[3:, i] = z

    return J[:, :7]