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

#ifndef CDYNAMICSBASE_HPP_
#define CDYNAMICSBASE_HPP_

#include <scl/DataTypes.hpp>
#include <scl/data_structs/SRobotIO.hpp>
#include <scl/data_structs/SGcModel.hpp>

#include <Eigen/Dense>
#include <string>

namespace scl {

class CDynamicsBase
{
public:
  /* *******************************************************************
   *                      Computational functions.
   * ******************************************************************* */
  virtual sBool computeGCModel(
      const SRobotSensors * arg_sensor_data,
      SGcModel * arg_gc_model)=0;

  /* *******************************************************************
   *                      Coordinate Transformations
   * ******************************************************************* */
  virtual sBool computeTransformsForAllLinks(
      sutil::CMappedTree<std::string, SRigidBodyDyn> &arg_tree,
      const Eigen::VectorXd& arg_q)
  { return false; }

  virtual sBool computeTransform(
      SRigidBodyDyn& arg_link,
      const Eigen::VectorXd& arg_q)
  { return false; }

  virtual sBool computeTransformToAncestor(
      Eigen::Affine3d& arg_T,
      SRigidBodyDyn& arg_link,
      const SRigidBodyDyn* arg_ancestor,
      const Eigen::VectorXd& arg_q)
  { return false; }

  /* *******************************************************************
   *                      Compute Jacobians
   * ******************************************************************* */
  virtual sBool computeJacobianWithTransforms(
      Eigen::MatrixXd& arg_J,
      SRigidBodyDyn& arg_link,
      const Eigen::VectorXd& arg_q,
      const Eigen::Vector3d& arg_pos_local)
  { return false; }

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

  virtual sBool computeJacobian(
      Eigen::MatrixXd& arg_J,
      const SRigidBodyDyn& arg_link,
      const SRigidBodyDyn* arg_ancestor,
      const Eigen::VectorXd& arg_q,
      const Eigen::Vector3d& arg_pos_local)
  { return false; }

  virtual sBool computeJacobianTrans(
      Eigen::MatrixXd& arg_J,
      const SRigidBodyDyn& arg_link,
      const SRigidBodyDyn* arg_ancestor,
      const Eigen::VectorXd& arg_q,
      const Eigen::Vector3d& arg_pos_local)
  { return false; }

  virtual sBool computeJacobianRot(
      Eigen::MatrixXd& arg_J,
      const SRigidBodyDyn& arg_link,
      const SRigidBodyDyn* arg_ancestor,
      const Eigen::VectorXd& arg_q,
      const Eigen::Vector3d& arg_pos_local)
  { return false; }

  /* *******************************************************************
   *                      Integrator functions.
   * ******************************************************************* */

  virtual sBool computeExternalContacts(
          sutil::CMappedList<std::string, SForce> & arg_contacts)
  {    return false; }

  virtual sBool integrate(
      SRobotIO& arg_inputs,
      const sFloat arg_time_interval)
  { return false; }

  /* *******************************************************************
   *                      Dynamics State functions.
   * ******************************************************************* */
  virtual sFloat computeEnergyKinetic(
      sutil::CMappedTree<std::string, SRigidBodyDyn> &arg_tree,
      const Eigen::VectorXd& arg_q,
      const Eigen::VectorXd& arg_dq)
  { return false; }

  virtual sFloat computeEnergyPotential(
      sutil::CMappedTree<std::string, SRigidBodyDyn> &arg_tree,
      const Eigen::VectorXd& arg_q)
  { return false; }

  /* *******************************************************************
   *                      Initialization functions.
   * ******************************************************************* */
  CDynamicsBase() : has_been_init_(false), robot_parsed_data_(NULL){}

  virtual ~CDynamicsBase(){}

  virtual sBool init(const SRobotParsed& arg_robot_data)=0;

  virtual sBool hasBeenInit() {  return has_been_init_;  }

protected:
  sBool has_been_init_;

  const SRobotParsed* robot_parsed_data_;
};

}

#endif /* CDYNAMICSBASE_HPP_ */