Tuning Controller Parameters#

We realize that each Panda arm has its own set of hyperparameters to achieve ideal control performance. In this repo, we provide a simple, pre-programmed procedure to allow users to select ideal controller parameters. For now we support such functionality for Operational Space Controller (OSC_POSE) and Joint Impedance Controlelr (JOINT_IMPEDANCE).

Tuning Operational Space Controller#

There are mainly three factors that affect the OSC controller performance:

python scripts/benchmark_osc.py

Run:

In this script, we iterate through the following controller parameters: * Controller gains in translational and rotation motion * Residual mass matrix * Trajectory interpolation time fraction

We briefly explain major variable that is covered in benchmark_osc.py.

Controller Gains#

As we mention in controller page, we have controller gains for positions and orientations separately.

Residual Mass Matrix#

As explained in controller, OSC formulat takes into account the mass matrix of a Panda robot. In practice, you might find the mass matrix returned by Panda’s own default api has very small values for the last three joints. This will consequently result in small torques of the last few joints that cannot even overcome the friction. Therefore, a simple and effective approach is to add a residual mass matrix to the original mass matrix to compute larger torques for the last few joints. We recognize that this term varies a lot among different robot arms, so the tuning is needed to finalize the residual mass matrix.

Trajectory interpolation time fraction#

This is the parameter \(\alpha\) in the trajectory interpolation formula.

For more details, please refer to trajectory interpolation page.

Tuning Joint Impedance Controller#

Similar to the script for automatically running osc controllers with different parameters, we also have a auto-tuning script for joint impedance controller:

python scripts/benchmark_joint_impedance.py

There are mainly two factors that affect the joint impedance controller performance:

  • Controller gains for each joint

  • Trajectory interpolation time fraction (Same as in OSC, omitted in this subsection)

Controller Gains#

As we mention in controller, there are two controller gains, one is P control gain \(K_{p}\) , and the other is D control gain \(K_{d}\) . \(K_{p}\) mainly governs how the manipulator behaves stiff or compliant. Larger \(K_{p}\) means more stiff motion, while smaller means more compliant. \(K_{d}\) is the control gain that regularizes based on the manipulator’s velocities. With \(K_{d}=2\sqrt{K_{p}}\) , the robot arm can follow a trajectory in a critical damping manner (aka no overshooting or undershooting), theoretically. In practice, we found that the theoretical values will cause oscillation. This is because a mass-spring modeling of robot arm simplifies the real-world complexity where friction and all other factors play a role. As a result, we usually set \(K_{d}\) to be slightly smaller than the theoretical values for critical dampling.