robot_pose.hpp

Go to the documentation of this file.

#pragma once
 
#include <optional>
#include <Eigen/Core>
#include <franka/control_types.h>
 
#include "franky/types.hpp"
#include "franky/elbow_state.hpp"
 
namespace franky {
 
class RobotPose {
 public:
  RobotPose();
 
  RobotPose(const RobotPose &robot_pose);
 
  // Suppress implicit conversion warning
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wimplicit-conversion"
  RobotPose(Affine end_effector_pose, std::optional<ElbowState> elbow_state = std::nullopt);
#pragma clang diagnostic pop
 
  explicit RobotPose(
      const Vector7d &vector_repr, bool ignore_elbow = false, FlipDirection flip_direction = FlipDirection::kNegative);
 
  explicit RobotPose(const Vector6d &vector_repr, std::optional<ElbowState> elbow_state = std::nullopt);
 
  explicit RobotPose(const franka::CartesianPose &franka_pose);
 
  [[nodiscard]] Vector7d vector_repr() const;
 
  [[nodiscard]] franka::CartesianPose as_franka_pose(
      FlipDirection default_elbow_flip_direction = FlipDirection::kNegative) const;
 
  [[nodiscard]] RobotPose leftTransform(const Affine &transform) const {
    return {transform * end_effector_pose_, elbow_state_};
  }
 
  [[nodiscard]] RobotPose rightTransform(const Affine &transform) const {
    return {end_effector_pose_ * transform, elbow_state_};
  }
 
  [[nodiscard]] RobotPose changeEndEffectorFrame(const Affine &transform) const {
    return rightTransform(transform);
  }
 
  [[nodiscard]] RobotPose withElbowState(const std::optional<ElbowState> elbow_state) const {
    return {end_effector_pose_, elbow_state};
  }
 
  [[nodiscard]] Affine end_effector_pose() const {
    return end_effector_pose_;
  }
 
  [[nodiscard]] std::optional<ElbowState> elbow_state() const {
    return elbow_state_;
  }
 
  friend std::ostream& operator<<(std::ostream& os, const RobotPose& robot_pose);
 
 private:
  Affine end_effector_pose_;
  std::optional<ElbowState> elbow_state_;
};
 
inline RobotPose operator*(const RobotPose &robot_pose, const Affine &right_transform) {
  return robot_pose.rightTransform(right_transform);
}
 
inline RobotPose operator*(const Affine &left_transform, const RobotPose &robot_pose) {
  return robot_pose.leftTransform(left_transform);
}
 
}  // namespace franky