In this post, we introduce first-order task-space impedance controller, and in particular, for controlling the 3D Cartesian position of any point on the robot’s body. While this post focuses on controlling the end-effector of the robot manipulator, the approach can be generalized to any point of the robot.
As always, we consider controlling an \(n\)-DOF open-chain torque-actuated robot manipulator, where the dynamics is governed by the following equations of motion: \[ \mathbf{M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)}=\boldsymbol{\tau}_\text{in} \tag{1} \] The notations of the equation are self-explanatory, and we assume there are no external forces applied to the robot.
As with the joint-space impedance controller, throughout this post, we assume that gravitational force vector \(\mathbf{g(q)}\), is compensated, i.e., the \(\mathbf{g(q)}\) term is neglected.
Given the joint posture \(\mathbf{q}\) and velocity \(\mathbf{\dot{q}}\), let the 3D Cartesian position and velocity of the end-effector be denoted as \(\mathbf{p}=\mathbf{p(q)}\) and \(\mathbf{\dot{p}}\), respectively. The \(\boldsymbol{\tau}_{\text{in}}\) for the first-order position task-space impedance controller is defined by: \[ \boldsymbol{\tau}_\text{in} = \mathbf{J_{p}(q)^{T}} \{ \mathbf{K_p(p_0-p) + B_p(\dot{p}_0-\dot{p})} \} \tag{2} \] In this equation, \(\mathbf{K_p},\mathbf{B_p} \in \mathbb{R}^{3\times 3}\) are the translational stiffness and damping matrices; \(\mathbf{p_0}=\mathbf{p_0}(t)\) is a virtual end-effector trajectory to which the (virtual translational) stiffness and damping are connected; \(\mathbf{J_p(q)}\in\mathbb{R}^{3\times n}\) is the Analytical Jacobian matrix [1], which can be derived by collecting the partial derivatives of \(\mathbf{p(q)}\) with respect to \(\mathbf{q}\). Note that the \(\mathbf{p(q)}\) can be derived by the Forward Kinematics map, and \(\mathbf{\dot{p}}=\mathbf{J(q)\dot{q}}\).
If \(\mathbf{K_p},\mathbf{B_p}\) are chosen to be constant symmetric positive-definite matrices, the first-order joint-space impedance controller has favorable stability property [3]. For this section, we assume no kinematic redundancy of the robot and the inverse is well-defined, i.e., Jacobian \(\mathbf{J_p(q)}\) is a square matrix and is invertible. Let \(\mathbf{q_0}\) be fixed. The closed-loop dynamics of the robotic manipualtor is: \[ \mathbf{M(q)\ddot{q}+\{C(q,\dot{q})+B_p\}\dot{q}+J^T(q)K_p\{p(q)-p_0\}}=\mathbf{0} \tag{3} \] We define the following scalar function \(V(\mathbf{q},\mathbf{\dot{q}})\): \[ V(\mathbf{q},\mathbf{\dot{q}}) = \frac{1}{2}\mathbf{\dot{q}^TM(q)\dot{q}}+\frac{1}{2}\mathbf{(p(q)-p_0)K_p(p(q)-p_0)} \]
It is quick to check that \(\dot{V}(\mathbf{q},\mathbf{\dot{q}})\) is negative semi-definite. Thus, using La-salle’s invariant set theorem, \(\mathbf{\dot{q}}\rightarrow \mathbf{0}\). Since \(\mathbf{J_p(q)}\) is invertible, \(\mathbf{\dot{p}}\rightarrow \mathbf{0}\), and the largest invariant set is when \(\mathbf{p(q)=p_0}\).
For the case with Kinematic Redundancy, i.e., the Jacobian is not a square matrix, the proof is technical and the readers may refer to [4].
The example of this controller can be checked .