CDynamicsSclSpatialMath.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/. */
/*
 * CDynamicsSclSpatialMath.hpp
 *
 *  Created on: Jun 9, 2014
 *      Author: Nayan Singhal <singhalnayan91@gmail.com>
 *              Brains in Silicon Lab,
 *              Stanford University.
 */

#ifndef CDYNAMICSSCLSPATIALMATH_HPP_
#define CDYNAMICSSCLSPATIALMATH_HPP_


#include <scl/DataTypes.hpp>
#include <scl/data_structs/SGcModel.hpp>
#include <scl/data_structs/SRigidBodyDyn.hpp>
#include <scl/robot/data_structs/SRobot.hpp>

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

#include <stack>
#include <iostream>


namespace scl_ext
{
  // ****************************************************
  //           Compute subspace motion and force constraint
  // ****************************************************

  bool calculateForceSubspace(
      Eigen::MatrixXd &arg_subspace,
      Eigen::MatrixXd &ret_force_subspace);

  bool calculateTransformationAndSubspace( 
      Eigen::MatrixXd & ret_Xlink,
      Eigen::MatrixXd & ret_subspace,
      scl::sInt arg_joint_type,
      scl::sFloat arg_q);

  bool calculateTransformationAndInertia( 
      scl::SGcModel *arg_gc_model);

  bool calculateOrderOfProcessing ( 
      scl::SGcModel *arg_gc_model,
      std::vector <std::string> &ret_processing_order );

  // ****************************************************
  //           Compute coordinate Transformation
  // ****************************************************

  bool computeRotXAxis(
      Eigen::MatrixXd & ret_transform,
      scl::sFloat arg_q);

  bool computeRotYAxis(
      Eigen::MatrixXd & ret_transform,
      scl::sFloat arg_q);

  bool computeRotZAxis(
      Eigen::MatrixXd & ret_transform,
      scl::sFloat arg_q);

  bool computeTranslation(
      Eigen::MatrixXd & ret_transform,
      Eigen::Vector3d r);

  bool computeRotFromQuaternion(
      Eigen::MatrixXd &ret_transform,
      Eigen::Quaternion<scl::sFloat> arg_ori);

  bool computeCrossForVelocity(
      Eigen::MatrixXd &ret_vcross,
      Eigen::MatrixXd arg_spatial_velocity);

  bool calculateSpatialInertia(
      scl::sSpatialXForm &spatial_inertia ,
      Eigen::Matrix3d arg_inertia,
      Eigen::Vector3d arg_com,
      scl::sFloat arg_mass);
}

#endif /* CDYNAMICSSCLSPATIALMATH_HPP_ */