Differentiable Environment Primitives for Contact State Estimation

In contact-rich manipulation, the robot dynamics are coupled with an environment that has application-specific dynamic properties (stiffness, inertia) and geometry (contact normal). Knowledge of these environmental parameters can improve control and monitoring, but they are often unobserved and may vary, either online or between task instances. Observers, such as the extended Kalman filter, can be used to estimate these parameters, but such model-based techniques can require too much engineering work to scale up to complex environments, such as multi-point contact. To accelerate environment modeling, we propose environment primitives: parameterized environment dynamics that can be connected in parallel and are expressed in an automatic differentiation framework. This simplifies offline gradient-based optimization to fit model parameters and linearization of the coupled dynamics for an observer. This method is implemented for stiffness contact models, allowing the fitting of contact geometry and stiffness offline or their online estimation by an extended Kalman filter. This method is applied to a collaborative robot, estimating external force, contact stiffness, and contact geometry from the motor position and current. The estimates of external force and stiffness are compared with a momentum observer and direct force measurements.