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__