constraint_solver_base.h

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * 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.
 */


#ifndef COB_TWIST_CONTROLLER_CONSTRAINT_SOLVERS_SOLVERS_CONSTRAINT_SOLVER_BASE_H
#define COB_TWIST_CONTROLLER_CONSTRAINT_SOLVERS_SOLVERS_CONSTRAINT_SOLVER_BASE_H

#include <set>
#include <Eigen/Core>
#include <kdl/jntarray.hpp>
#include <boost/shared_ptr.hpp>
#include <cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h>
#include "cob_twist_controller/damping_methods/damping_base.h"
#include "cob_twist_controller/constraints/constraint_base.h"
#include "cob_twist_controller/cob_twist_controller_data_types.h"
#include "cob_twist_controller/task_stack/task_stack_controller.h"

template <typename PINV = PInvBySVD>
class ConstraintSolver
{
    public:
        virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
                                      const JointStates& joint_states) = 0;

        inline void setDamping(boost::shared_ptr<DampingBase>& damping)
        {
            this->damping_ = damping;
        }

        inline void setConstraints(std::set<ConstraintBase_t>& constraints)
        {
            this->constraints_.clear();
            this->constraints_ = constraints;
        }

        inline void clearConstraints()
        {
            this->constraints_.clear();
        }

        virtual void setJacobianData(const Matrix6Xd_t& jacobian_data)
        {
            this->jacobian_data_ = jacobian_data;
        }

        virtual ~ConstraintSolver()
        {
            this->clearConstraints();
        }

        ConstraintSolver(const TwistControllerParams& params,
                         const LimiterParams& limiter_params,
                         TaskStackController_t& task_stack_controller) :
                params_(params),
                limiter_params_(limiter_params),
                task_stack_controller_(task_stack_controller)
        {}

    protected:
        std::set<ConstraintBase_t> constraints_;  
        const TwistControllerParams& params_;  
        const LimiterParams& limiter_params_;  
        Matrix6Xd_t jacobian_data_;  
        boost::shared_ptr<DampingBase> damping_;  
        PINV pinv_calc_;  
        TaskStackController_t& task_stack_controller_;  
};

#endif  // COB_TWIST_CONTROLLER_CONSTRAINT_SOLVERS_SOLVERS_CONSTRAINT_SOLVER_BASE_H