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 that this controller is a pose-based controller, regardless of is_delta set to true or false.

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.

Cartesian Velocity Controller#

This is a controller that allows users to specify end effector velocities. This controller is built on top of the cartesian velocity callback function from libfranka. Check out the example script to see how to use it.

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!