Made RobotVelocity.elbow_velocity non-optional

TimSchneider42 · May 27, 2024 · a9b5924 · a9b5924
1 parent 887dfd8
commit a9b5924
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 27 deletions.
diff --git a/include/franky/robot.hpp b/include/franky/robot.hpp
@@ -133,7 +133,7 @@ class Robot : public franka::Robot {
   [[nodiscard]] inline CartesianState currentCartesianState() {
     auto s = state();
     return {{Affine(Eigen::Matrix4d::Map(s.O_T_EE.data())), s.elbow[0]},
-            RobotVelocity(Vector6d::Map(s.O_dP_EE_c.data()), std::optional<double>(s.delbow_c[0]))};
+            RobotVelocity(franka::CartesianVelocities(s.O_dP_EE_c, s.delbow_c))};
   }
 
   [[nodiscard]] Vector7d currentJointPositions();

diff --git a/include/franky/robot_velocity.hpp b/include/franky/robot_velocity.hpp
@@ -11,13 +11,19 @@ namespace franky {
 
 class RobotVelocity {
  public:
+  RobotVelocity();
+
   RobotVelocity(const RobotVelocity &robot_velocity);
 
-  RobotVelocity(Twist end_effector_twist, std::optional<double> elbow_velocity = std::nullopt);
+  RobotVelocity(RobotVelocity &&robot_velocity) noexcept;
 
-  explicit RobotVelocity(const Vector7d &vector_repr, bool ignore_elbow = false);
+  // Suppress implicit conversion warning
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wimplicit-conversion"
+  RobotVelocity(Twist end_effector_twist, double elbow_velocity = 0.0);
+#pragma clang diagnostic pop
 
-  explicit RobotVelocity(const Vector6d &vector_repr, std::optional<double> elbow_velocity = std::nullopt);
+  explicit RobotVelocity(const Vector7d &vector_repr);
 
   explicit RobotVelocity(franka::CartesianVelocities franka_velocity);
 
@@ -42,10 +48,6 @@ class RobotVelocity {
     return {end_effector_twist_.propagateThroughLink(offset_world_frame), elbow_velocity_};
   }
 
-  [[nodiscard]] inline RobotVelocity with_elbow_velocity(const std::optional<double> elbow_velocity) const {
-    return {end_effector_twist_, elbow_velocity};
-  }
-
   [[nodiscard]] inline Twist end_effector_twist() const {
     return end_effector_twist_;
   }
@@ -56,7 +58,7 @@ class RobotVelocity {
 
  private:
   Twist end_effector_twist_;
-  std::optional<double> elbow_velocity_;
+  double elbow_velocity_ = 0.0;
 };
 
 inline RobotVelocity operator*(const Affine &affine, const RobotVelocity &robot_velocity) {

diff --git a/python/python.cpp b/python/python.cpp
@@ -532,10 +532,9 @@ PYBIND11_MODULE(_franky, m) {
   py::implicitly_convertible<Affine, RobotPose>();
 
   py::class_<RobotVelocity>(m, "RobotVelocity")
-      .def(py::init<Twist, std::optional<double>>(), "end_effector_twist"_a, "elbow_velocity"_a = std::nullopt)
+      .def(py::init<Twist, double>(), "end_effector_twist"_a, "elbow_velocity"_a = 0.0)
       .def(py::init<const RobotVelocity &>()) // Copy constructor
       .def("change_end_effector_frame", &RobotVelocity::changeEndEffectorFrame, "offset_world_frame"_a)
-      .def("with_elbow_velocity", &RobotVelocity::with_elbow_velocity, "elbow_velocity"_a)
       .def_property_readonly("end_effector_twist", &RobotVelocity::end_effector_twist)
       .def_property_readonly("elbow_velocity", &RobotVelocity::elbow_velocity)
       .def("__rmul__",
@@ -550,9 +549,9 @@ PYBIND11_MODULE(_franky, m) {
             return py::make_tuple(robot_velocity.end_effector_twist(), robot_velocity.elbow_velocity());
           },
           [](const py::tuple &t) {  // __setstate__
-            if (t.size() != 3)
+            if (t.size() != 2)
               throw std::runtime_error("Invalid state!");
-            return RobotVelocity(t[0].cast<Twist>(), t[1].cast<std::optional<double>>());
+            return RobotVelocity(t[0].cast<Twist>(), t[1].cast<double>());
           }
       ));
 

diff --git a/src/robot_velocity.cpp b/src/robot_velocity.cpp
@@ -9,37 +9,37 @@
 
 namespace franky {
 
+RobotVelocity::RobotVelocity() = default;
+
 RobotVelocity::RobotVelocity(const RobotVelocity &) = default;
 
-RobotVelocity::RobotVelocity(Twist end_effector_twist, std::optional<double> elbow_velocity)
+RobotVelocity::RobotVelocity(RobotVelocity &&) noexcept = default;
+
+RobotVelocity::RobotVelocity(Twist end_effector_twist, double elbow_velocity)
     : end_effector_twist_(std::move(end_effector_twist)),
       elbow_velocity_(elbow_velocity) {}
 
-RobotVelocity::RobotVelocity(const Vector7d &vector_repr, bool ignore_elbow)
-    : RobotVelocity(
-    vector_repr.head<6>(),
-    ignore_elbow ? std::optional<double>(std::nullopt) : vector_repr[6]) {}
-
-RobotVelocity::RobotVelocity(const Vector6d &vector_repr, std::optional<double> elbow_velocity)
-    : end_effector_twist_(vector_repr), elbow_velocity_(elbow_velocity) {}
+RobotVelocity::RobotVelocity(const Vector7d &vector_repr)
+    : RobotVelocity(Twist{vector_repr.head<6>()}, vector_repr[6]) {}
 
 RobotVelocity::RobotVelocity(const franka::CartesianVelocities franka_velocity)
-    : RobotVelocity(Vector6d::Map(franka_velocity.O_dP_EE.data()), std::optional<double>(franka_velocity.elbow[0])) {}
+    : RobotVelocity(
+    {
+        Vector6d::Map(franka_velocity.O_dP_EE.data()).tail<3>(),
+        Vector6d::Map(franka_velocity.O_dP_EE.data()).head<3>()
+    }, franka_velocity.elbow[0]) {}
 
 Vector7d RobotVelocity::vector_repr() const {
   Vector7d result;
-  result << end_effector_twist_.vector_repr(), elbow_velocity_.value_or(0.0);
+  result << end_effector_twist_.vector_repr(), elbow_velocity_;
   return result;
 }
 
 franka::CartesianVelocities RobotVelocity::as_franka_velocity() const {
   std::array<double, 6> array;
   auto vec = vector_repr().head<6>();
   std::copy(vec.data(), vec.data() + array.size(), array.begin());
-  if (elbow_velocity_.has_value()) {
-    return franka::CartesianVelocities(array, {elbow_velocity_.value(), -1});
-  }
-  return {array};
+  return franka::CartesianVelocities(array, {elbow_velocity_, -1});
 }
 
 }  // namespace franky