CDynamicsSclSpatial.hpp

/* This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
/*
 * CDynamicsSclSpatial.hpp
 *
 *  Created on: Jun 2, 2014
 *      Author: Nayan Singhal <singhalnayan91@gmail.com>
 *              Brains in Silicon Lab,
 *              Stanford University.
 */

#ifndef CDYNAMICSSCLSPATIAL_HPP_
#define CDYNAMICSSCLSPATIAL_HPP_

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

#include <Eigen/Geometry>
#include <Eigen/Core>

namespace scl_ext
{
  class CDynamicsSclSpatial : public scl::CDynamicsBase
  {
  public:
    /* *******************************************************************
     *                      Dynamics Algorithms
     * ******************************************************************* */
    bool forwardDynamicsCRBA(
        const scl::SRobotIO *arg_io_data,
        scl::SGcModel *arg_gc_model,
        Eigen::VectorXd &ret_ddq);

    bool forwardDynamicsABA(
        const scl::SRobotIO *arg_io_data,
        scl::SGcModel *arg_gc_model,
        Eigen::VectorXd &ret_ddq);

    bool inverseDynamicsNER(
        const scl::SRobotIO *arg_io_data,
        scl::SGcModel *arg_gc_model,
        Eigen::VectorXd &ret_fgc);



    /* *******************************************************************
     *                      Integrator functions.
     * ******************************************************************* */
    virtual scl::sBool integrate(
        scl::SRobotIO& arg_inputs,
        const scl::sFloat arg_time_interval)
    { return false; }

    bool integrator(
        scl::SRobotIO &arg_io_data,
        scl::SGcModel *arg_gc_model,
        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)
    { return false; }

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

    bool calculateKineticEnergy(
        const scl::SRobotIO *arg_io_data,
        scl::SGcModel *arg_gc_model,
        scl::sFloat &ret_kinetic_energy);

    bool calculatePotentialEnergy(
        const scl::SRobotIO *arg_io_data,
        scl::SGcModel *arg_gc_model,
        scl::sFloat &ret_potential_energy);

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

    /* *******************************************************************
     *                      Initialization functions.
     * ******************************************************************* */
    virtual bool init(const scl::SRobotParsed& arg_robot_data)
    {
      if(arg_robot_data.has_been_init_){
        robot_parsed_data_ = & arg_robot_data;
        has_been_init_ = true;
        return true;
      }
      return false;
    }

    CDynamicsSclSpatial();

    virtual ~CDynamicsSclSpatial();
  };

} /* namespace scl_ext */

#endif /* CDYNAMICSSCLSPATIAL_HPP_ */