DualQuaternionPose.hpp

#pragma once
 
#include "state_representation/space/dual_quaternion/DualQuaternionState.hpp"
 
namespace state_representation {
class DualQuaternionPose : public DualQuaternionState {
private:
  Eigen::Vector3d position; 
 
public:
  explicit DualQuaternionPose(const std::string& name, const std::string& reference = "world");
 
  DualQuaternionPose(const DualQuaternionPose& state);
 
  DualQuaternionPose(const DualQuaternionState& state);
 
  explicit DualQuaternionPose(const std::string& name,
                              const Eigen::Vector3d& position,
                              const Eigen::Quaterniond& rotation,
                              const std::string& reference = "world");
 
  const Eigen::Vector3d get_position() const;
 
  const Eigen::Quaterniond& get_orientation() const;
 
  void set_orientation(const Eigen::Quaterniond& orientation);
 
  void set_position(const Eigen::Vector3d& position);
 
  void set_position(const Eigen::Quaterniond& dual);
 
  const DualQuaternionPose conjugate() const;
 
  const DualQuaternionPose inverse() const;
 
  DualQuaternionPose& operator*=(const DualQuaternionPose& q);
 
  const DualQuaternionPose operator*(const DualQuaternionPose& p) const;
 
  DualQuaternionPose& operator*=(const DualQuaternionState& s);
 
  const DualQuaternionPose operator*(const DualQuaternionState& s) const;
 
  void operator=(const DualQuaternionState& s);
 
  void initialize();
 
  const DualQuaternionPose copy() const;
 
  friend const DualQuaternionState log(const DualQuaternionPose& state);
 
  friend std::ostream& operator<<(std::ostream& os, const DualQuaternionPose& state);
};
 
inline const Eigen::Vector3d DualQuaternionPose::get_position() const {
  return 2 * (this->get_dual() * this->get_primary().conjugate()).vec();
}
 
inline const Eigen::Quaterniond& DualQuaternionPose::get_orientation() const {
  return this->get_primary();
}
 
inline void DualQuaternionPose::set_orientation(const Eigen::Quaterniond& orientation) {
  Eigen::Quaterniond temp = orientation;
  if (orientation.norm() - 1 < 1e-4) { temp.normalize(); }
  if (orientation.w() < 0) {
    temp = Eigen::Quaterniond(-orientation.w(),
                              -orientation.x(),
                              -orientation.y(),
                              -orientation.z());
  }
  this->set_primary(temp);
  this->set_dual(Eigen::Quaterniond(0.5 * (Eigen::Quaterniond(0,
                                                              this->position(0),
                                                              this->position(1),
                                                              this->position(2)) * this->get_primary()).coeffs()));
}
 
inline void DualQuaternionPose::set_position(const Eigen::Vector3d& position) {
  this->position = position;
  this->set_dual(Eigen::Quaterniond(0.5 * (Eigen::Quaterniond(0,
                                                              this->position(0),
                                                              this->position(1),
                                                              this->position(2)) * this->get_primary()).coeffs()));
}
 
inline void DualQuaternionPose::set_position(const Eigen::Quaterniond& dual) {
  // WARNING - position is updated, but not dual. This might lead to conficts / different result between position and get_position
  this->position = 2 * (dual * this->get_primary().conjugate()).vec();
  this->set_dual(dual);
}
}