Task / Joint Space Controllers
=====================


Task Space Controllers
~~~~~~~~~~~~~~~~~~~~~~


One highlight of our codebase is the implementation of several task space controllers. More specifically, our codebase emphasizes on the operational space controller (OSC) (also known as cartesian impedance controller). The family of impedance controller is very useful in manipulation tasks, as we can increase the impedance to make robot behave more rigidly to track a trajectory accurately, or decrease the impedance to make robots behavior more compliantly to enable contact-rich behaviors without breaking itself.


----------------------------------------
Operational Space Controller (6DoF Pose)
----------------------------------------

In short, the most common OSC controller we use follows the formula:


$$\tau = \mathcal{J}^{T}_{pos}\Lambda_{pos}(K^{p}_{p}e_{p} - K^{p}_{v}v) + \mathcal{J}^{T}_{ori}\Lambda_{ori}(K^{r}_{p}e_{ori} - K^{r}_{v}\omega) + \tau_{null}$$

where $\Lambda=\mathcal{J}^{-T}\mathcal{M}\mathcal{J}^{-1}$ is the mass matrix represented in the end-effector space (while $M$ is the mass matrix corresponding to the each Joint).

.. note::

    For a robot like Panda, $\mathcal{M} \in \mathbb{R}^{7x7}$, $\mathcal{J} in \mathbb{R}^{6x7}$, and $\mathcal{M}$ is invertible, while there is no guarantee for $\mathcal{J}$. 
    Therefore, a numerically stable solution is to first compute $\Lambda^{-1}$, then convert it back to $\Lambda$. Concretely, we compute the mass matrix with respect to the end-effector as follows:
    $$\Lambda^{-1}=\mathcal{J}\mathcal{M}^{-1}\mathcal{J}^{T}$$
    $$\Lambda=pinv(\Lambda^{-1})$$

    Where $pinv$ is the `pseudo inverse function`_ in case the resulting matrix is not revertible

-------------------------------------------------
Position-only Operational Space Controller (3DoF)
-------------------------------------------------

This is a simplified version of the full OSC Controller. This controller only allows translational movement in x, y, z directions. This is very useful when you only care about top-down pick-place actions. 

------------------------------------------
Operational Space Yaw Controller (4DoF)
------------------------------------------

This is another simplified version of the full OSC Controller. This controller allows translational movement in x, y, z directions and yaw angle rotation. This is useful if your doenstream tasks require top-down gripper pose while rotation in z axis is required. 




Joint Space Controllers
~~~~~~~~~~~~~~~~~~~~~~~~

Joint space controllers focus on commanding robots to desired configurations instead of desired poses in the task space.

------------------------------
Joint Impedance Controller
------------------------------

Our implementation of joint impedance control follows the following equation:

$$\tau=K_{p}(q_{target}-q)-K_{d}\dot{q} + \tau_{external}$$


But notice that our current implementation assumes $\tau_{external}$ to be zero all the time. If you are an advanced user, feel free to make changes to meet your need.

------------------------------
Joint Position Controller
------------------------------

Majorly built upon the libfranka example script `generate_joint_position_motion.cpp`_. We won't explain too much here. 


The only thing to keep in mind is that you want to directly set the target joint positions for this controller, as this is not designed for following a trajectory of specified configuration. See the example `reset_joint_states.py`_. 

.. WARNING::
    This controller is NOT a motion planner. Make sure that the current robot configuration can reach to your target configuration with simple interpolation. Otherwise the robot might have severe self-collisiion!

.. _generate_joint_position_motion.cpp: https://github.com/frankaemika/libfranka/blob/0.9.0/examples/generate_joint_position_motion.cpp
.. _reset_joint_states.py: https://github.com/UT-Austin-RPL/deoxys_control/blob/main/deoxys/examples/reset_robot_joints.py
.. _pseudo inverse function: https://github.com/UT-Austin-RPL/deoxys_control/blob/33d9fd446023f86a99a17f65ee81784786267396/deoxys/franka-interface/include/utils/control_utils.h#L15