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

#ifndef CDYNAMICSANALYTICRPP_HPP_
#define CDYNAMICSANALYTICRPP_HPP_

#include <scl/DataTypes.hpp>
#include <scl/dynamics/CDynamicsAnalyticBase.hpp>

namespace scl
{
  class CDynamicsAnalyticRPP : public CDynamicsAnalyticBase
  {
  public:
    /* **************************************************************
     *             CDynamicsAnalyticBase API functions
     * ************************************************************** */
    virtual sBool computeTransformationMatrix(
        const Eigen::VectorXd &arg_q,
        sInt arg_link_id,
        sInt arg_ancestor_link_id,
        Eigen::Affine3d& arg_T);

    virtual sBool computeJacobian(
        const Eigen::VectorXd &arg_q,
        sInt arg_link_id,
        const Eigen::VectorXd& arg_pos_local,
        Eigen::MatrixXd& arg_J)
    { return false; }

    virtual sBool computeGCModel(
        const Eigen::VectorXd &arg_q,
        SGcModel& arg_gc_model)
    { return false; }

    /* **************************************************************
     *               Analytic Computation functions
     * ************************************************************** */

    void computeTOrg_0(const Eigen::VectorXd &arg_q,
          Eigen::Affine3d& arg_T);

    void computeT0_1(const Eigen::VectorXd &arg_q,
          Eigen::Affine3d& arg_T);

    void computeT1_2(const Eigen::VectorXd &arg_q,
          Eigen::Affine3d& arg_T);

    bool computeJcom(const Eigen::VectorXd &arg_q,
        unsigned int arg_link_id, Eigen::MatrixXd& arg_J);

    bool computeMgc(const Eigen::VectorXd &arg_q,
        Eigen::MatrixXd& arg_Mgc);

    /* **************************************************************
     *                   Data access functions
     * ************************************************************** */
    virtual sUInt getIdForLink(std::string arg_link_name)
    {
      if("link0" == arg_link_name)
      { return 0; }
      else if("link1" == arg_link_name)
      { return 1; }
      else if("link2" == arg_link_name)
      { return 2; }
      else
      { return -1; }
    }

    /* **************************************************************
     *                   Initialization functions
     * ************************************************************** */
    CDynamicsAnalyticRPP() : CDynamicsAnalyticBase() {}

    virtual ~CDynamicsAnalyticRPP(){}

    virtual sBool init(const SRobotParsed& arg_robot_data);

    virtual sBool hasBeenInit() {  return has_been_init_;  }

    /* **************************************************************
     *                            Data
     * ************************************************************** */
  protected:
    Eigen::Vector3d pos_root_in_global_org_;
  };

} /* namespace scl */
#endif /* CDYNAMICSANALYTICRPP_HPP_ */