CartesianImpedanceController.hpp

/*
    Copyright (c) Tobias Löw
    Written by Tobias Löw <https://tobiloew.ch>
 
    This file is part of orwell.
 
    gafro is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License version 3 as
    published by the Free Software Foundation.
 
    gafro is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.
 
    You should have received a copy of the GNU General Public License
    along with gafro. If not, see <http://www.gnu.org/licenses/>.
*/
 
#pragma once
 
#include <orwell/torque/TorqueController.hpp>
 
namespace orwell
{
 
    template <int dof, class Reference>
    class CartesianImpedanceController : public TorqueReferenceController<dof, Reference>
    {
      public:
        CartesianImpedanceController(const sackmesser::Interface::Ptr &interface, const std::string &name);
 
        virtual ~CartesianImpedanceController();
 
        typename RobotState<dof>::Vector computeCommand();
 
      protected:
        void setPositionError(const Eigen::Vector<double, 6> &position_error);
 
        void setVelocityError(const Eigen::Vector<double, 6> &velocity_error);
 
        void setAccelerationError(const Eigen::Vector<double, 6> &acceleration_error);
 
        Eigen::Vector<double, 6> limitWrench(const Eigen::Vector<double, 6> &wrench);
 
        virtual void computeStateError() = 0;
 
      private:
        Eigen::Vector<double, 6> getDesiredWrench();
 
      private:
        struct Configuration : public sackmesser::Configuration
        {
            bool load(const std::string &ns, const std::shared_ptr<sackmesser::Configurations> &server);
 
            Eigen::Matrix<double, 6, 6> stiffness_;
 
            Eigen::Matrix<double, 6, 6> damping_;
 
            Eigen::Matrix<double, 6, 6> inertia_;
 
            Eigen::Vector<double, 6> wrench_limits_;
        } config_;
 
        Eigen::Vector<double, 6> position_error_;
 
        Eigen::Vector<double, 6> velocity_error_;
 
        Eigen::Vector<double, 6> acceleration_error_;
    };
 
}  // namespace orwell
 
#include <orwell/torque/CartesianImpedanceController.hxx>