DualQuaternionTwist.hpp

#pragma once
 
#include "state_representation/space/dual_quaternion/DualQuaternionState.hpp"
 
namespace state_representation {
class DualQuaternionTwist : public DualQuaternionState {
private:
  Eigen::Vector3d position; 
  Eigen::Vector3d linear_velocity; 
 
public:
  explicit DualQuaternionTwist(const std::string& name, const std::string& reference = "world");
 
  DualQuaternionTwist(const DualQuaternionTwist& state);
 
  explicit DualQuaternionTwist(const std::string& name,
                               const Eigen::Vector3d& linear_velocity,
                               const Eigen::Vector3d& angular_velocity,
                               const Eigen::Vector3d& position,
                               const std::string& reference);
 
  const Eigen::Vector3d& get_position() const;
 
  const Eigen::Vector3d& get_linear_velocity() const;
 
  const Eigen::Vector3d get_angular_velocity() const;
 
  void set_linear_velocity(const Eigen::Vector3d& linear_velocity);
 
  void set_angular_velocity(const Eigen::Vector3d& angular_velocity);
 
  void set_position(const Eigen::Vector3d& position);
 
  void operator=(const DualQuaternionState& q);
 
  void initialize();
 
  const DualQuaternionTwist copy() const;
 
  friend std::ostream& operator<<(std::ostream& os, const DualQuaternionTwist& state);
};
 
inline const Eigen::Vector3d& DualQuaternionTwist::get_position() const {
  return this->position;
}
 
inline const Eigen::Vector3d& DualQuaternionTwist::get_linear_velocity() const {
  return this->linear_velocity;
}
 
inline const Eigen::Vector3d DualQuaternionTwist::get_angular_velocity() const {
  return this->get_primary().vec();
}
 
inline void DualQuaternionTwist::set_linear_velocity(const Eigen::Vector3d& linear_velocity) {
  this->linear_velocity = linear_velocity;
  Eigen::Vector3d temp = this->get_linear_velocity() + this->get_position().cross(this->get_angular_velocity());
  this->set_dual(Eigen::Quaterniond(0, temp(0), temp(1), temp(2)));
}
 
inline void DualQuaternionTwist::set_angular_velocity(const Eigen::Vector3d& angular_velocity) {
  this->set_primary(Eigen::Quaterniond(0, angular_velocity(0), angular_velocity(1), angular_velocity(2)));
  Eigen::Vector3d temp = this->get_linear_velocity() + this->get_position().cross(this->get_angular_velocity());
  this->set_dual(Eigen::Quaterniond(0, temp(0), temp(1), temp(2)));
}
 
inline void DualQuaternionTwist::set_position(const Eigen::Vector3d& position) {
  this->position = position;
  Eigen::Vector3d temp = this->get_linear_velocity() + this->get_position().cross(this->get_angular_velocity());
  this->set_dual(Eigen::Quaterniond(0, temp(0), temp(1), temp(2)));
}
}