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

#ifndef SGCMODEL_HPP_
#define SGCMODEL_HPP_

#include <scl/data_structs/SRobotParsed.hpp>
#include <scl/data_structs/SRigidBodyDyn.hpp>
#include <scl/DataTypes.hpp>

#include <Eigen/Dense>

#include <vector>

namespace scl
{
  class SGcModel : public SObject
  {
    /* *********************************************************************
     *                                 Data
     * ********************************************************************* */
  public:
    // Eigen requires redefining the new operator for classes that contain fixed size Eigen member-data.
    // See http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Eigen::MatrixXd M_gc_;

    Eigen::MatrixXd M_gc_inv_;

    Eigen::VectorXd force_gc_cc_;

    Eigen::VectorXd force_gc_grav_;

    Eigen::VectorXd q_,dq_;

    Eigen::Vector3d pos_com_;

    sFloat mass_;

    std::vector<std::string> processing_order_;

    bool computed_spatial_transformation_and_inertia_; 

    sutil::CMappedTree<std::string, SRigidBodyDyn> rbdyn_tree_;

    Eigen::VectorXd vec_scratch_[5];

    /* *********************************************************************
     *                      Initialization functions
     * ********************************************************************* */
    SGcModel();

    sBool init(const SRobotParsed& arg_robot_data);
  };
}

#endif /* SGCMODEL_HPP_ */