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

#ifndef CTASKOPPOSCONTACTFORCE_HPP_
#define CTASKOPPOSCONTACTFORCE_HPP_

#include <string>
#include <vector>

#include <scl/DataTypes.hpp>

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

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

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


namespace scl
{
class CTaskOpPosContactForce : public scl::CTaskBase
{
public:
  /********************************
   * CTaskBase API
   *********************************/
  virtual bool init(STaskBase* arg_task_data,
      CDynamicsBase* arg_dynamics);

  virtual STaskBase* getTaskData();

  virtual void reset();

  virtual bool computeServo(const SRobotSensors* arg_sensors);

  virtual bool computeModel(const SRobotSensors* arg_sensors);

  /********************************
   * CTaskOpPosContactForce specific functions
   *********************************/
  CTaskOpPosContactForce();

  virtual ~CTaskOpPosContactForce(){}

  inline void setGoal(const Eigen::VectorXd & arg_goal)
  { data_->x_goal_ = arg_goal;  }

  inline void setGoalVel(const Eigen::VectorXd & arg_goal)
  { data_->dx_goal_ = arg_goal;  }

  inline void setGoalAcc(const Eigen::VectorXd & arg_goal)
  { data_->ddx_goal_ = arg_goal;  }

  sBool achievedGoalPos();

private:
  STaskOpPosContactForce* data_;

  Eigen::VectorXd tmp1, tmp2;

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

  sBool lambda_inv_singular_;

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

}

#endif /* CTASKOPPOSCONTACTFORCE_HPP_ */