From 06bbeb3e5099e21c27386a5b30b5bf9de36e5c87 Mon Sep 17 00:00:00 2001 From: Marco Morganti Date: Fri, 28 Jan 2022 13:39:19 +0100 Subject: [PATCH 1/7] ADD: signal O_ddP_O and errors gravity_vector_initialization_timeout and gravity_vector_invalid_reading --- include/franka/errors.h | 22 +++++++++++++++++++--- include/franka/robot_state.h | 6 ++++++ src/errors.cpp | 14 +++++++++++--- src/robot_impl.cpp | 1 + src/robot_state.cpp | 1 + test/helpers.cpp | 9 ++++++++- 6 files changed, 46 insertions(+), 7 deletions(-) diff --git a/include/franka/errors.h b/include/franka/errors.h index 396684da..79b95a95 100644 --- a/include/franka/errors.h +++ b/include/franka/errors.h @@ -17,7 +17,7 @@ namespace franka { */ struct Errors { private: - std::array errors_{}; + std::array errors_{}; public: /** @@ -46,7 +46,7 @@ struct Errors { * * @param errors Array of error flags. */ - Errors(const std::array& errors); + Errors(const std::array& 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. + */ + const bool& gravity_vector_initialization_timeout; + /** + * True if the base acceleration O_ddP_O cannot be determined. + */ + const bool& gravity_vector_invalid_reading; }; /** @@ -237,6 +253,6 @@ struct Errors { * * @return Ostream instance */ -std::ostream& operator<<(std::ostream& ostream, const Errors& errors); + std::ostream& operator<<(std::ostream& ostream, const Errors& errors); } // namespace franka diff --git a/include/franka/robot_state.h b/include/franka/robot_state.h index 864860fd..e56a1021 100644 --- a/include/franka/robot_state.h +++ b/include/franka/robot_state.h @@ -326,6 +326,12 @@ struct RobotState { */ std::array O_dP_EE_d{}; // NOLINT(readability-identifier-naming) + /** + * \f$^OddP_O\f$ + * Linear component of the acceleration of the robot's base, expressed in the base frame. + */ + std::array 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 index 550e51de..cbe2a2ac 100644 --- 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{}) {} +Errors::Errors() : Errors(std::array{}) {} Errors::Errors(const Errors& other) : Errors(other.errors_) {} @@ -21,7 +21,7 @@ Errors& Errors::operator=(Errors other) { return *this; } -Errors::Errors(const std::array& errors) // NOLINT(modernize-pass-by-value) +Errors::Errors(const std::array& errors) // NOLINT(modernize-pass-by-value) : errors_(errors), joint_position_limits_violation( errors_[static_cast(Error::kJointPositionLimitsViolation)]), @@ -90,7 +90,15 @@ Errors::Errors(const std::array& errors) // NOLINT(modernize-pass-by- tau_j_range_violation(errors_[static_cast(Error::kTauJRangeViolation)]), instability_detected(errors_[static_cast(Error::kInstabilityDetection)]), joint_move_in_wrong_direction( - errors_[static_cast(Error::kJointMoveInWrongDirection)]) {} + errors_[static_cast(Error::kJointMoveInWrongDirection)]), + cartesian_spline_motion_generator_violation( + errors_[static_cast(Error::kCartesianSplineViolation)]), + joint_via_motion_generator_planning_joint_limit_violation( + errors_[static_cast(Error::kJointViaPlanLimitViolation)]), + gravity_vector_initialization_timeout( + errors_[static_cast(Error::kGravityVectorInitializationTimeout)]), + gravity_vector_invalid_reading( + errors_[static_cast(Error::kGravityVectorInvalidReading)]) {} 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 index 611790e4..4d95caec 100644 --- 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 index af9adac3..504716de 100644 --- a/src/robot_state.cpp +++ b/src/robot_state.cpp @@ -75,6 +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_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 diff --git a/test/helpers.cpp b/test/helpers.cpp index 8524472a..948e2fac 100644 --- 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.gravity_vector_initialization_timeout == + rhs.gravity_vector_initialization_timeout && lhs.gravity_vector_invalid_reading == + rhs.gravity_vector_invalid_reading; } } // namespace franka From d5fe23364313c03a06603e4c688e48f094eb796b Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Thu, 17 Feb 2022 09:58:59 +0100 Subject: [PATCH 2/7] CHANGE: Update common submodule to new base_acceleration naming --- common | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common b/common index af64d3e6..2b852fab 160000 --- a/common +++ b/common @@ -1 +1 @@ -Subproject commit af64d3e64087450d1ccc1639570183eb99d4c401 +Subproject commit 2b852fabb6176def77351cb0ba00f8033397409f From fb6d0d48a2bbcc629fd8466c1c195c6d05b3d7ce Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Thu, 17 Feb 2022 10:43:19 +0100 Subject: [PATCH 3/7] CHANGE: Rename gravity_vector -> base_acceleration --- include/franka/errors.h | 6 +++--- src/errors.cpp | 4 ++-- test/helpers.cpp | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/include/franka/errors.h b/include/franka/errors.h index 79b95a95..852de6c2 100644 --- a/include/franka/errors.h +++ b/include/franka/errors.h @@ -236,13 +236,13 @@ struct Errors { */ const bool& joint_via_motion_generator_planning_joint_limit_violation; /** - * True if the gravity vector could not be initialized. + * True if the gravity vector could not be initialized by measureing the base acceleration. */ - const bool& gravity_vector_initialization_timeout; + const bool& base_acceleration_initialization_timeout; /** * True if the base acceleration O_ddP_O cannot be determined. */ - const bool& gravity_vector_invalid_reading; + const bool& base_acceleration_invalid_reading; }; /** diff --git a/src/errors.cpp b/src/errors.cpp index cbe2a2ac..4fd8d2ce 100644 --- a/src/errors.cpp +++ b/src/errors.cpp @@ -95,9 +95,9 @@ Errors::Errors(const std::array& errors) // NOLINT(modernize-pass-by- errors_[static_cast(Error::kCartesianSplineViolation)]), joint_via_motion_generator_planning_joint_limit_violation( errors_[static_cast(Error::kJointViaPlanLimitViolation)]), - gravity_vector_initialization_timeout( + base_acceleration_initialization_timeout( errors_[static_cast(Error::kGravityVectorInitializationTimeout)]), - gravity_vector_invalid_reading( + base_acceleration_invalid_reading( errors_[static_cast(Error::kGravityVectorInvalidReading)]) {} Errors::operator bool() const noexcept { diff --git a/test/helpers.cpp b/test/helpers.cpp index 948e2fac..bd475063 100644 --- a/test/helpers.cpp +++ b/test/helpers.cpp @@ -717,8 +717,8 @@ bool operator==(const Errors& lhs, const Errors& rhs) { 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.gravity_vector_initialization_timeout == - rhs.gravity_vector_initialization_timeout && lhs.gravity_vector_invalid_reading == - rhs.gravity_vector_invalid_reading; + lhs.base_acceleration_initialization_timeout == + rhs.base_acceleration_initialization_timeout && lhs.base_acceleration_invalid_reading == + rhs.base_acceleration_invalid_reading; } } // namespace franka From a682f1a2d9e9e31aa0a486083df51644aae87f05 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Thu, 17 Feb 2022 10:47:58 +0100 Subject: [PATCH 4/7] FIX: Make docstring about O_ddP_O clearer --- include/franka/robot_state.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/include/franka/robot_state.h b/include/franka/robot_state.h index e56a1021..452bb569 100644 --- a/include/franka/robot_state.h +++ b/include/franka/robot_state.h @@ -327,8 +327,10 @@ struct RobotState { std::array O_dP_EE_d{}; // NOLINT(readability-identifier-naming) /** - * \f$^OddP_O\f$ - * Linear component of the acceleration of the robot's base, expressed in the base frame. + * \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 O_ddP_O{}; // NOLINT(readability-identifier-naming) From 7706d160e890f60b1103fe42ae15681729825bf8 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Thu, 17 Feb 2022 10:51:44 +0100 Subject: [PATCH 5/7] ADD: CHANGELOG --- CHANGELOG.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 78543a47..4f6d8240 100644 --- 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 From d1c1c09b81f128bead5f16cb507bcd74de17f28b Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Fri, 18 Feb 2022 10:39:38 +0100 Subject: [PATCH 6/7] CHANGE: Use new FCI version 5 of libfranka-common --- common | 2 +- src/errors.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/common b/common index 2b852fab..e6aa0fc2 160000 --- a/common +++ b/common @@ -1 +1 @@ -Subproject commit 2b852fabb6176def77351cb0ba00f8033397409f +Subproject commit e6aa0fc210d93fe618bfd8956829a264d5476ba8 diff --git a/src/errors.cpp b/src/errors.cpp index 4fd8d2ce..fbc80c0a 100644 --- a/src/errors.cpp +++ b/src/errors.cpp @@ -96,9 +96,9 @@ Errors::Errors(const std::array& errors) // NOLINT(modernize-pass-by- joint_via_motion_generator_planning_joint_limit_violation( errors_[static_cast(Error::kJointViaPlanLimitViolation)]), base_acceleration_initialization_timeout( - errors_[static_cast(Error::kGravityVectorInitializationTimeout)]), + errors_[static_cast(Error::kBaseAccelerationInitializationTimeout)]), base_acceleration_invalid_reading( - errors_[static_cast(Error::kGravityVectorInvalidReading)]) {} + errors_[static_cast(Error::kBaseAccelerationInvalidReading)]) {} Errors::operator bool() const noexcept { return std::any_of(errors_.cbegin(), errors_.cend(), [](bool x) { return x; }); From b8c71db2020979490da681e16fd296060acd201a Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Thu, 3 Mar 2022 09:49:00 +0100 Subject: [PATCH 7/7] FIX: Make Clang-Format happy --- include/franka/errors.h | 2 +- src/robot_state.cpp | 3 +-- test/helpers.cpp | 4 ++-- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/include/franka/errors.h b/include/franka/errors.h index 852de6c2..742d9a7c 100644 --- a/include/franka/errors.h +++ b/include/franka/errors.h @@ -253,6 +253,6 @@ struct Errors { * * @return Ostream instance */ - std::ostream& operator<<(std::ostream& ostream, const Errors& errors); +std::ostream& operator<<(std::ostream& ostream, const Errors& errors); } // namespace franka diff --git a/src/robot_state.cpp b/src/robot_state.cpp index 504716de..8f81ca39 100644 --- a/src/robot_state.cpp +++ b/src/robot_state.cpp @@ -75,8 +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_ddP_O\": " << robot_state.O_ddP_O - << ", \"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 index bd475063..e01ae518 100644 --- a/test/helpers.cpp +++ b/test/helpers.cpp @@ -718,7 +718,7 @@ bool operator==(const Errors& lhs, const Errors& rhs) { 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; + rhs.base_acceleration_initialization_timeout && + lhs.base_acceleration_invalid_reading == rhs.base_acceleration_invalid_reading; } } // namespace franka