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

#ifndef CDYNAMICSSCL_HPP_
#define CDYNAMICSSCL_HPP_

#include <scl/dynamics/CDynamicsBase.hpp>
#include <scl/data_structs/SRigidBodyDyn.hpp>

#include <sutil/CMappedTree.hpp>
#include <string>

namespace scl
{
  class CDynamicsScl : public scl::CDynamicsBase
  {
public:
  /* *******************************************************************
   *                      Overall Computational function.
   * ******************************************************************* */
  virtual sBool computeGCModel(
      const SRobotSensors * arg_sensor_data,
      SGcModel * arg_gc_model);

  /* *******************************************************************
   *                      Coordinate Transformations
   * ******************************************************************* */
  inline virtual sBool computeTransformsForAllLinks(
      sutil::CMappedTree<std::string, SRigidBodyDyn> &arg_tree,
      const Eigen::VectorXd& arg_q);

  virtual sBool computeTransform(
      SRigidBodyDyn& arg_link,
      const Eigen::VectorXd& arg_q);

  virtual sBool computeTransformToAncestor(
      Eigen::Affine3d& arg_T,
      SRigidBodyDyn& arg_link,
      const SRigidBodyDyn* arg_ancestor,
      const Eigen::VectorXd& arg_q);

  /* *******************************************************************
   *                      Compute Jacobians
   * ******************************************************************* */
  virtual sBool computeJacobianWithTransforms(
      Eigen::MatrixXd& arg_J,
      SRigidBodyDyn& arg_link,
      const Eigen::VectorXd& arg_q,
      const Eigen::Vector3d& arg_pos_local);

  virtual sBool computeJacobian(
      Eigen::MatrixXd& arg_J,
      const SRigidBodyDyn& arg_link,
      const Eigen::VectorXd& arg_q,
      const Eigen::Vector3d& arg_pos_local) const;

  virtual sBool computeJacobian(
      Eigen::MatrixXd& arg_J,
      SRigidBodyDyn& arg_link,
      const SRigidBodyDyn* arg_ancestor,
      const Eigen::VectorXd& arg_q,
      const Eigen::Vector3d& arg_pos_local,
      const bool arg_recompute_transforms=true)
  { return false; }

  inline sBool computeJacobianComForAllLinks(
      sutil::CMappedTree<std::string, SRigidBodyDyn> &arg_tree,
      const Eigen::VectorXd& arg_q) const
  {
    if(false == has_been_init_){  return false; }
    bool flag = true; sutil::CMappedTree<std::string, SRigidBodyDyn>::iterator it,ite;
    //Update the com Jacobians.
    for(it = arg_tree.begin(), ite = arg_tree.end(); it!=ite; ++it)
    { flag = flag && computeJacobian(it->J_com_,*it, arg_q, it->link_ds_->com_); }
    return flag;
  }

  /* *******************************************************************
   *                      Dynamics Helper functions.
   * ******************************************************************* */
  inline sBool computeInertiaGC(Eigen::MatrixXd &ret_Mgc,
      sutil::CMappedTree<std::string, SRigidBodyDyn> &arg_tree,
      const Eigen::VectorXd& arg_q)
  {
    if(false == has_been_init_){  return false; }
    bool flag = true; sutil::CMappedTree<std::string, SRigidBodyDyn>::iterator it,ite;
    int dof = robot_parsed_data_->dof_;
    ret_Mgc.setZero(dof, dof);//Set the generalized inertia to zero.
    for(it = arg_tree.begin(), ite = arg_tree.end(); it!=ite; ++it)
    {//Compute each link's contribution to the overall Mgc
      if(it->link_ds_->is_root_){ continue;  }//Root doesn't move
      ret_Mgc += it->link_ds_->mass_ * (it->J_com_.block(0,0,3,dof).transpose() * it->J_com_.block(0,0,3,dof));
      ret_Mgc += it->J_com_.block(3,0,3,dof).transpose() * it->link_ds_->inertia_ * it->J_com_.block(3,0,3,dof);
    }
    return flag;
  }

  inline sBool computeForceGravityGC(Eigen::VectorXd &ret_FgravGC,
      sutil::CMappedTree<std::string, SRigidBodyDyn> &arg_tree,
      const Eigen::VectorXd& arg_q)
  {
    if(false == has_been_init_){  return false; }
    bool flag = true; sutil::CMappedTree<std::string, SRigidBodyDyn>::iterator it,ite;
    //Update the generalized gravity force vector.
    int dof = robot_parsed_data_->dof_;
    ret_FgravGC.setZero(dof);
    for(it = arg_tree.begin(), ite = arg_tree.end(); it!=ite; ++it)
    {
      if(it->link_ds_->is_root_){ continue;  }//Root doesn't experience gravity
      ret_FgravGC += it->link_ds_->mass_ * (it->J_com_.block(0,0,3,dof).transpose() * robot_parsed_data_->gravity_);
    }
    return flag;
  }

  /* *******************************************************************
   *                      Dynamics State functions.
   * ******************************************************************* */
  virtual sFloat computeEnergyKinetic(
      sutil::CMappedTree<std::string, SRigidBodyDyn> &arg_tree,
      const Eigen::VectorXd& arg_q,
      const Eigen::VectorXd& arg_dq);

  virtual sFloat computeEnergyPotential(
      sutil::CMappedTree<std::string, SRigidBodyDyn> &arg_tree,
      const Eigen::VectorXd& arg_q);

  /* *******************************************************************
   *                      Contact Resolution functions.
   * ******************************************************************* */
  virtual sBool computeImpulseForce(
      const SRobotSensors &arg_sensor_data,
      const SGcModel &arg_gc_model,
      sFloat arg_coeff_rest,
      sFloat arg_collision_time,
      scl::SForceContact &ret_force_);

  /* *******************************************************************
   *                      Initialization functions.
   * ******************************************************************* */
  CDynamicsScl() : CDynamicsBase() { }

  virtual ~CDynamicsScl(){}

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

} /* namespace scl */
#endif /* CDYNAMICSSCL_HPP_ */