Passive SE(3) Energy Tank Control for Contact-Rich Manipulation
This project develops a controller that guarantees passivity for six degree of freedom robotic motion in contact rich environments. The work extends the concept of energy tanks, which are virtual reservoirs that regulate non-passive control actions, beyond translation into full SE(3) by incorporating rotational dynamics expressed in quaternion space.
Motivation
Manipulation tasks such as insertion, polishing, or groove following require robots to apply forces and torques while remaining stable during unexpected contact. Classical dynamical system controllers generate smooth, reactive velocity fields in free space but can lose passivity once additional wrenches are injected. Prior energy tank formulations only addressed translational motion, leaving orientation unaccounted for. The goal here was to unify both translation and rotation in a single passivity enforcing framework.
Controller Design
The controller introduces two parallel energy tanks.
The linear tank regulates non-passive translational forces, storing dissipated energy from damping and permitting controlled force injection only when energy is available.
The rotational tank is formulated in quaternion space and ensures torque based orientation control remains passive. The quaternion error is mapped to an axis angle vector, and torque commands are gated by the rotational tank state.
At each control cycle, the system computes nominal dynamical system flows, updates both tank levels through gating functions, and blocks any non-passive term if a tank is depleted. The final wrench is clamped to prescribed limits and sent to the robot.
Passivity Guarantee
By constructing a storage function that combines robot kinetic energy with both tank states, it can be shown that the time derivative of total stored energy is bounded by the external power input. This ensures the closed loop system never generates net energy, guaranteeing passivity with respect to external forces and torques across SE(3).
Experimental Validation
Experiments were conducted on a KUKA LBR iiwa 7 R800 equipped with an OptoForce six axis wrist sensor. The task involved inserting a cylindrical tool into a custom J shaped groove fixture and tracing the path under controlled force and torque. The controller allowed up to 40 N of commanded force during insertion and up to 3 Nm torque during rotation, while both tanks maintained positive margins.
Across multiple trials, the system demonstrated stable insertion and a smooth transition from translational to rotational phases of the groove following task. Measured peak forces remained below 56 N and peak torques below 1.8 Nm, showing that the commanded limits were never exceeded. Importantly, the tank levels never depleted during the trials, which confirmed continuous enforcement of passivity even under varying contact conditions.
Discussion
The dual tank architecture successfully decouples translational and rotational energy flows, enabling robust torque intensive manipulation. While binary gating functions introduced abrupt switches in some trials, the tanks prevented instability under all tested conditions. Future work includes smoothing the gating functions, exploring coordinated tank budgets, and extending validation to higher torque tasks such as fastening or screwing operations.
Conclusion
By extending energy tanks to quaternion based orientation control, this project demonstrates a practical, provably passive SE(3) controller. The approach enforces stability during forceful, contact rich tasks and provides a foundation for safe, adaptive manipulation in environments with uncertain or dynamic contact conditions.