CTaskComPos.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 CTaskComPos.hpp
 *
 *  Created on: Sep 2, 2012
 *
 *  Copyright (C) 2012
 *
 *  Author: Samir Menon <smenon@stanford.edu>
 */

#ifndef CCOMPOSTASK_HPP_
#define CCOMPOSTASK_HPP_

#include <scl/DataTypes.hpp>
#include <scl/control/task/tasks/data_structs/STaskComPos.hpp>
#include <scl/control/task/CTaskBase.hpp>

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

#include <string>
#include <vector>

namespace scl
{
  class CTaskComPos : 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; }

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

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

    virtual ~CTaskComPos(){}

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

    virtual void reset();

  private:
    STaskComPos* data_;

    Eigen::VectorXd tmp1, tmp2;

    Eigen::Matrix3d J_premultiplier_;

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

    sBool lambda_inv_singular_;

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

} /* namespace scl */
#endif /* CCOMPOSTASK_HPP_ */