| Directory: | ./ |
|---|---|
| File: | include/pinocchio/spatial/classic-acceleration.hpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 21 | 21 | 100.0% |
| Branches: | 26 | 52 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2019 INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_spatial_classic_acceleration_hpp__ | ||
| 6 | #define __pinocchio_spatial_classic_acceleration_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/spatial/se3.hpp" | ||
| 9 | #include "pinocchio/spatial/motion.hpp" | ||
| 10 | |||
| 11 | namespace pinocchio | ||
| 12 | { | ||
| 13 | /// | ||
| 14 | /// \brief Computes the classic acceleration from a given spatial velocity and spatial | ||
| 15 | /// acceleration. | ||
| 16 | /// | ||
| 17 | /// \tparam Motion1 type of the input spatial velocity. | ||
| 18 | /// \tparam Motion2 type of the input spatial acceleration. | ||
| 19 | /// \tparam Vector3Like type of the return type (a type similar to a 3D vector). | ||
| 20 | /// | ||
| 21 | /// \param[in] spatial_velocity input spatial velocity. | ||
| 22 | /// \param[in] spatial_acceleration input spatial acceleration. | ||
| 23 | /// \param[out] res computed classic acceleration. | ||
| 24 | /// | ||
| 25 | /// \remarks To be valid, the spatial velocity and the spatial acceleration have to be expressed | ||
| 26 | /// at the same Frame. | ||
| 27 | /// | ||
| 28 | template<typename Motion1, typename Motion2, typename Vector3Like> | ||
| 29 | 27 | inline void classicAcceleration( | |
| 30 | const MotionDense<Motion1> & spatial_velocity, | ||
| 31 | const MotionDense<Motion2> & spatial_acceleration, | ||
| 32 | const Eigen::MatrixBase<Vector3Like> & res) | ||
| 33 | { | ||
| 34 |
4/8✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
|
54 | PINOCCHIO_EIGEN_CONST_CAST(Vector3Like, res).noalias() = |
| 35 |
2/4✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
|
54 | spatial_acceleration.linear() + spatial_velocity.angular().cross(spatial_velocity.linear()); |
| 36 | 27 | } | |
| 37 | |||
| 38 | /// | ||
| 39 | /// \brief Computes the classic acceleration from a given spatial velocity and spatial | ||
| 40 | /// acceleration. | ||
| 41 | /// | ||
| 42 | /// \tparam Motion1 type of the input spatial velocity. | ||
| 43 | /// \tparam Motion2 type of the input spatial acceleration. | ||
| 44 | /// | ||
| 45 | /// \param[in] spatial_velocity input spatial velocity. | ||
| 46 | /// \param[in] spatial_acceleration input spatial acceleration. | ||
| 47 | /// | ||
| 48 | /// \remarks To be valid, the spatial velocity and the spatial acceleration have to be expressed | ||
| 49 | /// at the same Frame. | ||
| 50 | /// | ||
| 51 | template<typename Motion1, typename Motion2> | ||
| 52 | 27 | inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion2::Vector3) classicAcceleration( | |
| 53 | const MotionDense<Motion1> & spatial_velocity, | ||
| 54 | const MotionDense<Motion2> & spatial_acceleration) | ||
| 55 | { | ||
| 56 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion2::Vector3) ReturnType; | ||
| 57 | 27 | ReturnType res; | |
| 58 | 27 | classicAcceleration(spatial_velocity, spatial_acceleration, res); | |
| 59 | 27 | return res; | |
| 60 | } | ||
| 61 | |||
| 62 | /// | ||
| 63 | /// \brief Computes the classic acceleration of a given frame B | ||
| 64 | /// knowing the spatial velocity and spatial acceleration of a frame A | ||
| 65 | /// and the relative placement between these two frames. | ||
| 66 | /// | ||
| 67 | /// \tparam Motion1 type of the input spatial velocity. | ||
| 68 | /// \tparam Motion2 type of the input spatial acceleration. | ||
| 69 | /// \tparam SE3Scalar Scalar type of the SE3 object. | ||
| 70 | /// \tparam SE3Options Options of the SE3 object. | ||
| 71 | /// \tparam Vector3Like type of the return type (a type similar to a 3D vector). | ||
| 72 | /// | ||
| 73 | /// \param[in] spatial_velocity input spatial velocity. | ||
| 74 | /// \param[in] spatial_acceleration input spatial acceleration. | ||
| 75 | /// \param[in] placement relative placement betwen the frame A and the frame B. | ||
| 76 | /// \param[out] res computed classic acceleration. | ||
| 77 | /// | ||
| 78 | template< | ||
| 79 | typename Motion1, | ||
| 80 | typename Motion2, | ||
| 81 | typename SE3Scalar, | ||
| 82 | int SE3Options, | ||
| 83 | typename Vector3Like> | ||
| 84 | 272 | inline void classicAcceleration( | |
| 85 | const MotionDense<Motion1> & spatial_velocity, | ||
| 86 | const MotionDense<Motion2> & spatial_acceleration, | ||
| 87 | const SE3Tpl<SE3Scalar, SE3Options> & placement, | ||
| 88 | const Eigen::MatrixBase<Vector3Like> & res) | ||
| 89 | { | ||
| 90 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion1::LinearType) Vector3; | ||
| 91 | |||
| 92 |
2/4✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 136 times.
✗ Branch 5 not taken.
|
272 | const Vector3 linear_velocity_frame_B = |
| 93 |
4/8✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 136 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 136 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 136 times.
✗ Branch 11 not taken.
|
272 | spatial_velocity.linear() + spatial_velocity.angular().cross(placement.translation()); |
| 94 | |||
| 95 |
3/6✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 136 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 136 times.
✗ Branch 8 not taken.
|
544 | const Vector3 linear_acceleration_frame_B // expressed in the coordinate frame A |
| 96 |
2/4✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 136 times.
✗ Branch 5 not taken.
|
544 | = spatial_acceleration.linear() + spatial_velocity.angular().cross(linear_velocity_frame_B); |
| 97 | |||
| 98 |
5/10✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 136 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 136 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 136 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 136 times.
✗ Branch 15 not taken.
|
544 | PINOCCHIO_EIGEN_CONST_CAST(Vector3Like, res).noalias() = |
| 99 |
2/4✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 136 times.
✗ Branch 5 not taken.
|
272 | placement.rotation().transpose() |
| 100 |
2/4✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 136 times.
✗ Branch 5 not taken.
|
272 | * (linear_acceleration_frame_B - placement.translation().cross(spatial_acceleration.angular())); |
| 101 | 272 | } | |
| 102 | |||
| 103 | /// | ||
| 104 | /// \brief Computes the classic acceleration of a given frame B | ||
| 105 | /// knowing the spatial velocity and spatial acceleration of a frame A | ||
| 106 | /// and the relative placement between these two frames. | ||
| 107 | /// | ||
| 108 | /// \tparam Motion1 type of the input spatial velocity. | ||
| 109 | /// \tparam Motion2 type of the input spatial acceleration. | ||
| 110 | /// \tparam SE3Scalar Scalar type of the SE3 object. | ||
| 111 | /// \tparam SE3Options Options of the SE3 object. | ||
| 112 | /// | ||
| 113 | /// \param[in] spatial_velocity input spatial velocity. | ||
| 114 | /// \param[in] spatial_acceleration input spatial acceleration. | ||
| 115 | /// \param[in] placement relative placement betwen the frame A and the frame B. | ||
| 116 | /// | ||
| 117 | template<typename Motion1, typename Motion2, typename SE3Scalar, int SE3Options> | ||
| 118 | 67 | inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion2::Vector3) classicAcceleration( | |
| 119 | const MotionDense<Motion1> & spatial_velocity, | ||
| 120 | const MotionDense<Motion2> & spatial_acceleration, | ||
| 121 | const SE3Tpl<SE3Scalar, SE3Options> & placement) | ||
| 122 | { | ||
| 123 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion2::Vector3) ReturnType; | ||
| 124 | 67 | ReturnType res; | |
| 125 | 67 | classicAcceleration(spatial_velocity, spatial_acceleration, placement, res); | |
| 126 | 67 | return res; | |
| 127 | } | ||
| 128 | } // namespace pinocchio | ||
| 129 | |||
| 130 | #endif // ifndef __pinocchio_spatial_classic_acceleration_hpp__ | ||
| 131 |