Implemented streaming operator to convert types to string

TimSchneider42 · TimSchneider42 · commit fb9703e8d32a · 2025-03-31T19:11:49.000+02:00
diff --git a/include/franky/cartesian_state.hpp b/include/franky/cartesian_state.hpp
@@ -68,6 +68,8 @@ class CartesianState {
     return velocity_;
   }
 
+  friend inline std::ostream &operator<<(std::ostream &os, const CartesianState &cartesian_state);
+
  private:
   RobotPose pose_;
   RobotVelocity velocity_;
@@ -77,4 +79,9 @@ inline CartesianState operator*(const Affine &transform, const CartesianState &c
   return cartesian_state.transformWith(transform);
 }
 
+inline std::ostream &operator<<(std::ostream &os, const CartesianState &cartesian_state) {
+  os << "CartesianState(pose=" << cartesian_state.pose_ << ", velocity=" << cartesian_state.velocity_ << ")";
+  return os;
+}
+
 }  // namespace franky
diff --git a/include/franky/dynamics_limit.hpp b/include/franky/dynamics_limit.hpp
@@ -94,6 +94,9 @@ class DynamicsLimit {
    */
   const std::string desc;
 
+  template <typename LimitTypeStream>
+  friend std::ostream& operator<<(std::ostream& os, const DynamicsLimit<LimitTypeStream>& dynamics_limit);
+
  private:
   /**
    * @brief Validate the new value before setting it.
@@ -110,4 +113,10 @@ class DynamicsLimit {
   std::function<bool()> can_write_condition_; /**< Function to check if writing is allowed. */
 };
 
+template <typename LimitTypeStream>
+std::ostream& operator<<(std::ostream& os, const DynamicsLimit<LimitTypeStream>& dynamics_limit) {
+  os << dynamics_limit.value_ << " (max: " << dynamics_limit.max << ")";
+  return os;
+}
+
 }  // namespace franky
diff --git a/include/franky/elbow_state.hpp b/include/franky/elbow_state.hpp
@@ -2,6 +2,7 @@
 
 #include <optional>
 #include <array>
+#include <iostream>
 
 namespace franky {
 
@@ -14,6 +15,8 @@ enum class FlipDirection {
   kPositive = 1
 };
 
+std::ostream& operator<<(std::ostream& os, const FlipDirection& flip_direction);
+
 /**
  * @brief Elbow state of the robot.
  *
@@ -73,6 +76,8 @@ class ElbowState {
    */
   [[nodiscard]] inline std::optional<FlipDirection> joint_4_flip() const { return joint_4_flip_; }
 
+  friend std::ostream& operator<<(std::ostream& os, const ElbowState& elbow_state);
+
  private:
   double joint_3_pos_{0.0};
   std::optional<FlipDirection> joint_4_flip_{std::nullopt};
diff --git a/include/franky/joint_state.hpp b/include/franky/joint_state.hpp
@@ -50,9 +50,16 @@ class JointState {
     return velocity_;
   }
 
+  friend std::ostream& operator<<(std::ostream& os, const JointState& joint_state);
+
  private:
   Vector7d position_;
   Vector7d velocity_;
 };
 
+inline std::ostream& operator<<(std::ostream& os, const JointState& joint_state) {
+  os << "JointState(position=" << joint_state.position_ << ", velocity=" << joint_state.velocity_ << ")";
+  return os;
+}
+
 }  // namespace franky
diff --git a/include/franky/robot_pose.hpp b/include/franky/robot_pose.hpp
@@ -125,6 +125,8 @@ class RobotPose {
     return elbow_state_;
   }
 
+  friend std::ostream& operator<<(std::ostream& os, const RobotPose& robot_pose);
+
  private:
   Affine end_effector_pose_;
   std::optional<ElbowState> elbow_state_;
diff --git a/include/franky/robot_velocity.hpp b/include/franky/robot_velocity.hpp
@@ -129,6 +129,8 @@ class RobotVelocity {
     return elbow_velocity_;
   }
 
+  friend std::ostream& operator<<(std::ostream& os, const RobotVelocity& robot_velocity);
+
  private:
   Twist end_effector_twist_;
   std::optional<double> elbow_velocity_ = std::nullopt;
diff --git a/include/franky/twist.hpp b/include/franky/twist.hpp
@@ -3,6 +3,7 @@
 #include <Eigen/Core>
 
 #include "franky/types.hpp"
+#include "franky/util.hpp"
 
 namespace franky {
 
@@ -90,6 +91,8 @@ class Twist {
     return angular_velocity_;
   }
 
+  friend std::ostream& operator<<(std::ostream& os, const Twist& twist);
+
  private:
   Eigen::Vector3d linear_velocity_;
   Eigen::Vector3d angular_velocity_;
@@ -104,4 +107,9 @@ inline 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
diff --git a/include/franky/util.hpp b/include/franky/util.hpp
@@ -2,6 +2,7 @@
 
 #include <array>
 #include <Eigen/Core>
+#include <franka/duration.h>
 
 namespace franky {
 
@@ -46,4 +47,27 @@ std::array<double, dims> expand(const ScalarOrArray<dims> &input) {
   return output;
 }
 
+template<int dims>
+inline std::ostream &operator<<(std::ostream &os, const Eigen::Vector<double, dims> &vec) {
+  os << "[";
+  for (size_t i = 0; i < dims; i++) {
+    os << vec[i];
+    if (i != dims - 1)
+      os << " ";
+  }
+  os << "]";
+  return os;
+}
+
+inline std::ostream &operator<<(std::ostream &os, const Affine &affine) {
+  os << "Affine(t=" << affine.translation().eval() << ", q=" << Eigen::Quaterniond(affine.rotation()).coeffs()
+     << ")";
+  return os;
+}
+
+inline std::ostream &operator<<(std::ostream &os, const franka::Duration &duration) {
+  os << "Duration(" << duration.toMSec() << ")";
+  return os;
+}
+
 }  // namespace franky
diff --git a/python/bind_misc.cpp b/python/bind_misc.cpp
@@ -5,6 +5,8 @@
 
 #include "franky.hpp"
 
+#include "util.hpp"
+
 namespace py = pybind11;
 using namespace pybind11::literals; // to bring in the '_a' literal
 using namespace franky;
@@ -23,13 +25,7 @@ void bind_misc(py::module &m) {
       .def(py::self *= uint64_t())
       .def(py::self / uint64_t())
       .def(py::self /= uint64_t())
-      .def("__repr__",
-           [](const franka::Duration &duration) {
-             std::stringstream ss;
-             ss << "Duration(" << duration.toMSec() << ")";
-             return ss.str();
-           }
-      )
+      .def("__repr__", strFromStream<franka::Duration>)
       .def(py::pickle(
           [](const franka::Duration &duration) {  // __getstate__
             return py::make_tuple(duration.toMSec());
diff --git a/python/bind_robot.cpp b/python/bind_robot.cpp
@@ -6,6 +6,8 @@
 
 #include "franky.hpp"
 
+#include "util.hpp"
+
 namespace py = pybind11;
 using namespace pybind11::literals;  // to bring in the '_a' literal
 using namespace franky;
@@ -37,6 +39,7 @@ void bind_robot(py::module &m) {
   py::class_<DynamicsLimit<Vector7d>>(m, "VectorDynamicsLimit")
       .def("set", &DynamicsLimit<Vector7d>::setFrom<Array<7>>, "value"_a)
       .def("get", &DynamicsLimit<Vector7d>::get)
+      .def("__repr__", strFromStream<DynamicsLimit<Vector7d>>)
       .def_property_readonly(
           "max",
           [](const DynamicsLimit<Vector7d> &dynamics_limit) {
@@ -47,6 +50,7 @@ void bind_robot(py::module &m) {
   py::class_<DynamicsLimit<double>>(m, "DoubleDynamicsLimit")
       .def("set", &DynamicsLimit<double>::set, "value"_a)
       .def("get", &DynamicsLimit<double>::get)
+      .def("__repr__", strFromStream<DynamicsLimit<double>>)
       .def_readonly("max", &DynamicsLimit<double>::max)
       .def_readonly("desc", &DynamicsLimit<double>::desc);
 
diff --git a/python/bind_state_repr.cpp b/python/bind_state_repr.cpp
@@ -5,75 +5,12 @@
 
 #include "franky.hpp"
 
+#include "util.hpp"
+
 namespace py = pybind11;
 using namespace pybind11::literals; // to bring in the '_a' literal
 using namespace franky;
 
-template<int dims>
-std::string vecToStr(const Eigen::Vector<double, dims> &vec) {
-  std::stringstream ss;
-  ss << "[";
-  for (size_t i = 0; i < dims; i++) {
-    ss << vec[i];
-    if (i != dims - 1)
-      ss << " ";
-  }
-  ss << "]";
-  return ss.str();
-}
-
-std::string affineToStr(const Affine &affine) {
-  std::stringstream ss;
-  ss << "Affine(t=" << vecToStr(affine.translation().eval())
-     << ", q=" << vecToStr(Eigen::Quaterniond(affine.rotation()).coeffs()) << ")";
-  return ss.str();
-}
-
-std::string twistToStr(const Twist &twist) {
-  std::stringstream ss;
-  ss << "Twist(lin=" << vecToStr(twist.linear_velocity())
-     << ", ang=" << vecToStr(twist.angular_velocity()) << ")";
-  return ss.str();
-}
-
-std::string flipDirectionToStr(const FlipDirection &flip_direction) {
-  switch (flip_direction) {
-    case FlipDirection::kNegative:
-      return "FlipDirection.Negative";
-    case FlipDirection::kNeutral:
-      return "FlipDirection.Neutral";
-    default:
-      return "FlipDirection.Positive";
-  }
-}
-
-std::string elbowStateToStr(const ElbowState &elbow_state) {
-  std::stringstream ss;
-  ss << "ElbowState(joint_3_pos=" << elbow_state.joint_3_pos();
-  if (elbow_state.joint_4_flip().has_value())
-    ss << ", joint_4_flip=" << flipDirectionToStr(elbow_state.joint_4_flip().value());
-  ss << ")";
-  return ss.str();
-}
-
-std::string robotPoseToStr(const RobotPose &robot_pose) {
-  std::stringstream ss;
-  ss << "RobotPose(ee_pose=" << affineToStr(robot_pose.end_effector_pose());
-  if (robot_pose.elbow_state().has_value())
-    ss << ", elbow=" << elbowStateToStr(robot_pose.elbow_state().value());
-  ss << ")";
-  return ss.str();
-}
-
-std::string robotVelocityToStr(const RobotVelocity &robot_velocity) {
-  std::stringstream ss;
-  ss << "RobotVelocity(ee_twist=" << twistToStr(robot_velocity.end_effector_twist());
-  if (robot_velocity.elbow_velocity().has_value())
-    ss << ", elbow_vel=" << robot_velocity.elbow_velocity().value();
-  ss << ")";
-  return ss.str();
-}
-
 void bind_state_repr(py::module &m) {
   py::class_<Affine>(m, "Affine")
       .def(py::init<const Eigen::Matrix<double, 4, 4> &>(),
@@ -94,7 +31,7 @@ void bind_state_repr(py::module &m) {
       .def_property_readonly("matrix", [](const Affine &affine) {
         return affine.matrix();
       })
-      .def("__repr__", &affineToStr)
+      .def("__repr__", strFromStream<Affine>)
       .def(py::pickle(
           [](const Affine &affine) {  // __getstate__
             return py::make_tuple(affine.translation(), Eigen::Quaterniond(affine.rotation()).coeffs());
@@ -113,7 +50,7 @@ void bind_state_repr(py::module &m) {
       .def(py::init<const ElbowState &>()) // Copy constructor
       .def_property_readonly("joint_3_pos", &ElbowState::joint_3_pos)
       .def_property_readonly("joint_4_flip", &ElbowState::joint_4_flip)
-      .def("__repr__", &elbowStateToStr)
+      .def("__repr__", &strFromStream<ElbowState>)
       .def(py::pickle(
           [](const ElbowState &elbow_state) {  // __getstate__
             return py::make_tuple(elbow_state.joint_3_pos(), elbow_state.joint_4_flip());
@@ -151,7 +88,7 @@ void bind_state_repr(py::module &m) {
       .def("__rmul__",
            [](const Twist &twist, const Eigen::Vector4d &quaternion) { return Eigen::Quaterniond(quaternion) * twist; },
            py::is_operator())
-      .def("__repr__", &twistToStr)
+      .def("__repr__", &strFromStream<Twist>)
       .def(py::pickle(
           [](const Twist &twist) {  // __getstate__
             return py::make_tuple(twist.linear_velocity(), twist.angular_velocity());
@@ -178,7 +115,7 @@ void bind_state_repr(py::module &m) {
       .def("__rmul__",
            [](const RobotPose &robot_pose, const Affine &affine) { return affine * robot_pose; },
            py::is_operator())
-      .def("__repr__", robotPoseToStr)
+      .def("__repr__", strFromStream<RobotPose>)
       .def(py::pickle(
           [](const RobotPose &robot_pose) {  // __getstate__
             return py::make_tuple(robot_pose.end_effector_pose(), robot_pose.elbow_state());
@@ -206,7 +143,7 @@ void bind_state_repr(py::module &m) {
              return Eigen::Quaterniond(quaternion) * robot_velocity;
            },
            py::is_operator())
-      .def("__repr__", robotVelocityToStr)
+      .def("__repr__", strFromStream<RobotVelocity>)
       .def(py::pickle(
           [](const RobotVelocity &robot_velocity) {  // __getstate__
             return py::make_tuple(robot_velocity.end_effector_twist(), robot_velocity.elbow_velocity());
@@ -230,12 +167,7 @@ void bind_state_repr(py::module &m) {
       .def("__rmul__",
            [](const CartesianState &cartesian_state, const Affine &affine) { return affine * cartesian_state; },
            py::is_operator())
-      .def("__repr__", [](const CartesianState &cartesian_state) {
-        std::stringstream ss;
-        ss << "CartesianState(pose=" << robotPoseToStr(cartesian_state.pose())
-           << ", velocity=" << robotVelocityToStr(cartesian_state.velocity()) << ")";
-        return ss.str();
-      })
+      .def("__repr__", strFromStream<CartesianState>)
       .def(py::pickle(
           [](const CartesianState &cartesian_state) {  // __getstate__
             return py::make_tuple(cartesian_state.pose(), cartesian_state.velocity());
@@ -255,12 +187,7 @@ void bind_state_repr(py::module &m) {
       .def(py::init<const JointState &>()) // Copy constructor
       .def_property_readonly("position", &JointState::position)
       .def_property_readonly("velocity", &JointState::velocity)
-      .def("__repr__", [](const JointState &joint_state) {
-        std::stringstream ss;
-        ss << "JointState(position=" << vecToStr(joint_state.position())
-           << ", velocity=" << vecToStr(joint_state.velocity()) << ")";
-        return ss.str();
-      })
+      .def("__repr__", strFromStream<JointState>)
       .def(py::pickle(
           [](const JointState &joint_state) {  // __getstate__
             return py::make_tuple(joint_state.position(), joint_state.velocity());
diff --git a/python/util.hpp b/python/util.hpp
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <iostream>
+
+using namespace franky;
+
+template<typename T>
+std::string strFromStream(const T &obj) {
+  std::stringstream ss;
+  ss << obj;
+  return ss.str();
+}
diff --git a/src/dynamics_limit.cpp b/src/dynamics_limit.cpp
@@ -44,4 +44,9 @@ void DynamicsLimit<Vector7d>::check(const Vector7d &value) const {
   }
 }
 
+std::ostream &operator<<(std::ostream &os, const DynamicsLimit<Vector7d> &limit) {
+  os << limit.get() << " (max: " << Vector7d::Map(limit.max.data()).eval() << ")";
+  return os;
+}
+
 }  // namespace franky
diff --git a/src/elbow_state.cpp b/src/elbow_state.cpp
diff --git a/src/robot_pose.cpp b/src/robot_pose.cpp
diff --git a/src/robot_velocity.cpp b/src/robot_velocity.cpp

Original file line number	Diff line number	Diff line change
`@@ -125,6 +125,8 @@ class RobotPose {`
`125`	`125`	`return elbow_state_;`
`126`	`126`	`}`
`127`	`127`
	`128`	`+ friend std::ostream& operator<<(std::ostream& os, const RobotPose& robot_pose);`
	`129`	`+`
`128`	`130`	`private:`
`129`	`131`	`Affine end_effector_pose_;`
`130`	`132`	`std::optional<ElbowState> elbow_state_;`
Original file line number	Diff line number	Diff line change
`@@ -129,6 +129,8 @@ class RobotVelocity {`
`129`	`129`	`return elbow_velocity_;`
`130`	`130`	`}`
`131`	`131`
	`132`	`+ friend std::ostream& operator<<(std::ostream& os, const RobotVelocity& robot_velocity);`
	`133`	`+`
`132`	`134`	`private:`
`133`	`135`	`Twist end_effector_twist_;`
`134`	`136`	`std::optional<double> elbow_velocity_ = std::nullopt;`