CSimbodyDynamics.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 CSimbodyDynamics.hpp
 *
 *  Created on: Jul 24, 2012
 *
 *  Copyright (C) 2012
 *
 *  Author: Samir Menon <smenon@stanford.edu>
 */

#ifndef CSIMBODYDYNAMICS_HPP_
#define CSIMBODYDYNAMICS_HPP_

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

#include <Simbody.h>

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

namespace scl
{
  class CSimbodyDynamics : public CDynamicsBase
  {
  public:
    virtual bool init(const SRobotParsed& arg_robot_data);

    virtual bool computeGCModel(
        SRobotSensors const * sensor_data,
        SGcModel * js_model);

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

    virtual sBool computeTransform_Depracated(
        const void* arg_link_id,
        Eigen::Affine3d& arg_T);

    virtual sBool computeJacobian_Depracated(
        const void* arg_link_id,
        const Eigen::VectorXd& arg_pos_global,
        Eigen::MatrixXd& arg_J);

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

    virtual sFloat getKineticEnergy_Depracated();

    virtual sFloat getPotentialEnergy_Depracated();

    CSimbodyDynamics();

    virtual ~CSimbodyDynamics();

  private:
    sBool addNonRootLink(SimTK::MobilizedBody& arg_parent, const SRigidBody& arg_child_lnk);

    std::string robot_name_;

    size_t ndof_;

    Eigen::Vector3d gravity_;

    SimTK::MultibodySystem simbody_system_;
    SimTK::SimbodyMatterSubsystem simbody_matter_;
    SimTK::GeneralForceSubsystem simbody_forces_;
    SimTK::RungeKuttaMersonIntegrator* simbody_rkm_integ_;
    SimTK::TimeStepper* simbody_ts_;
    SimTK::State simbody_state_;
  };

} /* namespace scl */
#endif /* CSIMBODYDYNAMICS_HPP_ */