Extracting / retrieving denavit hartenberg parameters from a model?

[loerting]

Is there a way to extract or retrieve the denavit hartenberg parameters from a model? Are the models true to the original size? I want to compute forward kinematics externally.

[loerting]

I am now using the DH parameters for the Franka Panda robot taken from here: https://frankaemika.github.io/docs/control_parameters.html

I am computing FK using them, but getting different results on the end effector pose.

# joint config [ 1.4336555 -1.2559798 -1.8131765 -1.9919585 -2.7769492  3.1733205   2.2967923]

Expected (retrieved by xpos and xquat in mujoco):

# link0 [0. 0. 0.] [0. 0. 0.] 1.0
# link1 [0.    0.    0.333] [0.         0.         0.65699466] 0.7538952301455264
# link2 [0.    0.    0.333] [-0.1584711  -0.68912039  0.06276929] 0.7043152814086875
# link3 [-0.04107766 -0.29763846  0.43087735] [ 0.58666708  0.03086295 -0.15259722] 0.7947221582160214
# link4 [ 0.037423   -0.31466087  0.41205889] [ 0.60331682  0.77300487 -0.19400412] -0.028890787157653183
# link5 [ 0.41359188 -0.31698712  0.29912612] [-0.59138602  0.56361246 -0.32521138] 0.4762784204699823
# link6 [ 0.41359188 -0.31698712  0.29912612] [0.16984533 0.07870821 0.7648305 ] 0.6164348171858808
# link7 [ 0.3975476  -0.23165591  0.31344978] [ 0.77197275 -0.262593    0.48692373] -0.31306273005723173

Calculated using Franka Panda's DH and a tested FK implementation:

tensor([0., 0., 0., 0., 0., 0., 1.])
tensor([0.0000, 0.0000, 0.3330, 0.0000, 0.0000, 0.6570, 0.7539])
tensor([ 0.0000,  0.0000,  0.3330, -0.7043, -0.0627,  0.0627,  0.7043])
tensor([-0.0559,  0.3110,  0.3330,  0.0699, -0.7843,  0.0547,  0.6140])
tensor([-0.0346,  0.2384,  0.3003, -0.7608,  0.6034,  0.0833,  0.2238])
tensor([ 0.0122,  0.3348, -0.0776,  0.6038,  0.3067,  0.5967,  0.4305])
tensor([ 0.0122,  0.3348, -0.0776,  0.2166,  0.1326, -0.7280,  0.6368])
tensor([ 0.0826,  0.3755, -0.0439,  0.8025,  0.0984,  0.3009,  0.5058])

Are the DH parameters different in mujoco menagerie's model of Franka Panda?

[kevinzakka]

I haven't looked too deeply into this but the DH parameters you sent are for the FR3, not the Panda.

[kevinzakka]

@loerting Here's a minimal reproducible example for the Franka Panda demonstrating that MuJoCo, Pinocchio and the "Robotics Toolbox Python" (constructed from DH parameters I obtained from here) return the exact same values for forward kinematics.

"""Forward kinematics comparison between MuJoCo, Pinocchio and Robotics-Toolbox-Python.

pip install robot_descriptions pin mujoco numpy roboticstoolbox-python
"""

import numpy as np
import mujoco
import roboticstoolbox as rtb
import pinocchio as pin
from robot_descriptions.loaders.mujoco import (
    load_robot_description as load_mj_description,
)
from robot_descriptions.loaders.pinocchio import (
    load_robot_description as load_pin_description,
)

QPOS = np.asarray(
    [
        1.4336555,
        -1.2559798,
        -1.8131765,
        -1.9919585,
        -2.7769492,
        3.1733205,
        2.2967923,
        0,
        0,
    ]
)

def fk_mujoco():
    model = load_mj_description("panda_mj_description")
    data = mujoco.MjData(model)
    data.qpos = QPOS
    mujoco.mj_kinematics(model, data)
    positions = []
    quaternions = []
    for link in [f"link{i}" for i in range(1, 8)]:
        positions.append(data.body(link).xpos)
        quaternions.append(data.body(link).xquat)
    return positions, quaternions

def fk_rtp():
    d1 = 0.333
    pi2 = np.pi / 2
    d3 = 0.316
    d5 = 0.384
    a4 = 0.0825
    a5 = -0.0825
    a7 = 0.088
    robot = rtb.DHRobot(
        [
            rtb.RevoluteMDH(d=d1),
            rtb.RevoluteMDH(alpha=-pi2),
            rtb.RevoluteMDH(alpha=pi2, d=d3),
            rtb.RevoluteMDH(a=a4, alpha=pi2),
            rtb.RevoluteMDH(a=a5, alpha=-pi2, d=d5),
            rtb.RevoluteMDH(alpha=pi2),
            rtb.RevoluteMDH(a=a7, alpha=pi2),
        ]
    )
    positions = []
    quaternions = []
    for frame in robot.fkine_all(QPOS)[1:]:
        positions.append(frame.t.copy())
        quat = np.zeros(4)
        mujoco.mju_mat2Quat(quat, frame.R.copy().ravel())
        quaternions.append(quat)
    return positions, quaternions

def fk_pinocchio():
    robot = load_pin_description("panda_description", root_joint=None)
    pin.computeJointJacobians(robot.model, robot.data, QPOS)
    pin.updateFramePlacements(robot.model, robot.data)
    frames = [f"panda_link{i}" for i in range(1, 8)]
    positions = []
    quaternions = []
    for frame in frames:
        frame_id = robot.model.getFrameId(frame)
        T = robot.data.oMf[frame_id].copy()
        positions.append(T.translation)
        quat = np.zeros(4)
        mujoco.mju_mat2Quat(quat, T.rotation.ravel())
        quaternions.append(quat)
    return positions, quaternions

if __name__ == "__main__":
    pos_mj, quat_mj = fk_mujoco()
    pos_rtp, quat_rtp = fk_rtp()
    pos_pin, quat_pin = fk_pinocchio()

    for p_mj, p_rtp, p_pin in zip(pos_mj, pos_rtp, pos_pin):
        np.testing.assert_allclose(p_mj, p_rtp, atol=1e-7)
        np.testing.assert_allclose(p_rtp, p_pin, atol=1e-7)

    def assert_quat_equal(a, b):
        a *= np.sign(a[0])
        b *= np.sign(b[0])
        np.testing.assert_allclose(a, b, atol=1e-7)

    for q_mj, q_rtp, q_pin in zip(quat_mj, quat_rtp, quat_pin):
        assert_quat_equal(q_mj, q_rtp)
        assert_quat_equal(q_rtp, q_pin)

[loerting]

I finally got it to work. The mistake I did was to use the standard DH parameter transformation matrix instead of the modified (Craig's convention), as apparently Franka Panda uses the modified variant.

def forward_kinematics(dh_parameters, joint_configuration: torch.Tensor) -> torch.Tensor:
    """
    This function calculates the forward kinematics of the robot.
    :param dh_parameters: dh parameters of the robot.
    :param joint_configuration: joint configuration of the robot.
    :return: end effector pose of the robot.
    """

    cum_mat = torch.eye(4)
    for i in range(1, len(dh_parameters) + 1):
        cum_mat = torch.matmul(cum_mat, homog_trans_mat_craig(dh_parameters, i, joint_configuration))

    return mat_to_pose(cum_mat)

def mat_to_pose(mat: torch.Tensor) -> torch.Tensor:
    # extract position and quaternion orientation from the matrix.
    eta = 0.5 * torch.sqrt(1 + torch.trace(mat[:3, :3]))  # TODO fix sqrt NaN
    epsilon = 0.5 * torch.tensor(
        [torch.sign(mat[2, 1] - mat[1, 2]) * torch.sqrt(mat[0, 0] - mat[1, 1] - mat[2, 2] + 1),
         torch.sign(mat[0, 2] - mat[2, 0]) * torch.sqrt(mat[1, 1] - mat[2, 2] - mat[0, 0] + 1),
         torch.sign(mat[1, 0] - mat[0, 1]) * torch.sqrt(mat[2, 2] - mat[0, 0] - mat[1, 1] + 1)])

    return torch.tensor([mat[0, 3], mat[1, 3], mat[2, 3], epsilon[0], epsilon[1], epsilon[2], eta])

def homog_trans_mat_craig(dh_parameters_craig, i, joint_configuration: torch.Tensor) -> torch.Tensor:
    """
    This function returns T_{n}^{n-1}, expect when n=0, then returns T_0^0, which is I.
    :param dh_parameters_craig: modified dh parameters of the robot (craigs convention)
    :param i: number of the transformation (goal index).
    :param joint_configuration: joint configuration of the robot.
    :return: 4x4 homogeneous transformation matrix
    """

    if i == 0:
        return torch.eye(4)

    a_nm1 = torch.tensor([dh_parameters_craig[i - 1]['a']])
    alpha_nm1 = torch.tensor([dh_parameters_craig[i - 1]['alpha']])
    d_n = torch.tensor([dh_parameters_craig[i - 1]['d']])
    theta_n = torch.tensor([dh_parameters_craig[i - 1]['theta']]) + (joint_configuration[i - 1] if i - 1 < len(joint_configuration) else 0)

    return torch.tensor([
        [torch.cos(theta_n), -torch.sin(theta_n), 0, a_nm1],
        [torch.sin(theta_n) * torch.cos(alpha_nm1), torch.cos(theta_n) * torch.cos(alpha_nm1), -torch.sin(alpha_nm1),
         -d_n * torch.sin(alpha_nm1)],
        [torch.sin(theta_n) * torch.sin(alpha_nm1), torch.cos(theta_n) * torch.sin(alpha_nm1), torch.cos(alpha_nm1),
         d_n * torch.cos(alpha_nm1)],
        [0, 0, 0, 1]])

[loerting]

To answer the question on how to extract the DH parameters from a mujoco menagerie model is to find a reference of the original robot. The simulated robot is scaled exactly the same.

[loerting]

I haven't looked too deeply into this but the DH parameters you sent are for the FR3, not the Panda.

They are the same, by the way.

[kevinzakka]

Yeah I noticed that if I used regular DH parameters, the tests didn't pass. Had to switch to modified DH parameters.

In any case, to answer your original question, no there is no native way to extract DH parameters from a MuJoCo model. But as the tests above show, if your goal is just forward kinematics, then regular FK in MuJoCo will give you the exact same answer as any other library out there (assuming no bugs in our conversion pipeline from official URDF to MJCF).