.. DO NOT EDIT.
.. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY.
.. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE:
.. "examples/robolab/example_curi_inverse_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_curi_inverse_kinematics.py>`
        to download the full example code

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

.. _sphx_glr_examples_robolab_example_curi_inverse_kinematics.py:


CURI inverse kinematics
========================

Inverse kinematics of the CURI robot.

.. GENERATED FROM PYTHON SOURCE LINES 7-30

.. code-block:: default

    import os

    import numpy as np
    import pinocchio

    import rofunc as rf
    from rofunc.utils.oslab.path import get_rofunc_path

    model = pinocchio.buildModelFromUrdf(
        os.path.join(get_rofunc_path(), "simulator/assets/urdf/curi/urdf/curi_pinocchio_test.urdf"))
    print('model name: ' + model.name)

    POSE = [1, 0, 1]
    ORI = np.array([-1, 0, 0, 0, -1, 0, 0, 0, 1]).reshape(3, 3)
    q_ik = rf.robolab.ik(model, POSE, ORI, JOINT_ID=18)

    POSE_L = [1, 0.5, 0.5]
    POSE_R = [1, -0.5, 0.5]
    q_ik_dual = rf.robolab.ik_dual(model, POSE_L, POSE_R, JOINT_ID_L=18, JOINT_ID_R=27)

    # a = q_ik_dual.take(
    #     [0, 1, 2, 3, 4, 5, 6, 8, 10, 12, 21, 11, 13, 22, 23, 14, 15, 24, 16, 25, 26, 17, 18, 27, 19, 28])
    # print('\nresult: %s' % a.flatten().tolist())


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

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


.. _sphx_glr_download_examples_robolab_example_curi_inverse_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_curi_inverse_kinematics.py <example_curi_inverse_kinematics.py>`

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

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


.. only:: html

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

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