In this project, I Implemented a controller that builds a tower of 3 cubes using pose impedance control. The controller can be seen in the figure below.
Pick-and-Place-Franka-Demo.mp4
Controller:
Pose Impedence Controller Equation:
where:
-
$\boldsymbol{\tau} \in \mathbb{R}^7$ is the vector of joint torques -
$\mathbf{J} \in \mathbb{R}^{6 \times 7}$ is the Jacobian matrix of the robot -
$\mathbf{P} \in \mathbb{R}^{6 \times 6}$ is the proportional gain matrix -
$\mathbf{D} \in \mathbb{R}^{6 \times 6}$ is the derivative gain matrix - $\mathbf{T}{\text{current}}, \mathbf{T}{\text{desired}} \in \text{SE}(3)$ are the current and desired pose transformation matrices in
$\text{SE}(3)$ -
$\text{log}(\cdot)$ computes the matrix logarithm -
$(\cdot)^\vee$ denotes the vee map, converting a skew-symmetric matrix to a twist vector -
$\Delta \boldsymbol{V} \in \mathbb{R}^6$ is the twist error vector