twist.hpp

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
 
#include "franky/types.hpp"
 
namespace franky {
 
class Twist {
 public:
  Twist(const Twist &twist) = default;
 
  explicit Twist(Eigen::Vector3d linear_velocity = Eigen::Vector3d::Zero(),
                 Eigen::Vector3d angular_velocity = Eigen::Vector3d::Zero())
      : linear_velocity_(std::move(linear_velocity)), angular_velocity_(std::move(angular_velocity)) {}
 
  [[nodiscard]] static inline Twist fromVectorRepr(const Vector6d &vector_repr) {
    return Twist{vector_repr.head<3>(), vector_repr.tail<3>()};
  }
 
  [[nodiscard]] inline Vector6d vector_repr() const {
    Vector6d result;
    result << linear_velocity_, angular_velocity_;
    return result;
  }
 
  [[nodiscard]] inline Twist transformWith(const Affine &transformation) const {
    return transformWith(transformation.rotation());
  }
 
  template<typename RotationMatrixType>
  [[nodiscard]] inline Twist transformWith(const RotationMatrixType &rotation) const {
    return Twist{rotation * linear_velocity_, rotation * angular_velocity_};
  }
 
  [[nodiscard]] inline Twist propagateThroughLink(const Eigen::Vector3d &link_translation) const {
    return Twist{linear_velocity_ + angular_velocity_.cross(link_translation), angular_velocity_};
  }
 
  [[nodiscard]] inline Eigen::Vector3d linear_velocity() const {
    return linear_velocity_;
  }
 
  [[nodiscard]] inline Eigen::Vector3d angular_velocity() const {
    return angular_velocity_;
  }
 
 private:
  Eigen::Vector3d linear_velocity_;
  Eigen::Vector3d angular_velocity_;
};
 
inline Twist operator*(const Affine &affine, const Twist &twist) {
  return twist.transformWith(affine);
}
 
template<typename RotationMatrixType>
inline Twist operator*(const RotationMatrixType &rotation, const Twist &twist) {
  return twist.transformWith(rotation);
}
 
}  // namespace franky