fk_calc.py

import adam
from adam.pytorch import KinDynComputations
import numpy as np
import torch


model_path = "/samsung4tb/franka_description/urdfs/fr3_franka_hand.urdf"

# joints_name_list = ["base_joint", "fr3_joint1", "fr3_joint2", "fr3_joint3", "fr3_joint4", "fr3_joint5", "fr3_joint6", "fr3_joint7", "fr3_joint8", "fr3_hand_joint", "fr3_hand_tcp_joint", "fr3_finger_joint1", "fr3_finger_joint2"]

joints_name_list = ["fr3_joint1", "fr3_joint2", "fr3_joint3", "fr3_joint4", "fr3_joint5", "fr3_joint6", "fr3_joint7"]

kinDyn = KinDynComputations(model_path, joints_name_list, root_link="base")

# you need to set it manually like this to work
kinDyn.rbdalgos.root_link= "fr3_hand"
# kinDyn = KinDynComputations(model_path, joints_name_list, root_link="fr3_hand")

camera_calib_home = torch.Tensor(
[-0.19587807,  0.12579048,  0.72064054, -2.2718847 ,  0.427838  ,
        1.7621214 , -1.662231]
)

out1 = kinDyn.forward_kinematics('fr3_hand', np.eye(4), camera_calib_home)

out2 = kinDyn.forward_kinematics('fr3_hand_tcp', np.eye(4), camera_calib_home)