CDynamics3d.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 CDynamics3d.hpp
 *
 *  Created on: Mar 21, 2014
 *
 *  Copyright (C) 2014
 *
 *  Author: Samir Menon <smenon@stanford.edu>
 */

#ifndef CDYNAMICS3D_HPP_
#define CDYNAMICS3D_HPP_

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

#include <Eigen/Dense>

#include <string>

namespace scl
{
  class SGcModel;
  struct SRobotSensors;
}

//Dynamics3d forward declaration
class cDynamicBase;
class cDynamicWorld;

namespace scl_ext
{
  class CDynamics3d : public scl::CDynamicsBase
  {
  public:
    /* *******************************************************************
     *                      Integrator functions.
     * ******************************************************************* */

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

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

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

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

    /* given the name of an object, computes the forces and torques around that object
     * does not work with friction.
     */
    Eigen::Vector3d computeForce(std::string name);
    Eigen::Vector3d computeTorque(std::string name, Eigen::Vector3d pos);
    scl::sBool hasContacted(std::string name);
    scl::sInt getNumContacts(std::string name);

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

    virtual const void * getIdForLink(std::string arg_link_name);



    /* *******************************************************************
     *                      Initialization functions.
     * ******************************************************************* */
    CDynamics3d();

    virtual ~CDynamics3d();

    virtual bool init(const scl::SRobotParsed& arg_robot_data);
    /* Dynamics3d's base node */
    cDynamicBase *c_base;

  private:
    std::string robot_name_;

    size_t ndof_;

    Eigen::Vector3d gravity_;

  };

}

#endif /* CDYNAMICS3D_HPP_ */