Made Twist constructor arguments optional

TimSchneider42 · May 27, 2024 · 8872b84 · 8872b84
1 parent dba60a1
commit 8872b84
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 16 deletions.
diff --git a/include/franky/robot_velocity.hpp b/include/franky/robot_velocity.hpp
@@ -28,7 +28,7 @@ class RobotVelocity {
    * @param end_effector_twist The twist of the end effector.
    * @param elbow_velocity The velocity of the elbow. Default is 0.0.
    */
-  RobotVelocity(Twist end_effector_twist, double elbow_velocity = 0.0);
+  RobotVelocity(const Twist &end_effector_twist, double elbow_velocity = 0.0);
 #pragma clang diagnostic pop
 
   /**

diff --git a/include/franky/twist.hpp b/include/franky/twist.hpp
@@ -11,25 +11,22 @@ namespace franky {
  */
 class Twist {
  public:
-  /**
-   * @brief Default constructor. Initializes linear and angular velocities to zero.
-   */
-  Twist() : linear_velocity_(Eigen::Vector3d::Zero()), angular_velocity_(Eigen::Vector3d::Zero()) {}
-
   Twist(const Twist &twist) = default;
 
   /**
    * @param linear_velocity The linear velocity.
    * @param angular_velocity The angular velocity.
    */
-  Twist(Eigen::Vector3d linear_velocity, Eigen::Vector3d angular_velocity)
+  explicit Twist(Eigen::Vector3d linear_velocity = Eigen::Vector3d::Zero(),
+                 Eigen::Vector3d angular_velocity = Eigen::Vector3d::Zero())
       : linear_velocity_(std::move(linear_velocity)), angular_velocity_(std::move(angular_velocity)) {}
 
   /**
    * @param vector_repr The vector representation of the twist.
    */
-  explicit Twist(const Vector6d &vector_repr)
-      : linear_velocity_(vector_repr.head<3>()), angular_velocity_(vector_repr.tail<3>()) {}
+  [[nodiscard]] static inline Twist fromVectorRepr(const Vector6d &vector_repr) {
+    return Twist{vector_repr.head<3>(), vector_repr.tail<3>()};
+  }
 
   /**
    * @brief Get the vector representation of the twist. It consists of the linear and angular velocities.
@@ -60,7 +57,7 @@ class Twist {
    */
   template<typename RotationMatrixType>
   [[nodiscard]] inline Twist transformWith(const RotationMatrixType &rotation) const {
-    return {rotation * linear_velocity_, rotation * angular_velocity_};
+    return Twist{rotation * linear_velocity_, rotation * angular_velocity_};
   }
 
   /**
@@ -72,7 +69,7 @@ class Twist {
    * @return The twist propagated through the link.
    */
   [[nodiscard]] inline Twist propagateThroughLink(const Eigen::Vector3d &link_translation) const {
-    return {linear_velocity_ + angular_velocity_.cross(link_translation), angular_velocity_};
+    return Twist{linear_velocity_ + angular_velocity_.cross(link_translation), angular_velocity_};
   }
 
   /**

diff --git a/python/python.cpp b/python/python.cpp
@@ -475,7 +475,12 @@ PYBIND11_MODULE(_franky, m) {
       ));
 
   py::class_<Twist>(m, "Twist")
-      .def(py::init<Eigen::Vector3d, Eigen::Vector3d>(), "linear_velocity"_a, "angular_velocity"_a)
+      .def(py::init([](
+          const std::optional<Eigen::Vector3d> &linear_velocity,
+          const std::optional<Eigen::Vector3d> &angular_velocity) {
+        return Twist(
+            linear_velocity.value_or(Eigen::Vector3d::Zero()), angular_velocity.value_or(Eigen::Vector3d::Zero()));
+      }), "linear_velocity"_a = std::nullopt, "angular_velocity"_a = std::nullopt)
       .def(py::init<const Twist &>()) // Copy constructor
       .def("propagate_through_link", &Twist::propagateThroughLink, "link_translation"_a)
       .def("transform_with",

diff --git a/src/robot_velocity.cpp b/src/robot_velocity.cpp
@@ -13,16 +13,16 @@ RobotVelocity::RobotVelocity() = default;
 
 RobotVelocity::RobotVelocity(const RobotVelocity &) = default;
 
-RobotVelocity::RobotVelocity(Twist end_effector_twist, double elbow_velocity)
-    : end_effector_twist_(std::move(end_effector_twist)),
+RobotVelocity::RobotVelocity(const Twist &end_effector_twist, double elbow_velocity)
+    : end_effector_twist_(end_effector_twist),
       elbow_velocity_(elbow_velocity) {}
 
 RobotVelocity::RobotVelocity(const Vector7d &vector_repr)
-    : RobotVelocity(Twist{vector_repr.head<6>()}, vector_repr[6]) {}
+    : RobotVelocity(Twist::fromVectorRepr(vector_repr.head<6>()), vector_repr[6]) {}
 
 RobotVelocity::RobotVelocity(const franka::CartesianVelocities franka_velocity)
     : RobotVelocity(
-    {
+    Twist{
         Vector6d::Map(franka_velocity.O_dP_EE.data()).tail<3>(),
         Vector6d::Map(franka_velocity.O_dP_EE.data()).head<3>()
     }, franka_velocity.elbow[0]) {}