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"
 
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<double> elbow_position = std::nullopt);
#pragma clang diagnostic pop
 
  explicit RobotPose(const Vector7d &vector_repr, bool ignore_elbow = false);
 
  explicit RobotPose(const Vector6d &vector_repr, std::optional<double> elbow_position = std::nullopt);
 
  explicit RobotPose(franka::CartesianPose franka_pose);
 
  [[nodiscard]] Vector7d vector_repr() const;
 
  [[nodiscard]] franka::CartesianPose as_franka_pose() const;
 
  [[nodiscard]] inline RobotPose leftTransform(const Affine &transform) const {
    return {transform * end_effector_pose_, elbow_position_};
  }
 
  [[nodiscard]] inline RobotPose rightTransform(const Affine &transform) const {
    return {end_effector_pose_ * transform, elbow_position_};
  }
 
  [[nodiscard]] inline RobotPose changeEndEffectorFrame(const Affine &transform) const {
    return rightTransform(transform);
  }
 
  [[nodiscard]] inline RobotPose withElbowPosition(const std::optional<double> elbow_position) const {
    return {end_effector_pose_, elbow_position};
  }
 
  [[nodiscard]] inline Affine end_effector_pose() const {
    return end_effector_pose_;
  }
 
  [[nodiscard]] inline std::optional<double> elbow_position() const {
    return elbow_position_;
  }
 
 private:
  Affine end_effector_pose_;
  std::optional<double> elbow_position_;
};
 
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