energy.hpp

//
// Copyright (c) 2016-2020 CNRS INRIA
//

#ifndef __pinocchio_algorithm_energy_hpp__
#define __pinocchio_algorithm_energy_hpp__

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/check.hpp"

namespace pinocchio {
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline Scalar
  computeKineticEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                       DataTpl<Scalar,Options,JointCollectionTpl> & data);

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  inline Scalar
  computeKineticEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                       DataTpl<Scalar,Options,JointCollectionTpl> & data,
                       const Eigen::MatrixBase<ConfigVectorType> & q,
                       const Eigen::MatrixBase<TangentVectorType> & v)
  {
    forwardKinematics(model,data,q.derived(),v.derived());
    return computeKineticEnergy(model,data);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  PINOCCHIO_DEPRECATED
  inline Scalar
  kineticEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                DataTpl<Scalar,Options,JointCollectionTpl> & data,
                const Eigen::MatrixBase<ConfigVectorType> & q,
                const Eigen::MatrixBase<TangentVectorType> & v,
                const bool update_kinematics)
  {
    if(update_kinematics)
      return computeKineticEnergy(model,data,q.derived(),v.derived());
    else
      return computeKineticEnergy(model,data);
  }
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline Scalar
  computePotentialEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                         DataTpl<Scalar,Options,JointCollectionTpl> & data);
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline Scalar
  computePotentialEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                         DataTpl<Scalar,Options,JointCollectionTpl> & data,
                         const Eigen::MatrixBase<ConfigVectorType> & q)
  {
    forwardKinematics(model,data,q);
    return computePotentialEnergy(model,data);
  }
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  PINOCCHIO_DEPRECATED
  inline Scalar
  potentialEnergy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  DataTpl<Scalar,Options,JointCollectionTpl> & data,
                  const Eigen::MatrixBase<ConfigVectorType> & q,
                  const bool update_kinematics)
  {
    if(update_kinematics)
      return computePotentialEnergy(model,data,q);
    else
      return computePotentialEnergy(model,data);
  }
}

#include "pinocchio/algorithm/energy.hxx"

#endif // __pinocchio_algorithm_energy_hpp__