CTaskBase.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 CTaskBase.hpp
 *  Encapsulates a task servo and a task model
 *
 *  Created on: May 5, 2010
 *
 *  Copyright (C) 2010
 *
 *  Author: Samir Menon <smenon@stanford.edu>
 */

#ifndef CTASKBASE_HPP_
#define CTASKBASE_HPP_

#include <scl/DataTypes.hpp>

#include <scl/control/task/data_structs/STaskBase.hpp>
#include <scl/data_structs/SRobotIO.hpp>

#include <scl/dynamics/CDynamicsBase.hpp>

#ifdef DEBUG
#include <cassert>
#endif

namespace scl {

class CTaskBase {
public:
  /* **************************************************************
   *                   Computation Functions
   * ************************************************************** */
  virtual bool computeServo(const SRobotSensors* arg_sensors)=0;

  virtual bool computeModel(const SRobotSensors* arg_sensors)=0;

  /* **************************************************************
   *                   Status Get/Set Functions
   * ************************************************************** */
  virtual STaskBase* getTaskData()=0;

  virtual bool setGoalPos(const Eigen::VectorXd & arg_goal) { return false; }

  virtual bool setGoalVel(const Eigen::VectorXd & arg_goal) { return false; }

  virtual bool setGoalAcc(const Eigen::VectorXd & arg_goal) { return false; }

  virtual bool getGoalPos(Eigen::VectorXd & arg_goal) const { return false; }

  virtual bool getGoalVel(Eigen::VectorXd & arg_goal) const { return false; }

  virtual bool getGoalAcc(Eigen::VectorXd & arg_goal) const { return false; }

  virtual bool getPos(Eigen::VectorXd & arg_pos) const { return false; }

  virtual bool getVel(Eigen::VectorXd & arg_vel) const { return false; }

  virtual bool getAcc(Eigen::VectorXd & arg_acc) const { return false; }

  /* **************************************************************
   *                   Initialization Functions
   * ************************************************************** */
  CTaskBase():
    has_been_init_(false),
    dynamics_(S_NULL) {}

  virtual ~CTaskBase(){}

  virtual bool init(STaskBase* arg_task_data,
      CDynamicsBase* arg_dynamics)=0;

  virtual void reset()=0;

  virtual sBool hasBeenInit() { return has_been_init_;  }

  /* **************************************************************
   *                   Runtime Enable/Disable Functions
   * ************************************************************** */
  virtual sBool setActivated(sBool arg_activate)
  {
    STaskBase* t_ds = getTaskData();
    if(S_NULL == t_ds) { return false; }        //Can't access task data struct.
    if(!t_ds->has_been_init_) { return false; } //Task data struct not initialized
    t_ds->has_been_activated_=arg_activate;
    return true;
  }

  virtual sBool hasBeenActivated()
  {
    STaskBase* t_ds = getTaskData();
    if(S_NULL == t_ds) { return false; } //Can't access task data.
#ifdef DEBUG
    if(!t_ds->has_been_init_)//If it is not initailized, it shouldn't be activated.
    { assert(!t_ds->has_been_activated_);  }
#endif
    return t_ds->has_been_activated_;
  }

protected:
  sBool has_been_init_;

  CDynamicsBase* dynamics_;
};

}

#endif /* CTASKBASE_HPP_ */