from scripts.utils.transforms import getTransform, getChain, getAggregateTransform
import json
from urdf_parser_py.urdf import URDF
from atom_core.naming import generateKey
from tf.transformations import quaternion_from_matrix, quaternion_from_euler


file = "../xacros/well_optimized.urdf.xacro"
xml_robot = URDF.from_xml_file(file)
print(xml_robot.joint_map.__dir__())
print(xml_robot.joint_map.items.__dir__())

dict = {}

for joint in xml_robot.joints:

    child = joint.child
    parent = joint.parent
    xyz = joint.origin.xyz
    rpy = joint.origin.rpy
    key = generateKey(parent, child)

    dict[key] = {}
    dict[key]['child'] = child
    dict[key]['parent'] = parent
    dict[key]['trans'] = xyz
    dict[key]['quat'] = list(quaternion_from_euler(rpy[0], rpy[1], rpy[2], axes='sxyz'))

T=getTransform('world', 'lidar_1_base_link', dict)
print('This is the transform between world and lidar1\n ')
print(T)