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

#ifndef CDYNAMICSTAO_HPP_
#define CDYNAMICSTAO_HPP_

#include <scl/DataTypes.hpp>
#include <scl/dynamics/CDynamicsBase.hpp>
#include <scl/dynamics/tao/jspace/State.hpp>
#include <scl/dynamics/tao/tao/dynamics/taoNode.h>
#include <scl/dynamics/tao/tao/dynamics/taoJoint.h>
#include <scl/dynamics/tao/tao/dynamics/taoDynamics.h>

#include <Eigen/Dense>

#include <string>


class taoDNode;

namespace jspace {
  struct STaoTreeInfo;
  class Model;
}

namespace scl
{

  class SGcModel;
  struct SRobotSensors;

  class CDynamicsTao : public CDynamicsBase
  {
  public:
    /* *******************************************************************
     *                      Computational functions.
     * ******************************************************************* */
    virtual bool computeGCModel(
        SRobotSensors const * arg_sensor_data,
        SGcModel * arg_gc_model)
    { return false; }

    taoDNode * getTaoIdForLink(std::string arg_link_name);

    virtual sBool integrate(
        SRobotIO& arg_inputs,
        const sFloat arg_time_interval);

    /* *******************************************************************
     *                      Dynamics State functions.
     * ******************************************************************* */
    sBool setGeneralizedCoordinates(Eigen::VectorXd &arg_q);

    sBool setGeneralizedVelocities(Eigen::VectorXd &arg_dq);

    sBool setGeneralizedForces(Eigen::VectorXd &arg_fgc);

    /* *******************************************************************
     *                      Initialization functions.
     * ******************************************************************* */
    CDynamicsTao();
    virtual ~CDynamicsTao();

    virtual bool init(const SRobotParsed& arg_robot_data);

  private:
    std::string robot_name_;

    taoNodeRoot *tao_tree_q_root_, *tao_tree_q_dq_root_;

    jspace::Model * model_;

    size_t ndof_;

    jspace::State state_;

    Eigen::Vector3d gravity_;
  };

}

#endif /* CDYNAMICSTAO_HPP_ */