Merge pull request #146 in SWDEV/libfranka from variable_g_vec_for_wa…

…ll_mount to develop * commit 'b8c71db2020979490da681e16fd296060acd201a': FIX: Make Clang-Format happy CHANGE: Use new FCI version 5 of libfranka-common ADD: CHANGELOG FIX: Make docstring about O_ddP_O clearer CHANGE: Rename gravity_vector -> base_acceleration CHANGE: Update common submodule to new base_acceleration naming ADD: signal O_ddP_O and errors gravity_vector_initialization_timeout and gravity_vector_invalid_reading
frankaemika · Mar 8, 2022 · e612260 · e612260
2 parents 976a143 + b8c71db
commit e612260
Show file tree

Hide file tree

Showing 8 changed files with 55 additions and 8 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,5 +1,12 @@
 # CHANGELOG
 
+## 0.9.0 - UNRELEASED
+
+Requires Panda system version >= 4.2.1
+
+  * **BREAKING** Add `O_ddP_O` base acceleration to robot state
+  * **BREAKING** New `base_acceleration_initialization_timeout` & `base_acceleration_invalid_reading` reflexes
+
 ## 0.8.1 - UNRELEASED
 
 Requires Panda system version >= 4.0.0

diff --git a/common b/common
diff --git a/include/franka/errors.h b/include/franka/errors.h
@@ -17,7 +17,7 @@ namespace franka {
  */
 struct Errors {
  private:
-  std::array<bool, 37> errors_{};
+  std::array<bool, 41> errors_{};
 
  public:
   /**
@@ -46,7 +46,7 @@ struct Errors {
    *
    * @param errors Array of error flags.
    */
-  Errors(const std::array<bool, 37>& errors);
+  Errors(const std::array<bool, 41>& errors);
 
   /**
    * Check if any error flag is set to true.
@@ -227,6 +227,22 @@ struct Errors {
    * further towards the limit.
    */
   const bool& joint_move_in_wrong_direction;
+  /**
+   * True if the generated motion violates a joint limit.
+   */
+  const bool& cartesian_spline_motion_generator_violation;
+  /**
+   * True if the generated motion violates a joint limit.
+   */
+  const bool& joint_via_motion_generator_planning_joint_limit_violation;
+  /**
+   * True if the gravity vector could not be initialized by measureing the base acceleration.
+   */
+  const bool& base_acceleration_initialization_timeout;
+  /**
+   * True if the base acceleration O_ddP_O cannot be determined.
+   */
+  const bool& base_acceleration_invalid_reading;
 };
 
 /**

diff --git a/include/franka/robot_state.h b/include/franka/robot_state.h
@@ -326,6 +326,14 @@ struct RobotState {
    */
   std::array<double, 6> O_dP_EE_d{};  // NOLINT(readability-identifier-naming)
 
+  /**
+   * \f${^OddP}_O\f$
+   * Linear component of the acceleration of the robot's base, expressed in frame parallel to the
+   * @ref o-frame "base frame", i.e. the base's translational acceleration. If the base is resting
+   * this shows the direction of the gravity vector.
+   */
+  std::array<double, 3> O_ddP_O{};  // NOLINT(readability-identifier-naming)
+
   /**
    * \f${^OT_{EE}}_{c}\f$
    * Last commanded end effector pose of motion generation in @ref o-frame "base frame".

diff --git a/src/errors.cpp b/src/errors.cpp
@@ -12,7 +12,7 @@ using Error = research_interface::robot::Error;
 
 namespace franka {
 
-Errors::Errors() : Errors(std::array<bool, 37>{}) {}
+Errors::Errors() : Errors(std::array<bool, 41>{}) {}
 
 Errors::Errors(const Errors& other) : Errors(other.errors_) {}
 
@@ -21,7 +21,7 @@ Errors& Errors::operator=(Errors other) {
   return *this;
 }
 
-Errors::Errors(const std::array<bool, 37>& errors)  // NOLINT(modernize-pass-by-value)
+Errors::Errors(const std::array<bool, 41>& errors)  // NOLINT(modernize-pass-by-value)
     : errors_(errors),
       joint_position_limits_violation(
           errors_[static_cast<size_t>(Error::kJointPositionLimitsViolation)]),
@@ -90,7 +90,15 @@ Errors::Errors(const std::array<bool, 37>& errors)  // NOLINT(modernize-pass-by-
       tau_j_range_violation(errors_[static_cast<size_t>(Error::kTauJRangeViolation)]),
       instability_detected(errors_[static_cast<size_t>(Error::kInstabilityDetection)]),
       joint_move_in_wrong_direction(
-          errors_[static_cast<size_t>(Error::kJointMoveInWrongDirection)]) {}
+          errors_[static_cast<size_t>(Error::kJointMoveInWrongDirection)]),
+      cartesian_spline_motion_generator_violation(
+          errors_[static_cast<size_t>(Error::kCartesianSplineViolation)]),
+      joint_via_motion_generator_planning_joint_limit_violation(
+          errors_[static_cast<size_t>(Error::kJointViaPlanLimitViolation)]),
+      base_acceleration_initialization_timeout(
+          errors_[static_cast<size_t>(Error::kBaseAccelerationInitializationTimeout)]),
+      base_acceleration_invalid_reading(
+          errors_[static_cast<size_t>(Error::kBaseAccelerationInvalidReading)]) {}
 
 Errors::operator bool() const noexcept {
   return std::any_of(errors_.cbegin(), errors_.cend(), [](bool x) { return x; });

diff --git a/src/robot_impl.cpp b/src/robot_impl.cpp
@@ -347,6 +347,7 @@ RobotState convertRobotState(const research_interface::robot::RobotState& robot_
   converted.O_F_ext_hat_K = robot_state.O_F_ext_hat_K;
   converted.K_F_ext_hat_K = robot_state.K_F_ext_hat_K;
   converted.O_dP_EE_d = robot_state.O_dP_EE_d;
+  converted.O_ddP_O = robot_state.O_ddP_O;
   converted.O_T_EE_c = robot_state.O_T_EE_c;
   converted.O_dP_EE_c = robot_state.O_dP_EE_c;
   converted.O_ddP_EE_c = robot_state.O_ddP_EE_c;

diff --git a/src/robot_state.cpp b/src/robot_state.cpp
@@ -75,7 +75,7 @@ std::ostream& operator<<(std::ostream& ostream, const franka::RobotState& robot_
           << ", \"O_F_ext_hat_K\": " << robot_state.O_F_ext_hat_K
           << ", \"K_F_ext_hat_K\": " << robot_state.K_F_ext_hat_K
           << ", \"O_dP_EE_d\": " << robot_state.O_dP_EE_d
-          << ", \"O_T_EE_c\": " << robot_state.O_T_EE_c
+          << ", \"O_ddP_O\": " << robot_state.O_ddP_O << ", \"O_T_EE_c\": " << robot_state.O_T_EE_c
           << ", \"O_dP_EE_c\": " << robot_state.O_dP_EE_c
           << ", \"O_ddP_EE_c\": " << robot_state.O_ddP_EE_c << ", \"theta\": " << robot_state.theta
           << ", \"dtheta\": " << robot_state.dtheta

diff --git a/test/helpers.cpp b/test/helpers.cpp
@@ -712,6 +712,13 @@ bool operator==(const Errors& lhs, const Errors& rhs) {
              rhs.joint_p2p_insufficient_torque_for_planning &&
          lhs.tau_j_range_violation == rhs.tau_j_range_violation &&
          lhs.instability_detected == rhs.instability_detected &&
-         lhs.joint_move_in_wrong_direction == rhs.joint_move_in_wrong_direction;
+         lhs.joint_move_in_wrong_direction == rhs.joint_move_in_wrong_direction &&
+         lhs.cartesian_spline_motion_generator_violation ==
+             rhs.cartesian_spline_motion_generator_violation &&
+         lhs.joint_via_motion_generator_planning_joint_limit_violation ==
+             rhs.joint_via_motion_generator_planning_joint_limit_violation &&
+         lhs.base_acceleration_initialization_timeout ==
+             rhs.base_acceleration_initialization_timeout &&
+         lhs.base_acceleration_invalid_reading == rhs.base_acceleration_invalid_reading;
 }
 }  // namespace franka
+13 −1		include/research_interface/robot/error.h
+3 −2		include/research_interface/robot/rbk_types.h
+1 −1		include/research_interface/robot/service_types.h