twist_acceleration.hpp

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
 
#include "franky/types.hpp"
#include "franky/util.hpp"
#include "franky/twist.hpp"
 
namespace franky {
 
class TwistAcceleration {
 public:
  TwistAcceleration(const TwistAcceleration &twist_acceleration_acceleration) = default;
 
  explicit TwistAcceleration(Eigen::Vector3d linear_acceleration = Eigen::Vector3d::Zero(),
                 Eigen::Vector3d angular_acceleration = Eigen::Vector3d::Zero())
      : linear_acceleration_(std::move(linear_acceleration)), angular_acceleration_(std::move(angular_acceleration)) {}
 
  [[nodiscard]] static TwistAcceleration fromVectorRepr(const Vector6d &vector_repr) {
    return TwistAcceleration{vector_repr.head<3>(), vector_repr.tail<3>()};
  }
 
  [[nodiscard]] Vector6d vector_repr() const {
    Vector6d result;
    result << linear_acceleration_, angular_acceleration_;
    return result;
  }
 
  [[nodiscard]] TwistAcceleration transformWith(const Affine &transformation) const {
    return transformWith(transformation.rotation());
  }
 
  template<typename RotationMatrixType>
  [[nodiscard]] TwistAcceleration transformWith(const RotationMatrixType &rotation) const {
    return TwistAcceleration{rotation * linear_acceleration_, rotation * angular_acceleration_};
  }
 
  [[nodiscard]] TwistAcceleration propagateThroughLink(
      const Eigen::Vector3d &link_translation,
      const Eigen::Vector3d &base_angular_velocity) const
  {
    Eigen::Vector3d propagated_linear_acceleration =
        linear_acceleration_ +
        angular_acceleration_.cross(link_translation) +
        base_angular_velocity.cross( base_angular_velocity.cross(link_translation));
 
    return TwistAcceleration{propagated_linear_acceleration, angular_acceleration_};
  }
 
  [[nodiscard]] Eigen::Vector3d linear_acceleration() const {
    return linear_acceleration_;
  }
 
  [[nodiscard]] Eigen::Vector3d angular_acceleration() const {
    return angular_acceleration_;
  }
 
  friend std::ostream& operator<<(std::ostream& os, const TwistAcceleration& twist_acceleration);
 
 private:
  Eigen::Vector3d linear_acceleration_;
  Eigen::Vector3d angular_acceleration_;
};
 
inline TwistAcceleration operator*(const Affine &affine, const TwistAcceleration &twist_acceleration) {
  return twist_acceleration.transformWith(affine);
}
 
template<typename RotationMatrixType>
TwistAcceleration operator*(const RotationMatrixType &rotation, const TwistAcceleration &twist_acceleration) {
  return twist_acceleration.transformWith(rotation);
}
 
inline std::ostream& operator<<(std::ostream& os, const TwistAcceleration& twist_acceleration) {
  os << "TwistAcceleration(lin=" <<twist_acceleration.linear_acceleration_ << ", ang=" << twist_acceleration.angular_acceleration_ << ")";
  return os;
}
 
}  // namespace franky