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

.. only:: html

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

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

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

.. _sphx_glr_examples_simulator_example_curi_track_traj_w_interference.py:


Tracking the trajectory with interference by CURI
=================================

This example runs a Tai Chi demo bimanual trajectory by using CURI.

.. GENERATED FROM PYTHON SOURCE LINES 7-44

.. code-block:: default

    import os
    import numpy as np
    from isaacgym import gymutil
    import torch
    import rofunc as rf

    args = gymutil.parse_arguments()
    args.use_gpu_pipeline = False

    traj_l = np.load('../data/LQT_LQR/taichi_1l.npy')
    traj_r = np.load('../data/LQT_LQR/taichi_1r.npy')
    rf.lqt.plot_3d_bi(traj_l, traj_r, ori=False)

    num_envs = 1
    num_bodies = 39
    num_dofs = 25
    force_intf = 10000
    torque_intf = 100000

    forces = torch.zeros((num_envs, num_bodies, 3), dtype=torch.float)
    torques = torch.zeros((num_envs, num_bodies, 3), dtype=torch.float)
    # efforts = torch.zeros((num_envs, num_dofs, 1), dtype=torch.float)
    efforts = np.zeros(num_dofs, dtype=np.float32)
    # forces[:, 25, 2] = force_intf
    torques[:, 9, 1] = torque_intf
    # forces[:, 9, 2] = force_intf
    # torques[:, 36, 2] = torque_intf
    # efforts[:, 9, 0] = 100000
    efforts[4] = -100000

    CURIsim = rf.sim.CURISim(args, num_envs=num_envs, fix_base_link=True)
    CURIsim.init()
    CURIsim.run_traj_multi_joints_with_interference(traj=[traj_l, traj_r],
                                                    attracted_rigid_bodies=["panda_left_hand", "panda_right_hand"],
                                                    intf_mode="body_forces",
                                                    intf_forces=forces, intf_torques=torques,
                                                    intf_index=[50], update_freq=0.001, save_name='state7')


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

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


.. _sphx_glr_download_examples_simulator_example_curi_track_traj_w_interference.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_track_traj_w_interference.py <example_curi_track_traj_w_interference.py>`

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

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


.. only:: html

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

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