Program Listing for File classic-acceleration.hpp

Return to documentation for file (include/pinocchio/spatial/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__