Documentation : https://auctus-team.gitlabpages.inria.fr/components/control/torque_qp/index.html
Torque QP
A generic low-level joint torque controller with a QP formulation.
torque_qp has been decoupled from ROS and the Franka Emika robot. It is now a standalone library to control a generic robot at the joint torque level using quadratic programming formulations. The trajectory generation and PID controller have also been removed from torque_qp to let the user choose the solution suiting best its needs.
To see an implementation of torque_qp on a panda robot go to panda_qp_control.
Installation
Currently, torque_qp needs catkin for its installation. The installation procedure is the following
# go to a working catkin workspace
git clone https://gitlab.inria.fr/auctus-team/components/control/torque_qp.git
rosdep install --from-paths torque_qp/ --ignore-src -r -y
git clone https://github.com/kuka-isir/qpOASES.git
git clone https://gitlab.inria.fr/auctus-team/components/control/qp_solver.git
caktin build Note: torque_qp uses pinocchio for the modelling of the robot. It requires a urdf description of the robot with the limits specified.
Quadratic programming formulation
Given a n-dof robot performing a task requiring m-dof, the problem solved by torque_qp is :
\begin{array}{ccl}
\tau^{opt} = & \arg \min & || \dot{v}^{*} - \dot{v}(\tau) ||^2_2 + \omega_{reg}|| \tau - ( g(q) - kd \dot{q} ) ||^2_2\\
& s.t & A \tau \leq b \\
\end{array}Main task
Where q \in \mathbb{R}^n and \dot{q} \in \mathbb{R}^n are respectively the robot joint configuration and joint velocity, \dot{v}(\tau) \in \mathbb{R}^{m*n} is the robot model based Cartesian acceleration and \dot{v}^{*} is a target Cartesian acceleration that one want to reach to perform a given task. \dot{v}^{*} is often computed through a standard PID controller given a desired trajectory (X^{traj}, v^{traj},\dot{v}^{traj}) (e.g v^{*} = kp(X^{traj} - X )+kd ( v^{traj} - v ) + \dot{v}^{traj} ) . || \dot{v}^{*} - \dot{v}(\tau) ||^2_2 is considered as the main robot task.
Regularization task
\omega_{reg}|| \tau - g(q) - kd \dot{q} ||^2_2 is called a regularization task. When n>m the robot is redundant relatively to its task. In such situation, the main robot task doesn't constrain all the robot degrees of freedom. The regularization task is used to prevent such case. In the current case, this task uses the set of redundant axis to compensate for gravity. A damping term is added to add virtual viscuous friction to prevent unwanted motion of the redundant axis due to the robot dynamic. \omega_{reg} is a small weight (\approx 1e^{-5}) used to ensure that the regularization task doesn't interfere with the main task (the main task has a weight of 1).
Constraints
A \dot{q} \leq b represent the constraints the robot is subject to. These constraint are expressed linearly relatively to the optimization variable \dot{q}. The implemented constraints in torque_qp are:
q^{min} \leq q_{k+1}(\dot{q}) \leq q^{max}\dot{q}^{min} \leq \dot{q}_{k+1} \leq \dot{q}^{max}\tau^{min} \leq \tau_{k+1} \leq \tau^{max}\dot{\tau}^{min} \leq \dot{\tau}_{k+1} \leq \dot{\tau}^{max}
\dot{q}(\tau) is approximate using a Taylor expansion such that : \dot{q}_{k+1} = \dot{q}_{k} +\ddot{q}_{k+1} dt.
With the equation of motion M(q) \ddot{q} + b(q,\dot{q}) + g(q) = \tau, \dot{q} can be expressed as a function of \tau.
The same reasonning applies for q(\tau).
Finally, \dot{\tau}_{k+1} = \frac{\tau_{k+1} - \tau_{k}}{dt}