classic-acceleration.hpp

//
// Copyright (c) 2019 INRIA
//
 
#ifndef __pinocchio_spatial_classic_acceleration_hpp__
#define __pinocchio_spatial_classic_acceleration_hpp__
 
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/motion.hpp"
 
namespace pinocchio
{
  template<typename Motion1, typename Motion2, typename Vector3Like>
  inline void classicAcceleration(
    const MotionDense<Motion1> & spatial_velocity,
    const MotionDense<Motion2> & spatial_acceleration,
    const Eigen::MatrixBase<Vector3Like> & res)
  {
    PINOCCHIO_EIGEN_CONST_CAST(Vector3Like, res).noalias() =
      spatial_acceleration.linear() + spatial_velocity.angular().cross(spatial_velocity.linear());
  }
 
  template<typename Motion1, typename Motion2>
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion2::Vector3) classicAcceleration(
    const MotionDense<Motion1> & spatial_velocity,
    const MotionDense<Motion2> & spatial_acceleration)
  {
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion2::Vector3) ReturnType;
    ReturnType res;
    classicAcceleration(spatial_velocity, spatial_acceleration, res);
    return res;
  }
 
  template<
    typename Motion1,
    typename Motion2,
    typename SE3Scalar,
    int SE3Options,
    typename Vector3Like>
  inline void classicAcceleration(
    const MotionDense<Motion1> & spatial_velocity,
    const MotionDense<Motion2> & spatial_acceleration,
    const SE3Tpl<SE3Scalar, SE3Options> & placement,
    const Eigen::MatrixBase<Vector3Like> & res)
  {
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion1::LinearType) Vector3;
 
    const Vector3 linear_velocity_frame_B =
      spatial_velocity.linear() + spatial_velocity.angular().cross(placement.translation());
 
    const Vector3 linear_acceleration_frame_B // expressed in the coordinate frame A
      = spatial_acceleration.linear() + spatial_velocity.angular().cross(linear_velocity_frame_B);
 
    PINOCCHIO_EIGEN_CONST_CAST(Vector3Like, res).noalias() =
      placement.rotation().transpose()
      * (linear_acceleration_frame_B - placement.translation().cross(spatial_acceleration.angular()));
  }
 
  template<typename Motion1, typename Motion2, typename SE3Scalar, int SE3Options>
  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion2::Vector3) classicAcceleration(
    const MotionDense<Motion1> & spatial_velocity,
    const MotionDense<Motion2> & spatial_acceleration,
    const SE3Tpl<SE3Scalar, SE3Options> & placement)
  {
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion2::Vector3) ReturnType;
    ReturnType res;
    classicAcceleration(spatial_velocity, spatial_acceleration, placement, res);
    return res;
  }
} // namespace pinocchio
 
#endif // ifndef __pinocchio_spatial_classic_acceleration_hpp__