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

#ifndef SRIGIDBODY_HPP_
#define SRIGIDBODY_HPP_

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

#include <sutil/CMappedList.hpp>

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

#include <string>
#include <vector>

namespace scl
{

class SRigidBodyGraphics
{
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::VectorXd pos_in_parent_;
  //NOTE TODO : Not using Eigen::Quaternion here because of weird Eigen dynamic_cast errors! Figure them out later.
  Eigen::VectorXd ori_parent_quat_; //x y z real
  Eigen::VectorXd scaling_; //Between 0 and 1. Default is 1.

  sInt collision_type_;//NOTE TODO : Should be an enum

  std::string file_name_;

  enum EGraphicObjectType{ GRAPHIC_TYPE_FILE_OBJ, GRAPHIC_TYPE_SPHERE, GRAPHIC_TYPE_CUBOID, GRAPHIC_TYPE_CYLINDER,
    GRAPHIC_TYPE_NOT_INIT };

  EGraphicObjectType class_;

  double color_[3];

  SRigidBodyGraphics()
  {
    pos_in_parent_.setZero(3);
    ori_parent_quat_.setZero(4);
    ori_parent_quat_(3) = 1;
    scaling_.setConstant(3,1);//x y z scaling. Default = 1
    collision_type_ = 0;
    file_name_ = "";
    class_ = GRAPHIC_TYPE_NOT_INIT;
    color_[0] = 0; color_[1] = 0; color_[2] = 0;
  }
};

struct SRigidBody : 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

  //Constructor@End of the class:
  
  //****************************************************************************************
  //Robot Branching data:
  
  // Identifiers
  std::string robot_name_;
  sInt link_id_;
  
  // Tree structure information: (Enables manual tree parsing)
  std::string parent_name_;
  SRigidBody* parent_addr_;
  std::vector<SRigidBody*> child_addrs_;

  //3. Graph structure information: (Enables manual graph parsing)
  std::vector<SRigidBody*> gr_parent_names_;
  std::vector<SRigidBody*> gr_parent_addrs_;
  std::vector<SRigidBody*> gr_child_addrs_;
  
  //****************************************************************************************
  // Static Link Properties (wrt. parent at the zero position)
  sBool is_root_;

  Eigen::Vector3d pos_in_parent_; 

  Eigen::Quaternion<sFloat> ori_parent_quat_;

  Eigen::Vector3d com_;     
  sFloat mass_;             
  Eigen::Matrix3d inertia_; 

  sInt link_is_fixed_;      

  //****************************************************************************************
  // Joint information
  std::string joint_name_;
  sFloat joint_limit_lower_, joint_limit_upper_;
  sFloat joint_default_pos_;
  EJointType joint_type_;
  
  //****************************************************************************************
  // Spatial joint information
  std::vector<std::string> sp_joint_name_;
  std::vector<sBool> sp_joint_broken_in_spanning_tree_;
  Eigen::VectorXd sp_joint_limit_lower_, sp_joint_limit_upper_, sp_joint_default_pos_;
  Eigen::MatrixXd sp_S_joint_;     
  Eigen::MatrixXd sp_Sorth_joint_; 

  //****************************************************************************************
  // Graphics data
  std::vector<SRigidBodyGraphics> graphics_obj_vec_;
  sInt collision_type_;
  ERenderType render_type_;

  //****************************************************************************************
  SRigidBody();

  void init();
};

}//end of namespace scl_parser

#endif /*SRIGIDBODY_HPP_*/