#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Fri Apr  7 15:38:46 2023

@author: Pierre Lecomte
"""
import numpy as np
from kinematics_from_description.kfd import KinematicsFromDescriptionTool as KFD

tool = KFD(
            {
                "body_frame": "sma_link",
                "space_frame": "base_link",
                "namespace": "wx250s_custom",
            }            
            )

tool.load_desc_from_file('out.urdf')
tool.run()

print("Slist:")
print(tool.Slist.T)

print("M:")
print(tool.M)

x = f"""class wx250s_custom(ModernRoboticsDescription):
    Slist = np.array(
        {np.array2string(tool.Slist.T, separator=',')}
        ).T

    M = np.array({np.array2string(tool.M, separator=',')})
    """

with open("mr_descriptions.py","a") as f:
    f.writelines(x)