Task Servoing

The desired terms, $\acc_{i}^{\text{des}}$ , $\jsrd_{i}^{\text{des}}$ , $\we_{i}^{\text{des}}$ , and $\torque^{\text{des}}$ , from (1), (1), (1), and (1), respectively are provided by higher-level task servoing. Commonly, the high-level reference of a task is simply to attain some pose, and in the case of a wrench task, some force and/or torque. For acceleration tasks, if the desired task value is expressed as a pose, position, or orientation, then it must be converted to an acceleration. This is done here using a feedforward (PD) controller,

(1) $\acc_{i}^{\text{des}}(t+\Delta t) = \acc_{i}^{\text{ref}}(t+\Delta t) + K_{p}\bs{\epsilon}_{i}(t) + K_{d}\dot{\bs{\epsilon}}_{i}(t) \tc$

noindent where $\acc_{i}^{\text{ref}}(t+\Delta t)$ is the feedforward frame acceleration term, $\bs{\epsilon}_{i}(t)$ and $\dot{\bs{\epsilon}}_{i}(t)$ are the current pose error and its derivative, with $K_{p}$ and $K_{d}=2\sqrt{K_{p}}$ , their proportional and derivative gains respectively. This term also serves to remove drift at the controller level and stabilize the output of the task. The terms, $\bs{\epsilon}_{i}(t)$ and $\dot{\bs{\epsilon}}_{i}(t)$ , are not explicitly defined here because they are representation dependent (see citep{Siciliano2008}). For wrench and torque tasks a similar servoing controller can be developed using a Proportional-Integral (PI) controller.

(2) $\w^{des}(t+\Delta t) = \w^{ref}(t+\Delta t) + K_{p}\err_{\w}(t) + K_{i} \int \err_{\w}(t) dt$

This servoing helps stabilize the whole-body controller by driving the desired task values to some fixed state in asymptotically stable manner. Without the servoing the the task error objective term, $\ft_{i}(\optvar)$ , could change discontinuously between time steps resulting in discontinuous jumps in the optimal joint torques determined between two time steps.