twist.hpp

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
 
#include "franky/types.hpp"
#include "franky/util.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 Twist fromVectorRepr(const Vector6d &vector_repr) {
    return Twist{vector_repr.head<3>(), vector_repr.tail<3>()};
  }
 
  [[nodiscard]] Vector6d vector_repr() const {
    Vector6d result;
    result << linear_velocity_, angular_velocity_;
    return result;
  }
 
  [[nodiscard]] Twist transformWith(const Affine &transformation) const {
    return transformWith(transformation.rotation());
  }
 
  template<typename RotationMatrixType>
  [[nodiscard]] Twist transformWith(const RotationMatrixType &rotation) const {
    return Twist{rotation * linear_velocity_, rotation * angular_velocity_};
  }
 
  [[nodiscard]] Twist propagateThroughLink(const Eigen::Vector3d &link_translation) const {
    return Twist{linear_velocity_ + angular_velocity_.cross(link_translation), angular_velocity_};
  }
 
  [[nodiscard]] Eigen::Vector3d linear_velocity() const {
    return linear_velocity_;
  }
 
  [[nodiscard]] Eigen::Vector3d angular_velocity() const {
    return angular_velocity_;
  }
 
  friend std::ostream& operator<<(std::ostream& os, const Twist& twist);
 
 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>
Twist operator*(const RotationMatrixType &rotation, const Twist &twist) {
  return twist.transformWith(rotation);
}
 
inline std::ostream& operator<<(std::ostream& os, const Twist& twist) {
  os << "Twist(lin=" <<twist.linear_velocity_ << ", ang=" << twist.angular_velocity_ << ")";
  return os;
}
 
}  // namespace franky