SRigidBodyDyn.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 SRigidBodyDyn.hpp
 *
 *  Created on: Oct 7, 2013
 *
 *  Copyright (C) 2013
 *
 *  Author: Samir Menon <smenon@stanford.edu>
 */

#ifndef SRIGIDBODYDYN_HPP_
#define SRIGIDBODYDYN_HPP_

#include <scl/DataTypes.hpp>
#include <scl/data_structs/SRigidBody.hpp>

#include <Eigen/Core>

namespace scl
{
  class SRigidBodyDyn : public SObject
  {
  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

    const SRigidBody* link_ds_;

    Eigen::MatrixXd J_com_;

    //****************************************************************************************
    // For using normal vectors
    // NOTE : This will soon be obsolete!!!

    Eigen::Affine3d T_o_lnk_;

    Eigen::Affine3d T_lnk_;

    sFloat q_T_;

    //****************************************************************************************
    // For using spatial vectors (and multi-dof joints)
    Eigen::VectorXd sp_q_T_, sp_dq_T_;

    sSpatialXForm sp_inertia_;

  Eigen::MatrixXd spatial_velocity_;

  Eigen::MatrixXd spatial_acceleration_;

  Eigen::MatrixXd spatial_force_;


    sSpatialXForm sp_X_within_link_;

    sSpatialXForm sp_X_o_lnk_;

    sutil::CMappedList<std::string,sSpatialXForm> sp_X_joint_;

    Eigen::MatrixXd sp_S_joint_;     
    Eigen::MatrixXd sp_Sorth_joint_; 

    //****************************************************************************************
    //Robot Branching Structure data:
    // (Spanning) Tree structure information: (Enables manual tree parsing)
    std::string parent_name_;
    SRigidBodyDyn* parent_addr_;
    std::vector<SRigidBodyDyn*> child_addrs_;

    // Graph structure information: (Enables manual graph parsing)
    std::vector<SRigidBodyDyn*> gr_parent_names_;
    std::vector<SRigidBodyDyn*> gr_parent_addrs_;
    std::vector<SRigidBodyDyn*> gr_child_addrs_;

    //****************************************************************************************
    SRigidBodyDyn() : SObject("SRigidBodyDyn"),
        link_ds_(S_NULL), q_T_(std::numeric_limits<sFloat>::quiet_NaN()),
        parent_name_(""), parent_addr_(S_NULL) {}
  };

} /* namespace scl */
#endif /* SRIGIDBODYDYN_HPP_ */