CTaskOpPos.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 CTaskOpPos.hpp
 *
 *  Created on: Aug 19, 2010
 *
 *  Copyright (C) 2010
 *
 *  Author: Samir Menon <smenon@stanford.edu>
 */

#ifndef COPPOINTTASK_HPP_
#define COPPOINTTASK_HPP_

#include <string>
#include <vector>

#include <scl/DataTypes.hpp>

#include <scl/control/task/tasks/data_structs/STaskOpPos.hpp>

#include <scl/control/task/CTaskBase.hpp>

#include <Eigen/Dense>
#include <Eigen/SVD>


namespace scl
{
class CTaskOpPos : public scl::CTaskBase
{
public:
  /********************************
   * CTaskBase API
   *********************************/
  virtual bool computeServo(const SRobotSensors* arg_sensors);

  virtual bool computeModel(const SRobotSensors* arg_sensors);

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

  virtual bool setGoalPos(const Eigen::VectorXd & arg_goal);

  virtual bool setGoalVel(const Eigen::VectorXd & arg_goal);

  virtual bool setGoalAcc(const Eigen::VectorXd & arg_goal);

  virtual bool getGoalPos(Eigen::VectorXd & arg_goal) const
  { arg_goal = data_->x_goal_; return true; }

  virtual bool getGoalVel(Eigen::VectorXd & arg_goal) const
  { arg_goal = data_->dx_goal_; return true; }

  virtual bool getGoalAcc(Eigen::VectorXd & arg_goal) const
  { arg_goal = data_->ddx_goal_; return true; }

  virtual bool getPos(Eigen::VectorXd & arg_pos) const
  { arg_pos = data_->x_; return true; }

  virtual bool getVel(Eigen::VectorXd & arg_vel) const
  { arg_vel = data_->dx_; return true; }

  virtual bool getAcc(Eigen::VectorXd & arg_acc) const
  { arg_acc = data_->ddx_; return true; }

  /* *******************************
   * CTaskOpPos specific functions
   ******************************** */
  sBool achievedGoalPos();

  void setFlagComputeOpPosGravity(sBool arg_compute_grav)
  { flag_compute_gravity_ = arg_compute_grav; }

  /* *******************************
   * Initialization specific functions
   ******************************** */
  CTaskOpPos();

  virtual ~CTaskOpPos(){}

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

  virtual void reset();

protected:
  STaskOpPos* data_;

  Eigen::VectorXd tmp1, tmp2;

  Eigen::ColPivHouseholderQR<Eigen::Matrix3d> qr_;

  sBool lambda_inv_singular_;

  Eigen::JacobiSVD<Eigen::Matrix3d > svd_;
  Eigen::Matrix3d singular_values_;

  sBool flag_compute_gravity_;
};

}

#endif /* CMARKERTRACKTASK_HPP_ */