Initial commit

.gitlab-ci.yml

+stages:
+  - build 
+
+# builds  
+build:noetic:
+  stage: build
+  tags:
+    - docker
+  image: noetic
+  script:
+    - apt update
+    - rm -rf /home/ci/auctus_ws/ci/*
+    - mkdir -p /home/ci/auctus_ws/ci/src/torque_qp
+    - mv * /home/ci/auctus_ws/ci/src/torque_qp 
+    - cd /home/ci/auctus_ws/ci/src
+    - git clone https://github.com/kuka-isir/qpOASES.git
+    - git clone https://gitlab.inria.fr/auctus-team/components/control/qp_solver.git
+    - rosdep install --from-paths ../src --ignore-src -r -y
+    - cd /home/ci/auctus_ws/ci/
+    - catkin config --init  
+    - catkin build

CMakeLists.txt

+cmake_minimum_required(VERSION 3.5)
+project(acceleration_qp VERSION 0.0.1 LANGUAGES CXX )
+
+set(CMAKE_BUILD_TYPE Release)
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(Eigen3 REQUIRED)
+find_package(pinocchio REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+  qp_solver
+)
+
+include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
+
+catkin_package(
+  LIBRARIES ${PROJECT_NAME} 
+  INCLUDE_DIRS include 
+  CATKIN_DEPENDS
+  qp_solver
+  DEPENDS
+  pinocchio
+)
+
+add_library(${PROJECT_NAME} 
+  src/controller.cpp
+)
+
+add_dependencies(${PROJECT_NAME}
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+target_link_libraries(${PROJECT_NAME}
+  pinocchio::pinocchio
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}  
+)
+
+target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
+  include
+  ${EIGEN3_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
\ No newline at end of file

README.md

+[![pipeline status](https://gitlab.inria.fr/auctus-team/components/torque_qp/badges/master/pipeline.svg)](https://gitlab.inria.fr/auctus/panda/torque_qp)
+<!-- [![Quality Gate Status](https://sonarqube.inria.fr/sonarqube/api/project_badges/measure?project=auctus%3Apanda%3Atorque-qp&metric=alert_status)](https://sonarqube.inria.fr/sonarqube/dashboard?id=auctus%3Apanda%3Atorque-qp)
+[![Coverage](https://sonarqube.inria.fr/sonarqube/api/project_badges/measure?project=auctus%3Apanda%3Atorque-qp&metric=coverage)](https://sonarqube.inria.fr/sonarqube/dashboard?id=auctus%3Apanda%3Atorque-qp)
+
+# Links
+- Sonarqube : https://sonarqube.inria.fr/sonarqube/dashboard?id=auctus%3Apanda%3Atorque-qp
+- Documentation : https://auctus.gitlabpages.inria.fr/panda/torque_qp/index.html -->
+
+
+
+# Velocity 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](https://gitlab.inria.fr/auctus-team/components/robots/panda/panda_qp_control).
+
+
+# Installation
+
+Currently, torque_qp needs `catkin` for its installation. The installation procedure is the following 
+
+```terminal
+# 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](https://github.com/stack-of-tasks/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 :
+
+```math
+
+\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, $`J(q) \in \mathbb{R}^{m*n}`$ is the robot Jacobian and $`v^{*}`$ is a target Cartesian velocity that one want to reach to perform a given task. $`v^{*}`$ is often computed through a standard PID controller given a desired trajectory ($`X^{traj}, v^{traj}`$) (e.g $`v^{*} = kp(X-X^{traj})+v^{traj}`$ ) . $` || v^{*} - J(q)\dot{q} ||^2_2 `$ is considered as the main robot task.
+
+## Regularization task
+
+$`\omega_{reg}|| k_{p,reg} (q_{ref} - 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, $`q_{ref}\in \mathbb{R}^n`$ is a reference configuration, $`k_{p,reg}`$ is a proprotionnal controller gain. $`|| k_{p,reg} (q_{ref} - q) ||^2_2 `$ is thus a task pushing the robot toward a desired configuration. $`\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}`$ 
+
+$`q(\dot{q})`$ is approximate using a Taylor expansion such that : $`q_{k+1} = q_{k} +\dot{q}_{k+1} dt`$
+

include/acceleration_qp/controller.h

+/*
+ * Copyright 2020 <copyright holder> <email>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#pragma once
+
+// Pinocchio
+#include "pinocchio/fwd.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/aba.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/model.hpp"
+
+// Eigen
+#include <Eigen/Dense>
+
+
+// qpOASES
+#include <qp_solver/qp_solver.hpp>
+
+namespace QPController
+{
+class Acceleration
+{
+public:
+    /**
+    * \fn bool init
+    * \brief Initializes the controller
+    * \param ros::NodeHandle& node_handle a ros node handle
+    * \param Eigen::VectorXd q_init the robot initial joint position
+    * \param Eigen::VectorXd qd_init the robot initial joint velocity
+    * \return true if the controller is correctly initialized
+    */
+    bool init(std::string robot_description, std::vector<std::string> joint_names) ;
+
+    /**
+     * \fn Eigen::VectorXd update
+     * \brief Update the controller to get the new desired joint torque to send to the robot.
+     * \param Eigen::VectorXd q the current joint position of the robot
+     * \param Eigen::VectorXd qd the current joint velocity of the robot
+     * \param const ros::Duration& period the refresh rate of the control
+     * \return A Eigen::VectorXd with the desired joint torque
+     */
+    Eigen::VectorXd update(Eigen::VectorXd q, Eigen::VectorXd qd, Eigen::VectorXd qdot_d , Eigen::VectorXd q2dot_d,Eigen::Matrix<double,6,1> xd_star);
+
+    template<typename T>
+    bool is_in_vector(const std::vector<T> & vector, const T & elt)
+    {
+      return vector.end() != std::find(vector.begin(),vector.end(),elt);
+    }
+
+    void setControlledFrame(std::string controlled_frame);
+
+    void setq2dotMax(Eigen::VectorXd q2dot_max);
+
+    void setq3dotMax(Eigen::VectorXd q3dot_max);
+
+    void setRegularizationWeight(double regularisation_weight);
+
+    void setRegularizationConfiguration(Eigen::VectorXd  q_mean);
+
+    void setControlPeriod(double  control_period);
+
+    void setRegularizationGains(Eigen::VectorXd regularisation_gains);
+
+    pinocchio::Model getRobotModel();
+    std::string getControlledFrame();
+
+    /**
+    * \fn bool load_robot 
+    * \brief Loads Panda robot
+    * \param ros::NodeHandle& node_handle a ros node handle
+    * \return true if the robot is correctly loaded
+    */
+    bool load_robot_model(std::string robot_description, std::vector<std::string> joint_names);
+
+    void getQPLimits(Eigen::VectorXd& q3dot_lim, Eigen::VectorXd& q2dot_lim, Eigen::VectorXd& qdot_lim, Eigen::VectorXd& q_lim_lower, Eigen::VectorXd& q_lim_upper  );
+    void getQPConstraints(Eigen::VectorXd& q3dot_constraint, Eigen::VectorXd& q2dot_constraint, Eigen::VectorXd& qdot_constraint, Eigen::VectorXd& q_constraint);
+
+  private:
+    // Publishers
+    pinocchio::Model model;
+    pinocchio::Data data;
+
+    pinocchio::Motion xd;
+    Eigen::VectorXd nonLinearTerms; /*!< @brief Matrix representing M^{-1} ( g(q) + c(q,qd) ) */
+    pinocchio::Data::Matrix6x J; /*!< @brief Jacobian matrix */
+    Eigen::Matrix<double, 6, 1> Jdot_qdot;
+    std::string controlled_frame; /*!< @brief tip link of the KDL chain (usually the end effector*/
+    bool control_frame_set = false;
+    double control_period;
+    double regularisation_weight; /*!< @brief Regularisation weight */
+    Eigen::VectorXd kp_reg; /*!< @brief Proportional gain for the damping in the regularisation term */
+    Eigen::VectorXd q_mean; /*!< @brief Mean joint position of the robot (for the regularization task*/
+
+    QPSolver qp_solver;
+    QPSolver::qpProblem qp;
+    int number_of_constraints; /*!< @brief Number of constraints of the QP problem*/
+    int number_of_variables; /*!< @brief Number of optimization variables of the QP problem*/
+    Eigen::Matrix<double, 6, Eigen::Dynamic> a; /*!< @brief Matrix to ease comprehension */
+    Eigen::Matrix<double, 6, 1> b; /*!< @brief Matrix to ease comprehension */
+    Eigen::VectorXd q2dot_max;
+    Eigen::VectorXd q3dot_max;
+    Eigen::VectorXd dtorque_max;
+    Eigen::VectorXd q3dot_constraint;
+    Eigen::VectorXd q2dot_constraint;
+    Eigen::VectorXd qdot_constraint;
+    Eigen::VectorXd q_constraint;
+    Eigen::VectorXd qp_solution;
+
+};
+}  // namespace QPController
+

package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>acceleration_qp</name>
+  <version>0.0.1</version>
+  <description>Interface to talk with a Panda robot</description>
+  <maintainer email="lucas.joseph@inria.fr">Lucas Joseph</maintainer>
+  <license>Apache 2.0</license>
+
+  <author>Franka Emika GmbH</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>pinocchio</depend>
+  <depend>qp_solver</depend>
+ 
+</package>

src/controller.cpp

+#include "acceleration_qp/controller.h"
+
+namespace QPController
+{
+bool Acceleration::init(std::string robot_description, std::vector<std::string> joint_names) 
+{
+    //--------------------------------------
+    // LOAD ROBOT
+    //--------------------------------------
+    if (!load_robot_model(robot_description,joint_names))
+        return false;
+    if (!control_frame_set)
+        controlled_frame = model.frames[model.nframes-1].name;
+    setRegularizationWeight(1e-5);
+    setControlPeriod(0.001);
+    kp_reg = Eigen::MatrixXd::Ones(7,1) * 1.0; 
+    q_mean = (model.upperPositionLimit + model.lowerPositionLimit) / 2.0 ;
+
+    //--------------------------------------
+    // QPOASES
+    //--------------------------------------
+    number_of_variables = model.nv;
+    // number_of_constraints = 3 * model.nv;
+    number_of_constraints = 0;
+    q3dot_constraint.resize(model.nv);
+    q2dot_constraint.resize(model.nv);
+    qdot_constraint.resize(model.nv);
+    q_constraint.resize(model.nv);
+    // qp_solution.resize(model.nv);
+    qp = qp_solver.configureQP(number_of_variables, number_of_constraints);
+    return true;
+}
+
+Eigen::VectorXd Acceleration::update(Eigen::VectorXd q, Eigen::VectorXd qdot, Eigen::VectorXd qdot_d, Eigen::VectorXd q2dot_d, Eigen::Matrix<double,6,1> xdd_star)
+{
+    // Update the model
+    // First calls the forwardKinematics on the model, then computes the placement of each frame.
+    pinocchio::forwardKinematics(model,data,q,qdot,0.0*qdot);
+    pinocchio::updateFramePlacements(model,data);
+    pinocchio::computeJointJacobians(model,data,q);
+    pinocchio::getFrameJacobian(model, data, model.getFrameId(controlled_frame), pinocchio::ReferenceFrame::WORLD, J);
+    Jdot_qdot = pinocchio::getFrameClassicalAcceleration(model, data, model.getFrameId(controlled_frame),pinocchio::ReferenceFrame::WORLD).toVector();
+
+    // Formulate QP problem such that
+    // joint_torque_out = argmin 1/2 tau^T H tau + tau^T g_
+    //                       s.t     lbA < A tau < ubA
+    //                                   lb < tau < ub 
+    
+      // Regularisation task
+    qp.hessian =  2.0 * regularisation_weight * Eigen::MatrixXd::Identity(model.nv,model.nv);
+    qp.gradient = -2.0 * regularisation_weight * (kp_reg.cwiseProduct((q_mean- q)) - 2.0* (kp_reg.cwiseSqrt()).cwiseProduct(qdot) );
+    
+    a.noalias() = J ;
+    b.noalias() =  Jdot_qdot - xdd_star;
+
+    qp.hessian  +=  2.0 * a.transpose() *  a;
+    qp.gradient +=  2.0 * a.transpose() * b;    
+
+    double horizon_dt = 1.0 * control_period;
+
+    qp.ub = ( q3dot_max * horizon_dt + q2dot_d ).cwiseMin
+            ( q2dot_max ).cwiseMin
+            ( (model.velocityLimit - qdot_d)/horizon_dt ).cwiseMin
+            ( 2.0 * ( model.upperPositionLimit - q - qdot_d * horizon_dt ) / ( horizon_dt * horizon_dt) );
+
+    qp.lb = (-q3dot_max * horizon_dt + q2dot_d ).cwiseMax
+            (-q2dot_max ).cwiseMax
+            ( (-model.velocityLimit - qdot_d)/horizon_dt ).cwiseMax
+            ( 2.0 * ( model.lowerPositionLimit - q - qdot_d * horizon_dt ) / (horizon_dt * horizon_dt) );
+    
+    // qp.ub = q2dot_max;
+    // qp.lb = -q2dot_max;
+    
+    // qp.a_constraints.block(0, 0, model.nv, model.nv) =  Eigen::MatrixXd::Identity(model.nv,model.nv);
+    // qp.a_constraints.block(model.nv, 0, model.nv, model.nv) =  Eigen::MatrixXd::Identity(model.nv,model.nv);
+    // qp.a_constraints.block(2*model.nv, 0, model.nv, model.nv) =  Eigen::MatrixXd::Identity(model.nv,model.nv);
+    
+    // qp.lb_a.block(0, 0, model.nv, 1) = -q3dot_max * horizon_dt + q2dot_d;
+    // qp.lb_a.block(model.nv, 0, model.nv, 1) = (-model.velocityLimit - qdot_d)/horizon_dt;
+    // qp.lb_a.block(2*model.nv, 0, model.nv, 1) = 2.0 * ( model.lowerPositionLimit - q - qdot_d * horizon_dt ) / (horizon_dt * horizon_dt);
+
+    // qp.ub_a.block(0, 0, model.nv, 1) =  q3dot_max * horizon_dt + q2dot_d;
+    // qp.ub_a.block(model.nv, 0, model.nv, 1) = (model.velocityLimit - qdot_d)/horizon_dt;
+    // qp.ub_a.block(2*model.nv, 0, model.nv, 1) = 2.0 * ( model.upperPositionLimit - q - qdot_d * horizon_dt ) / (horizon_dt * horizon_dt);
+
+    qp_solution = qp_solver.SolveQP(qp);
+
+    q3dot_constraint = (qp_solution - q2dot_d) / horizon_dt;
+    q2dot_constraint = qp_solution;
+    qdot_constraint = qdot_d + qp_solution * horizon_dt;
+    q_constraint = q + qdot_d * horizon_dt + 0.5 * qp_solution * horizon_dt * horizon_dt;
+
+    return qp_solution;
+}
+
+void Acceleration::getQPConstraints(Eigen::VectorXd& q3dot_constraint, Eigen::VectorXd& q2dot_constraint, Eigen::VectorXd& qdot_constraint, Eigen::VectorXd& q_constraint )
+{
+    q3dot_constraint = this->q3dot_constraint;
+    q2dot_constraint = this->q2dot_constraint;
+    qdot_constraint = this->qdot_constraint;
+    q_constraint = this->q_constraint;
+}
+
+void Acceleration::getQPLimits(Eigen::VectorXd& q3dot_lim, Eigen::VectorXd& q2dot_lim, Eigen::VectorXd& qdot_lim, Eigen::VectorXd& q_lim_lower, Eigen::VectorXd& q_lim_upper )
+{
+    q3dot_lim= this->q3dot_max;
+    q2dot_lim= this->q2dot_max;
+    qdot_lim= this->model.velocityLimit;
+    q_lim_lower = model.lowerPositionLimit;
+    q_lim_upper = model.upperPositionLimit;
+}
+
+
+bool Acceleration::load_robot_model(std::string robot_description, std::vector<std::string> joint_names)
+{
+    // Load the urdf model
+    pinocchio::urdf::buildModelFromXML(robot_description,model,false);
+    if (!joint_names.empty())
+    {
+        std::vector<pinocchio::JointIndex> list_of_joints_to_keep_unlocked_by_id;
+        for(std::vector<std::string>::const_iterator it = joint_names.begin();
+            it != joint_names.end(); ++it)
+        {
+            const std::string & joint_name = *it;
+            if(model.existJointName(joint_name))
+            list_of_joints_to_keep_unlocked_by_id.push_back(model.getJointId(joint_name));
+            else
+            std::cout << "joint: " << joint_name << " does not belong to the model.";
+        }
+        
+        // Transform the list into a list of joints to lock
+        std::vector<pinocchio::JointIndex> list_of_joints_to_lock_by_id;
+        for(pinocchio::JointIndex joint_id = 1; joint_id < model.joints.size(); ++joint_id)
+        {
+            const std::string joint_name = model.names[joint_id];
+            if(is_in_vector(joint_names,joint_name))
+            continue;
+            else
+            {
+            list_of_joints_to_lock_by_id.push_back(joint_id);
+            }
+        }
+        
+        // Build the reduced model from the list of lock joints
+        Eigen::VectorXd q_rand = randomConfiguration(model);
+        model = pinocchio::buildReducedModel(model,list_of_joints_to_lock_by_id,q_rand);
+    }
+    data = pinocchio::Data(model);
+
+    // Resize and initialise varialbes 
+    J.resize(6,model.nv);
+    J.setZero();
+    nonLinearTerms.resize(model.nv);
+    a.resize(6,model.nv);
+
+    return true;
+}
+
+pinocchio::Model Acceleration::getRobotModel()
+{
+    return model;
+}
+
+void Acceleration::setControlledFrame(std::string controlled_frame)
+{
+    this->controlled_frame = controlled_frame; 
+    control_frame_set = true;
+}
+
+std::string Acceleration::getControlledFrame()
+{
+    return controlled_frame;
+}
+
+void Acceleration::setRegularizationWeight(double regularisation_weight)
+{
+    this->regularisation_weight = regularisation_weight; 
+}
+
+void Acceleration::setq2dotMax(Eigen::VectorXd q2dot_max)
+{
+    this->q2dot_max = q2dot_max; 
+}
+
+void Acceleration::setq3dotMax(Eigen::VectorXd q3dot_max)
+{
+    this->q3dot_max = q3dot_max; 
+}
+
+
+void Acceleration::setRegularizationGains(Eigen::VectorXd regularisation_gains)
+{
+    this->kp_reg = regularisation_gains; 
+}
+
+
+void Acceleration::setControlPeriod(double control_period)
+{
+    this->control_period = control_period; 
+}
+
+
+} // namespace QPController

torque_qp.rosinstall

+# IT IS UNLIKELY YOU WANT TO EDIT THIS FILE BY HAND,
+# UNLESS FOR REMOVING ENTRIES.
+# IF YOU WANT TO CHANGE THE ROS ENVIRONMENT VARIABLES
+# USE THE rosinstall TOOL INSTEAD.
+# IF YOU CHANGE IT, USE rosinstall FOR THE CHANGES TO TAKE EFFECT
+
+
+- git: {local-name: franka_description, uri: 'https://gitlab.inria.fr/auctus/panda/franka_description.git'}
+- git: {local-name: panda_traj, uri: 'https://gitlab.inria.fr/auctus/panda/panda_traj.git',  version: 'hanoi'}
+- git: {local-name: qpOASES, uri: 'https://github.com/kuka-isir/qpOASES.git'}
+- git: {local-name: qp_solver, uri: 'https://gitlab.inria.fr/auctus/panda/qp_solver.git'}