.. DO NOT EDIT.
.. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY.
.. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE:
.. "examples/robolab/example_forward_kinematics.py"
.. LINE NUMBERS ARE GIVEN BELOW.

.. only:: html

    .. note::
        :class: sphx-glr-download-link-note

        :ref:`Go to the end <sphx_glr_download_examples_robolab_example_forward_kinematics.py>`
        to download the full example code

.. rst-class:: sphx-glr-example-title

.. _sphx_glr_examples_robolab_example_forward_kinematics.py:


FK from models
========================

Forward kinematics from URDF or MuJoCo XML files.

.. GENERATED FROM PYTHON SOURCE LINES 7-74

.. code-block:: default


    import pprint

    import math

    import rofunc as rf

    rf.logger.beauty_print("########## Forward kinematics from URDF or MuJoCo XML files with RobotModel class ##########")
    rf.logger.beauty_print("---------- Forward kinematics for Franka Panda using URDF file ----------")
    model_path = "../../rofunc/simulator/assets/urdf/franka_description/robots/franka_panda.urdf"

    joint_value = [[0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0, 0.0, 0.0],
                   [0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0, 0.0, 0.0]]
    export_link = "panda_hand"

    # # Build the robot model with kinpy
    # # Deprecated: kinpy is not supported anymore, just for checking the results!!!! Please use pytorch_kinematics instead.
    # robot = rf.robolab.RobotModel(model_path, solve_engine="kinpy", verbose=False)
    # # Show the robot chain and joint names, can also be done by verbose=True
    # robot.show_chain()
    # # Get the forward kinematics of export_link
    # pos, rot, ret = robot.get_fk(joint_value, export_link)
    #
    # # Convert the orientation representation and print the results
    # rot = rf.robolab.convert_quat_order(rot, "wxyz", "xyzw")
    # rf.logger.beauty_print(f"Position of {export_link}: {pos}")
    # rf.logger.beauty_print(f"Rotation of {export_link}: {rot}")
    # pprint.pprint(ret, width=1)

    # Try the same thing with pytorch_kinematics
    robot = rf.robolab.RobotModel(model_path, solve_engine="pytorch_kinematics", verbose=False)
    pos, rot, ret = robot.get_fk(joint_value, export_link)
    rf.logger.beauty_print(f"Position of {export_link}: {pos}")
    rf.logger.beauty_print(f"Rotation of {export_link}: {rot}")
    pprint.pprint(ret, width=1)

    rf.logger.beauty_print("---------- Forward kinematics for Bruce Humanoid Robot using MJCF file ----------")
    model_path = "../../rofunc/simulator/assets/mjcf/bruce/bruce.xml"
    joint_value = [0.0 for _ in range(16)]

    export_link = "elbow_pitch_link_r"

    # # Build the robot model with pytorch_kinematics, kinpy is not supported for MJCF files
    robot = rf.robolab.RobotModel(model_path, solve_engine="pytorch_kinematics", verbose=True)
    # Get the forward kinematics of export_link
    pos, rot, ret = robot.get_fk(joint_value, export_link)
    #
    # # Print the results
    # rf.logger.beauty_print(f"Position of {export_link}: {pos}")
    # rf.logger.beauty_print(f"Rotation of {export_link}: {rot}")
    # pprint.pprint(ret, width=1)

    rf.logger.beauty_print("---------- Forward kinematics for United Digital Human (UDH) using MJCF file ----------")
    model_path = "../../rofunc/simulator/assets/mjcf/hotu/hotu_humanoid.xml"
    joint_value = [0.1 for _ in range(34)]

    export_link = "left_hand_2"

    # # Build the robot model with pytorch_kinematics, kinpy is not supported for MJCF files
    robot = rf.robolab.RobotModel(model_path, solve_engine="pytorch_kinematics", verbose=True)
    # Get the forward kinematics of export_link
    pos, rot, ret = robot.get_fk(joint_value, export_link)

    # # Print the results
    rf.logger.beauty_print(f"Position of {export_link}: {pos}")
    rf.logger.beauty_print(f"Rotation of {export_link}: {rot}")
    pprint.pprint(ret, width=1)


.. rst-class:: sphx-glr-timing

   **Total running time of the script:** (0 minutes 0.000 seconds)


.. _sphx_glr_download_examples_robolab_example_forward_kinematics.py:

.. only:: html

  .. container:: sphx-glr-footer sphx-glr-footer-example




    .. container:: sphx-glr-download sphx-glr-download-python

      :download:`Download Python source code: example_forward_kinematics.py <example_forward_kinematics.py>`

    .. container:: sphx-glr-download sphx-glr-download-jupyter

      :download:`Download Jupyter notebook: example_forward_kinematics.ipynb <example_forward_kinematics.ipynb>`


.. only:: html

 .. rst-class:: sphx-glr-signature

    `Gallery generated by Sphinx-Gallery <https://sphinx-gallery.github.io>`_