Implemented proper elbow handling

Author: TimSchneider42

include/franky/elbow_state.hpp

@@ -0,0 +1,81 @@
+#pragma once
+
+#include <optional>
+#include <array>
+
+namespace franky {
+
+/**
+ * @brief Flip direction of a joint.
+ */
+enum class FlipDirection {
+  kNegative = -1,
+  kNeutral = 0,
+  kPositive = 1
+};
+
+/**
+ * @brief Elbow state of the robot.
+ *
+ * This class encapsulates the elbow state of a robot, which comprises the position of the 3rd joint
+ * and the flip direction of the 4th joint. For details see
+ * https://frankaemika.github.io/libfranka/0.15.0/structfranka_1_1RobotState.html#a43485841c427d70e7f36a912cc3116d1
+ */
+class ElbowState {
+ public:
+  /**
+   * @brief Construct an elbow state with the given joint position and optional flip direction.
+   *
+   * @param joint_3_pos  The position of the 3rd joint.
+   * @param joint_4_flip The flip direction of the 4th joint.
+   */
+  explicit ElbowState(double joint_3_pos, std::optional<FlipDirection> joint_4_flip = std::nullopt)
+      : joint_3_pos_(joint_3_pos), joint_4_flip_(joint_4_flip) {}
+
+  /**
+   * @brief Construct an elbow state from an array containing joint position and flip direction.
+   *
+   * @param elbow_state The joint position and flip direction as a double array.
+   */
+  explicit ElbowState(const std::array<double, 2>& elbow_state)
+      : joint_3_pos_(elbow_state[0]),
+        joint_4_flip_([&]() -> std::optional<FlipDirection> {
+          if (elbow_state[1] < 0.0) {
+            return FlipDirection::kNegative;
+          } else if (elbow_state[1] == 0.0) {
+            return FlipDirection::kNeutral;
+          } else {
+            return FlipDirection::kPositive;
+          }
+        }()) {}
+
+  ElbowState(const ElbowState&) = default;
+  ElbowState() = default;
+
+  /**
+   * @brief Get the joint position and flip direction as an array.
+   *
+   * @param default_flip_direction The default flip direction to use if not explicitly set.
+   * @return std::array containing the joint position and flip direction.
+   */
+  [[nodiscard]] inline std::array<double, 2> to_array(
+      FlipDirection default_flip_direction = FlipDirection::kNegative) const {
+    return {joint_3_pos_, static_cast<double>(joint_4_flip_.value_or(default_flip_direction))};
+  }
+
+  /**
+   * @brief The position of the 3rd joint.
+   */
+  [[nodiscard]] inline double joint_3_pos() const { return joint_3_pos_; }
+
+  /**
+   * @brief The flip direction of the 4th joint.
+   */
+  [[nodiscard]] inline std::optional<FlipDirection> joint_4_flip() const { return joint_4_flip_; }
+
+ private:
+  double joint_3_pos_{0.0};
+  std::optional<FlipDirection> joint_4_flip_{std::nullopt};
+};
+
+}  // namespace franky

include/franky/motion/cartesian_velocity_waypoint_motion.hpp

@@ -52,7 +52,10 @@ class CartesianVelocityWaypointMotion : public VelocityWaypointMotion<franka::Ca
   [[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const override;
 
   [[nodiscard]] franka::CartesianVelocities getControlSignal(
-      const franka::Duration &time_step, const ruckig::InputParameter<7> &input_parameter) override;
+      const franka::RobotState &robot_state,
+      const franka::Duration &time_step,
+      const std::optional<franka::CartesianVelocities> &previous_command,
+      const ruckig::InputParameter<7> &input_parameter) override;
 
  private:
   Affine ee_frame_;

include/franky/motion/cartesian_waypoint_motion.hpp

@@ -54,7 +54,10 @@ class CartesianWaypointMotion : public PositionWaypointMotion<franka::CartesianP
   [[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const override;
 
   [[nodiscard]] franka::CartesianPose getControlSignal(
-      const franka::Duration &time_step, const ruckig::InputParameter<7> &input_parameter) override;
+      const franka::RobotState &robot_state,
+      const franka::Duration &time_step,
+      const std::optional<franka::CartesianPose> &previous_command,
+      const ruckig::InputParameter<7> &input_parameter) override;
 
  private:
   CartesianState target_state_;

include/franky/motion/joint_velocity_waypoint_motion.hpp

@@ -45,7 +45,10 @@ class JointVelocityWaypointMotion : public VelocityWaypointMotion<franka::JointV
   [[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const override;
 
   [[nodiscard]] franka::JointVelocities getControlSignal(
-      const franka::Duration &time_step, const ruckig::InputParameter<7> &input_parameter) override;
+      const franka::RobotState &robot_state,
+      const franka::Duration &time_step,
+      const std::optional<franka::JointVelocities> &previous_command,
+      const ruckig::InputParameter<7> &input_parameter) override;
 };
 
 }  // namespace franky

include/franky/motion/joint_waypoint_motion.hpp

@@ -43,7 +43,10 @@ class JointWaypointMotion : public PositionWaypointMotion<franka::JointPositions
   [[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const override;
 
   [[nodiscard]] franka::JointPositions getControlSignal(
-      const franka::Duration &time_step, const ruckig::InputParameter<7> &input_parameter) override;
+      const franka::RobotState &robot_state,
+      const franka::Duration &time_step,
+      const std::optional<franka::JointPositions> &previous_command,
+      const ruckig::InputParameter<7> &input_parameter) override;
 };
 
 }  // namespace franky

include/franky/motion/waypoint_motion.hpp

@@ -103,10 +103,10 @@ class WaypointMotion : public Motion<ControlSignalType> {
           }
         }
         if (waypoint_iterator_ == waypoints_.end()) {
-          auto output_pose = getControlSignal(sub_step, input_parameter_);
+          auto command = getControlSignal(robot_state, time_step, previous_command, input_parameter_);
           if (!return_when_finished_)
-            return output_pose;
-          return franka::MotionFinished(output_pose);
+            return command;
+          return franka::MotionFinished(command);
         } else {
           checkWaypoint(*waypoint_iterator_);
           setNewWaypoint(robot_state, previous_command, *waypoint_iterator_, input_parameter_);
@@ -123,7 +123,7 @@ class WaypointMotion : public Motion<ControlSignalType> {
       }
     }
 
-    return getControlSignal(sub_step, input_parameter_);
+    return getControlSignal(robot_state, time_step, previous_command, input_parameter_);
   };
 
   virtual void initWaypointMotion(
@@ -142,7 +142,10 @@ class WaypointMotion : public Motion<ControlSignalType> {
   [[nodiscard]] virtual std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const = 0;
 
   [[nodiscard]] virtual ControlSignalType getControlSignal(
-      const franka::Duration &time_step, const ruckig::InputParameter<7> &input_parameter) = 0;
+      const franka::RobotState &robot_state,
+      const franka::Duration &time_step,
+      const std::optional<ControlSignalType> &previous_command,
+      const ruckig::InputParameter<7> &input_parameter) = 0;
 
   virtual void setInputLimits(const WaypointType &waypoint, ruckig::InputParameter<7> &input_parameter) const = 0;
 

include/franky/robot.hpp

@@ -248,7 +248,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]},
+    return {{Affine(Eigen::Matrix4d::Map(s.O_T_EE.data())), ElbowState{s.elbow}},
             RobotVelocity(franka::CartesianVelocities(s.O_dP_EE_c, s.delbow_c))};
   }
 

include/franky/robot_pose.hpp

@@ -5,6 +5,7 @@
 #include <franka/control_types.h>
 
 #include "franky/types.hpp"
+#include "franky/elbow_state.hpp"
 
 namespace franky {
 
@@ -24,31 +25,33 @@ class RobotPose {
 #pragma clang diagnostic ignored "-Wimplicit-conversion"
   /**
    * @param end_effector_pose The pose of the end effector.
-   * @param elbow_position The position of the elbow. Optional.
+   * @param elbow_state       The state of the elbow. Optional.
    */
-  RobotPose(Affine end_effector_pose, std::optional<double> elbow_position = std::nullopt);
+  RobotPose(Affine end_effector_pose, std::optional<ElbowState> elbow_state = std::nullopt);
 #pragma clang diagnostic pop
 
   /**
-   * @param vector_repr The vector representation of the pose.
-   * @param ignore_elbow Whether to ignore the elbow position. Default is false.
+   * @param vector_repr    The vector representation of the pose.
+   * @param ignore_elbow   Whether to ignore the elbow state. Default is false.
+   * @param flip_direction The flip direction to use if the elbow is not ignored. Default is negative.
    */
-  explicit RobotPose(const Vector7d &vector_repr, bool ignore_elbow = false);
+  explicit RobotPose(
+      const Vector7d &vector_repr, bool ignore_elbow = false, FlipDirection flip_direction = FlipDirection::kNegative);
 
   /**
    * @param vector_repr The vector representation of the pose.
-   * @param elbow_position The position of the elbow. Optional.
+   * @param elbow_state The state of the elbow. Optional.
    */
-  explicit RobotPose(const Vector6d &vector_repr, std::optional<double> elbow_position = std::nullopt);
+  explicit RobotPose(const Vector6d &vector_repr, std::optional<ElbowState> elbow_state = std::nullopt);
 
   /**
    * @param franka_pose The franka pose.
    */
-  explicit RobotPose(franka::CartesianPose franka_pose);
+  explicit RobotPose(const franka::CartesianPose &franka_pose);
 
   /**
    * @brief Get the vector representation of the pose, which consists of the end effector position and orientation
-   * (as rotation vector) and the elbow position.
+   * (as rotation vector) and the elbow position. Does not contain the flip component of the elbow state.
    *
    * @return The vector representation of the pose.
    */
@@ -57,9 +60,11 @@ class RobotPose {
   /**
    * @brief Convert this pose to a franka pose.
    *
+   * @param default_elbow_flip_direction The default flip direction to use if the elbow flip direction is not set.
    * @return The franka pose.
    */
-  [[nodiscard]] franka::CartesianPose as_franka_pose() const;
+  [[nodiscard]] franka::CartesianPose as_franka_pose(
+      FlipDirection default_elbow_flip_direction = FlipDirection::kNegative) const;
 
   /**
    * @brief Transform this pose with a given affine transformation from the left side.
@@ -68,7 +73,7 @@ class RobotPose {
    * @return The transformed robot pose.
    */
   [[nodiscard]] inline RobotPose leftTransform(const Affine &transform) const {
-    return {transform * end_effector_pose_, elbow_position_};
+    return {transform * end_effector_pose_, elbow_state_};
   }
 
   /**
@@ -78,7 +83,7 @@ class RobotPose {
    * @return The transformed robot pose.
    */
   [[nodiscard]] inline RobotPose rightTransform(const Affine &transform) const {
-    return {end_effector_pose_ * transform, elbow_position_};
+    return {end_effector_pose_ * transform, elbow_state_};
   }
 
   /**
@@ -93,13 +98,13 @@ class RobotPose {
   }
 
   /**
-   * @brief Get the pose with a new elbow position.
+   * @brief Get the pose with a new elbow state.
    *
-   * @param elbow_position The new elbow position.
-   * @return The pose with the new elbow position.
+   * @param elbow_state The new elbow state.
+   * @return The pose with the new elbow state.
    */
-  [[nodiscard]] inline RobotPose withElbowPosition(const std::optional<double> elbow_position) const {
-    return {end_effector_pose_, elbow_position};
+  [[nodiscard]] inline RobotPose withElbowState(const std::optional<ElbowState> elbow_state) const {
+    return {end_effector_pose_, elbow_state};
   }
 
   /**
@@ -112,17 +117,17 @@ class RobotPose {
   }
 
   /**
-   * @brief Get the elbow position.
+   * @brief Get the elbow state.
    *
-   * @return The elbow position.
+   * @return The elbow state.
    */
-  [[nodiscard]] inline std::optional<double> elbow_position() const {
-    return elbow_position_;
+  [[nodiscard]] inline std::optional<ElbowState> elbow_state() const {
+    return elbow_state_;
   }
 
  private:
   Affine end_effector_pose_;
-  std::optional<double> elbow_position_;
+  std::optional<ElbowState> elbow_state_;
 };
 
 inline RobotPose operator*(const RobotPose &robot_pose, const Affine &right_transform) {

include/franky/robot_velocity.hpp

@@ -6,6 +6,7 @@
 
 #include "franky/types.hpp"
 #include "franky/twist.hpp"
+#include "franky/elbow_state.hpp"
 
 namespace franky {
 
@@ -59,12 +60,14 @@ class RobotVelocity {
   /**
    * @brief Get the franka velocity.
    *
-   * @param elbow_position The elbow position to use. Note, that franka::CartesianVelocities contains the elbow
-   *                       position and not the elbow velocity, contrary to RobotVelocity.
+   * @param elbow_state                  The elbow state to use. Note, that franka::CartesianVelocities contains the
+   *                                     elbow state and not the elbow velocity, contrary to RobotVelocity.
+   * @param default_elbow_flip_direction The default flip direction of the elbow if it is not set.
    * @return The franka velocity.
    */
   [[nodiscard]] franka::CartesianVelocities as_franka_velocity(
-      std::optional<double> elbow_position = std::nullopt) const;
+      const std::optional<ElbowState> &elbow_state = std::nullopt,
+      FlipDirection default_elbow_flip_direction = FlipDirection::kNegative) const;
 
   /**
    * @brief Transform the frame of the velocity by applying the given affine transform.

python/bind_enums.cpp

@@ -34,4 +34,9 @@ void bind_enums(py::module &m) {
       .value("Reflex", franka::RobotMode::kReflex)
       .value("UserStopped", franka::RobotMode::kUserStopped)
       .value("AutomaticErrorRecovery", franka::RobotMode::kAutomaticErrorRecovery);
+
+  py::enum_<FlipDirection>(m, "FlipDirection")
+      .value("Negative", FlipDirection::kNegative)
+      .value("Neutral", FlipDirection::kNeutral)
+      .value("Positive", FlipDirection::kPositive);
 }