STaskBase.hpp

/* This file is part of scl, a control and simulation library
for robots and biomechanical models.

scl is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 3 of the License, or (at your option) any later version.

Alternatively, you can redistribute it and/or
modify it under the terms of the GNU General Public License as
published by the Free Software Foundation; either version 2 of
the License, or (at your option) any later version.

scl 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 Lesser General Public
License and a copy of the GNU General Public License along with
scl. If not, see <http://www.gnu.org/licenses/>.
*/
/* \file STaskBase.hpp
 *
 *  Created on: Dec 25, 2010
 *
 *  Copyright (C) 2010
 *
 *  Author: Samir Menon <smenon@stanford.edu>
 */

#ifndef STASKBASE_HPP_
#define STASKBASE_HPP_

#include <string>

#include <scl/DataTypes.hpp>

#include <scl/data_structs/SObject.hpp>
#include <scl/data_structs/SRobotParsed.hpp>
#include <scl/data_structs/SRobotIO.hpp>

#include <scl/data_structs/SGcModel.hpp>

namespace scl
{
  //Forward declaration allows pointers to the parent controller
  class SControllerMultiTask;

  class STaskBase : public SObject
  {
  public:
    const SControllerMultiTask* parent_controller_;

    std::string type_task_;

    scl::sBool has_been_activated_;

    scl::sBool has_control_null_space_;

    sUInt priority_;

    scl::sUInt dof_task_;

    const SRobotParsed* robot_;

    const SGcModel* gc_model_;

    Eigen::MatrixXd J_, J_6_, J_dyn_inv_;

    Eigen::MatrixXd null_space_;

    Eigen::MatrixXd M_task_, M_task_inv_;

    Eigen::MatrixXd force_task_cc_;

    Eigen::MatrixXd force_task_grav_;

    Eigen::VectorXd force_task_;

    Eigen::VectorXd force_task_max_,force_task_min_;

    Eigen::VectorXd force_gc_;

    Eigen::MatrixXd range_space_;

    Eigen::VectorXd kp_, kv_, ka_, ki_;

    std::vector<sString2> task_nonstd_params_;

    /* *********************************************************************
     *                       External shared data interface
     * ********************************************************************* */
    Eigen::VectorXd shared_data_;

    /* *********************************************************************
     *                            Initialization functions
     * ********************************************************************* */
    STaskBase();

    virtual ~STaskBase(){}

    bool init(const std::string & arg_name,
        const std::string & arg_type,
        const sUInt arg_priority,
        const scl::sUInt arg_task_dof,
        const SRobotParsed* arg_robot_ds,
        /* The remaining variables initialize model_ and servo_ */
        const SGcModel* arg_gc_model,
        const Eigen::VectorXd & arg_kp,
        const Eigen::VectorXd & arg_kv,
        const Eigen::VectorXd & arg_ka,
        const Eigen::VectorXd & arg_ki,
        const Eigen::VectorXd & arg_ftask_max,
        const Eigen::VectorXd & arg_ftask_min,
        const std::vector<scl::sString2>& arg_nonstd_params);

    bool setParentController(const SControllerMultiTask* arg_parent);

    virtual bool initTaskParams()=0;
  };
}
#endif /* STASKBASE_HPP_ */