diff --git a/include/franka/errors.h b/include/franka/errors.h index 396684da..40d93cb5 100644 --- a/include/franka/errors.h +++ b/include/franka/errors.h @@ -17,20 +17,23 @@ namespace franka { */ struct Errors { private: - std::array errors_{}; + static constexpr int kMaxNumErrors = 37; + std::array errors_{}; public: + const std::array& errors() const { return errors_; } + /** * Creates an empty Errors instance. */ - Errors(); + Errors() = default; /** * Copy constructs a new Errors instance. * * @param[in] other Other Errors instance. */ - Errors(const Errors& other); + Errors(const Errors& other) = default; /** * Assigns this Errors instance from another Errors value. @@ -39,7 +42,7 @@ struct Errors { * * @return Errors instance. */ - Errors& operator=(Errors other); + Errors& operator=(const Errors& other) = default; /** * Creates a new Errors instance from the given array. @@ -67,166 +70,166 @@ struct Errors { /** * True if the robot moved past the joint limits. */ - const bool& joint_position_limits_violation; + const bool& joint_position_limits_violation() const; /** * True if the robot moved past any of the virtual walls. */ - const bool& cartesian_position_limits_violation; + const bool& cartesian_position_limits_violation() const; /** * True if the robot would have collided with itself. */ - const bool& self_collision_avoidance_violation; + const bool& self_collision_avoidance_violation() const; /** * True if the robot exceeded joint velocity limits. */ - const bool& joint_velocity_violation; + const bool& joint_velocity_violation() const; /** * True if the robot exceeded Cartesian velocity limits. */ - const bool& cartesian_velocity_violation; + const bool& cartesian_velocity_violation() const; /** * True if the robot exceeded safety threshold during force control. */ - const bool& force_control_safety_violation; + const bool& force_control_safety_violation() const; /** * True if a collision was detected, i.e.\ the robot exceeded a torque threshold in a joint * motion. */ - const bool& joint_reflex; + const bool& joint_reflex() const; /** * True if a collision was detected, i.e.\ the robot exceeded a torque threshold in a Cartesian * motion. */ - const bool& cartesian_reflex; + const bool& cartesian_reflex() const; /** * True if internal motion generator did not reach the goal pose. */ - const bool& max_goal_pose_deviation_violation; + const bool& max_goal_pose_deviation_violation() const; /** * True if internal motion generator deviated from the path. */ - const bool& max_path_pose_deviation_violation; + const bool& max_path_pose_deviation_violation() const; /** * True if Cartesian velocity profile for internal motions was exceeded. */ - const bool& cartesian_velocity_profile_safety_violation; + const bool& cartesian_velocity_profile_safety_violation() const; /** * True if an external joint position motion generator was started with a pose too far from the * current pose. */ - const bool& joint_position_motion_generator_start_pose_invalid; + const bool& joint_position_motion_generator_start_pose_invalid() const; /** * True if an external joint motion generator would move into a joint limit. */ - const bool& joint_motion_generator_position_limits_violation; + const bool& joint_motion_generator_position_limits_violation() const; /** * True if an external joint motion generator exceeded velocity limits. */ - const bool& joint_motion_generator_velocity_limits_violation; + const bool& joint_motion_generator_velocity_limits_violation() const; /** * True if commanded velocity in joint motion generators is discontinuous (target values are too * far apart). */ - const bool& joint_motion_generator_velocity_discontinuity; + const bool& joint_motion_generator_velocity_discontinuity() const; /** * True if commanded acceleration in joint motion generators is discontinuous (target values are * too far apart). */ - const bool& joint_motion_generator_acceleration_discontinuity; + const bool& joint_motion_generator_acceleration_discontinuity() const; /** * True if an external Cartesian position motion generator was started with a pose too far from * the current pose. */ - const bool& cartesian_position_motion_generator_start_pose_invalid; + const bool& cartesian_position_motion_generator_start_pose_invalid() const; /** * True if an external Cartesian motion generator would move into an elbow limit. */ - const bool& cartesian_motion_generator_elbow_limit_violation; + const bool& cartesian_motion_generator_elbow_limit_violation() const; /** * True if an external Cartesian motion generator would move with too high velocity. */ - const bool& cartesian_motion_generator_velocity_limits_violation; + const bool& cartesian_motion_generator_velocity_limits_violation() const; /** * True if commanded velocity in Cartesian motion generators is discontinuous (target values are * too far apart). */ - const bool& cartesian_motion_generator_velocity_discontinuity; + const bool& cartesian_motion_generator_velocity_discontinuity() const; /** * True if commanded acceleration in Cartesian motion generators is discontinuous (target values * are too far apart). */ - const bool& cartesian_motion_generator_acceleration_discontinuity; + const bool& cartesian_motion_generator_acceleration_discontinuity() const; /** * True if commanded elbow values in Cartesian motion generators are inconsistent. */ - const bool& cartesian_motion_generator_elbow_sign_inconsistent; + const bool& cartesian_motion_generator_elbow_sign_inconsistent() const; /** * True if the first elbow value in Cartesian motion generators is too far from initial one. */ - const bool& cartesian_motion_generator_start_elbow_invalid; + const bool& cartesian_motion_generator_start_elbow_invalid() const; /** * True if the joint position limits would be exceeded after IK calculation. */ - const bool& cartesian_motion_generator_joint_position_limits_violation; + const bool& cartesian_motion_generator_joint_position_limits_violation() const; /** * True if the joint velocity limits would be exceeded after IK calculation. */ - const bool& cartesian_motion_generator_joint_velocity_limits_violation; + const bool& cartesian_motion_generator_joint_velocity_limits_violation() const; /** * True if the joint velocity in Cartesian motion generators is discontinuous after IK * calculation. */ - const bool& cartesian_motion_generator_joint_velocity_discontinuity; + const bool& cartesian_motion_generator_joint_velocity_discontinuity() const; /** * True if the joint acceleration in Cartesian motion generators is discontinuous after IK * calculation. */ - const bool& cartesian_motion_generator_joint_acceleration_discontinuity; + const bool& cartesian_motion_generator_joint_acceleration_discontinuity() const; /** * True if the Cartesian pose is not a valid transformation matrix. */ - const bool& cartesian_position_motion_generator_invalid_frame; + const bool& cartesian_position_motion_generator_invalid_frame() const; /** * True if desired force exceeds the safety thresholds. */ - const bool& force_controller_desired_force_tolerance_violation; + const bool& force_controller_desired_force_tolerance_violation() const; /** * True if the torque set by the external controller is discontinuous. */ - const bool& controller_torque_discontinuity; + const bool& controller_torque_discontinuity() const; /** * True if the start elbow sign was inconsistent. * * Applies only to motions started from Desk. */ - const bool& start_elbow_sign_inconsistent; + const bool& start_elbow_sign_inconsistent() const; /** * True if minimum network communication quality could not be held during a motion. */ - const bool& communication_constraints_violation; + const bool& communication_constraints_violation() const; /** * True if commanded values would result in exceeding the power limit. */ - const bool& power_limit_violation; + const bool& power_limit_violation() const; /** * True if the robot is overloaded for the required motion. * * Applies only to motions started from Desk. */ - const bool& joint_p2p_insufficient_torque_for_planning; + const bool& joint_p2p_insufficient_torque_for_planning() const; /** * True if the measured torque signal is out of the safe range. */ - const bool& tau_j_range_violation; + const bool& tau_j_range_violation() const; /** * True if an instability is detected. */ - const bool& instability_detected; + const bool& instability_detected() const; /** * True if the robot is in joint position limits violation error and the user guides the robot * further towards the limit. */ - const bool& joint_move_in_wrong_direction; + const bool& joint_move_in_wrong_direction() const; }; /** diff --git a/src/errors.cpp b/src/errors.cpp index 550e51de..271caf2a 100644 --- a/src/errors.cpp +++ b/src/errors.cpp @@ -12,85 +12,8 @@ using Error = research_interface::robot::Error; namespace franka { -Errors::Errors() : Errors(std::array{}) {} - -Errors::Errors(const Errors& other) : Errors(other.errors_) {} - -Errors& Errors::operator=(Errors other) { - std::swap(errors_, other.errors_); - return *this; -} - Errors::Errors(const std::array& errors) // NOLINT(modernize-pass-by-value) - : errors_(errors), - joint_position_limits_violation( - errors_[static_cast(Error::kJointPositionLimitsViolation)]), - cartesian_position_limits_violation( - errors_[static_cast(Error::kCartesianPositionLimitsViolation)]), - self_collision_avoidance_violation( - errors_[static_cast(Error::kSelfcollisionAvoidanceViolation)]), - joint_velocity_violation(errors_[static_cast(Error::kJointVelocityViolation)]), - cartesian_velocity_violation( - errors_[static_cast(Error::kCartesianVelocityViolation)]), - force_control_safety_violation( - errors_[static_cast(Error::kForceControlSafetyViolation)]), - joint_reflex(errors_[static_cast(Error::kJointReflex)]), - cartesian_reflex(errors_[static_cast(Error::kCartesianReflex)]), - max_goal_pose_deviation_violation( - errors_[static_cast(Error::kMaxGoalPoseDeviationViolation)]), - max_path_pose_deviation_violation( - errors_[static_cast(Error::kMaxPathPoseDeviationViolation)]), - cartesian_velocity_profile_safety_violation( - errors_[static_cast(Error::kCartesianVelocityProfileSafetyViolation)]), - joint_position_motion_generator_start_pose_invalid( - errors_[static_cast(Error::kJointPositionMotionGeneratorStartPoseInvalid)]), - joint_motion_generator_position_limits_violation( - errors_[static_cast(Error::kJointMotionGeneratorPositionLimitsViolation)]), - joint_motion_generator_velocity_limits_violation( - errors_[static_cast(Error::kJointMotionGeneratorVelocityLimitsViolation)]), - joint_motion_generator_velocity_discontinuity( - errors_[static_cast(Error::kJointMotionGeneratorVelocityDiscontinuity)]), - joint_motion_generator_acceleration_discontinuity( - errors_[static_cast(Error::kJointMotionGeneratorAccelerationDiscontinuity)]), - cartesian_position_motion_generator_start_pose_invalid( - errors_[static_cast(Error::kCartesianPositionMotionGeneratorStartPoseInvalid)]), - cartesian_motion_generator_elbow_limit_violation( - errors_[static_cast(Error::kCartesianMotionGeneratorElbowLimitViolation)]), - cartesian_motion_generator_velocity_limits_violation( - errors_[static_cast(Error::kCartesianMotionGeneratorVelocityLimitsViolation)]), - cartesian_motion_generator_velocity_discontinuity( - errors_[static_cast(Error::kCartesianMotionGeneratorVelocityDiscontinuity)]), - cartesian_motion_generator_acceleration_discontinuity( - errors_[static_cast(Error::kCartesianMotionGeneratorAccelerationDiscontinuity)]), - cartesian_motion_generator_elbow_sign_inconsistent( - errors_[static_cast(Error::kCartesianMotionGeneratorElbowSignInconsistent)]), - cartesian_motion_generator_start_elbow_invalid( - errors_[static_cast(Error::kCartesianMotionGeneratorStartElbowInvalid)]), - cartesian_motion_generator_joint_position_limits_violation(errors_[static_cast( - Error::kCartesianMotionGeneratorJointPositionLimitsViolation)]), - cartesian_motion_generator_joint_velocity_limits_violation(errors_[static_cast( - Error::kCartesianMotionGeneratorJointVelocityLimitsViolation)]), - cartesian_motion_generator_joint_velocity_discontinuity( - errors_[static_cast(Error::kCartesianMotionGeneratorJointVelocityDiscontinuity)]), - cartesian_motion_generator_joint_acceleration_discontinuity(errors_[static_cast( - Error::kCartesianMotionGeneratorJointAccelerationDiscontinuity)]), - cartesian_position_motion_generator_invalid_frame( - errors_[static_cast(Error::kCartesianPositionMotionGeneratorInvalidFrame)]), - force_controller_desired_force_tolerance_violation( - errors_[static_cast(Error::kForceControllerDesiredForceToleranceViolation)]), - controller_torque_discontinuity( - errors_[static_cast(Error::kControllerTorqueDiscontinuity)]), - start_elbow_sign_inconsistent( - errors_[static_cast(Error::kStartElbowSignInconsistent)]), - communication_constraints_violation( - errors_[static_cast(Error::kCommunicationConstraintsViolation)]), - power_limit_violation(errors_[static_cast(Error::kPowerLimitViolation)]), - joint_p2p_insufficient_torque_for_planning( - errors_[static_cast(Error::kJointP2PInsufficientTorqueForPlanning)]), - 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_(errors) {} Errors::operator bool() const noexcept { return std::any_of(errors_.cbegin(), errors_.cend(), [](bool x) { return x; }); @@ -116,6 +39,155 @@ Errors::operator std::string() const { return error_string; } +const bool& Errors::joint_position_limits_violation() const { + return errors_[static_cast(Error::kJointPositionLimitsViolation)]; +} + +const bool& Errors::cartesian_position_limits_violation() const { + return errors_[static_cast(Error::kCartesianPositionLimitsViolation)]; +} + +const bool& Errors::self_collision_avoidance_violation() const { + return errors_[static_cast(Error::kSelfcollisionAvoidanceViolation)]; +} + +const bool& Errors::joint_velocity_violation() const { + return errors_[static_cast(Error::kJointVelocityViolation)]; +} + +const bool& Errors::cartesian_velocity_violation() const { + return errors_[static_cast(Error::kCartesianVelocityViolation)]; +} + +const bool& Errors::force_control_safety_violation() const { + return errors_[static_cast(Error::kForceControlSafetyViolation)]; +} + +const bool& Errors::joint_reflex() const { + return errors_[static_cast(Error::kJointReflex)]; +} + +const bool& Errors::cartesian_reflex() const { + return errors_[static_cast(Error::kCartesianReflex)]; +} + +const bool& Errors::max_goal_pose_deviation_violation() const { + return errors_[static_cast(Error::kMaxGoalPoseDeviationViolation)]; +} + +const bool& Errors::max_path_pose_deviation_violation() const { + return errors_[static_cast(Error::kMaxPathPoseDeviationViolation)]; +} + +const bool& Errors::cartesian_velocity_profile_safety_violation() const { + return errors_[static_cast(Error::kCartesianVelocityProfileSafetyViolation)]; +} + +const bool& Errors::joint_position_motion_generator_start_pose_invalid() const { + return errors_[static_cast(Error::kJointPositionMotionGeneratorStartPoseInvalid)]; +} + +const bool& Errors::joint_motion_generator_position_limits_violation() const { + return errors_[static_cast(Error::kJointMotionGeneratorPositionLimitsViolation)]; +} + +const bool& Errors::joint_motion_generator_velocity_limits_violation() const { + return errors_[static_cast(Error::kJointMotionGeneratorVelocityLimitsViolation)]; +} + +const bool& Errors::joint_motion_generator_velocity_discontinuity() const { + return errors_[static_cast(Error::kJointMotionGeneratorVelocityDiscontinuity)]; +} + +const bool& Errors::joint_motion_generator_acceleration_discontinuity() const { + return errors_[static_cast(Error::kJointMotionGeneratorAccelerationDiscontinuity)]; +} + +const bool& Errors::cartesian_position_motion_generator_start_pose_invalid() const { + return errors_[static_cast(Error::kCartesianPositionMotionGeneratorStartPoseInvalid)]; +} + +const bool& Errors::cartesian_motion_generator_elbow_limit_violation() const { + return errors_[static_cast(Error::kCartesianMotionGeneratorElbowLimitViolation)]; +} + +const bool& Errors::cartesian_motion_generator_velocity_limits_violation() const { + return errors_[static_cast(Error::kCartesianMotionGeneratorVelocityLimitsViolation)]; +} + +const bool& Errors::cartesian_motion_generator_velocity_discontinuity() const { + return errors_[static_cast(Error::kCartesianMotionGeneratorVelocityDiscontinuity)]; +} + +const bool& Errors::cartesian_motion_generator_acceleration_discontinuity() const { + return errors_[static_cast(Error::kCartesianMotionGeneratorAccelerationDiscontinuity)]; +} + +const bool& Errors::cartesian_motion_generator_elbow_sign_inconsistent() const { + return errors_[static_cast(Error::kCartesianMotionGeneratorElbowSignInconsistent)]; +} + +const bool& Errors::cartesian_motion_generator_start_elbow_invalid() const { + return errors_[static_cast(Error::kCartesianMotionGeneratorStartElbowInvalid)]; +} + +const bool& Errors::cartesian_motion_generator_joint_position_limits_violation() const { + return errors_[static_cast(Error::kCartesianMotionGeneratorJointPositionLimitsViolation)]; +} + +const bool& Errors::cartesian_motion_generator_joint_velocity_limits_violation() const { + return errors_[static_cast(Error::kCartesianMotionGeneratorJointVelocityLimitsViolation)]; +} + +const bool& Errors::cartesian_motion_generator_joint_velocity_discontinuity() const { + return errors_[static_cast(Error::kCartesianMotionGeneratorJointVelocityDiscontinuity)]; +} + +const bool& Errors::cartesian_motion_generator_joint_acceleration_discontinuity() const { + return errors_[static_cast( + Error::kCartesianMotionGeneratorJointAccelerationDiscontinuity)]; +} + +const bool& Errors::cartesian_position_motion_generator_invalid_frame() const { + return errors_[static_cast(Error::kCartesianPositionMotionGeneratorInvalidFrame)]; +} + +const bool& Errors::force_controller_desired_force_tolerance_violation() const { + return errors_[static_cast(Error::kForceControllerDesiredForceToleranceViolation)]; +} + +const bool& Errors::controller_torque_discontinuity() const { + return errors_[static_cast(Error::kControllerTorqueDiscontinuity)]; +} + +const bool& Errors::start_elbow_sign_inconsistent() const { + return errors_[static_cast(Error::kStartElbowSignInconsistent)]; +} + +const bool& Errors::communication_constraints_violation() const { + return errors_[static_cast(Error::kCommunicationConstraintsViolation)]; +} + +const bool& Errors::power_limit_violation() const { + return errors_[static_cast(Error::kPowerLimitViolation)]; +} + +const bool& Errors::joint_p2p_insufficient_torque_for_planning() const { + return errors_[static_cast(Error::kJointP2PInsufficientTorqueForPlanning)]; +} + +const bool& Errors::tau_j_range_violation() const { + return errors_[static_cast(Error::kTauJRangeViolation)]; +} + +const bool& Errors::instability_detected() const { + return errors_[static_cast(Error::kInstabilityDetection)]; +} + +const bool& Errors::joint_move_in_wrong_direction() const { + return errors_[static_cast(Error::kJointMoveInWrongDirection)]; +} + std::ostream& operator<<(std::ostream& ostream, const Errors& errors) { ostream << static_cast(errors); return ostream; diff --git a/test/helpers.cpp b/test/helpers.cpp index 18febd53..87b48a2d 100644 --- a/test/helpers.cpp +++ b/test/helpers.cpp @@ -635,61 +635,6 @@ bool operator==(const Move::Deviation& left, const Move::Deviation& right) { namespace franka { bool operator==(const Errors& lhs, const Errors& rhs) { - return lhs.joint_position_limits_violation == rhs.joint_position_limits_violation && - lhs.cartesian_position_limits_violation == rhs.cartesian_position_limits_violation && - lhs.self_collision_avoidance_violation == rhs.self_collision_avoidance_violation && - lhs.joint_velocity_violation == rhs.joint_velocity_violation && - lhs.cartesian_velocity_violation == rhs.cartesian_velocity_violation && - lhs.force_control_safety_violation == rhs.force_control_safety_violation && - lhs.joint_reflex == rhs.joint_reflex && lhs.cartesian_reflex == rhs.cartesian_reflex && - lhs.max_goal_pose_deviation_violation == rhs.max_goal_pose_deviation_violation && - lhs.max_path_pose_deviation_violation == rhs.max_path_pose_deviation_violation && - lhs.cartesian_velocity_profile_safety_violation == - rhs.cartesian_velocity_profile_safety_violation && - lhs.joint_position_motion_generator_start_pose_invalid == - rhs.joint_position_motion_generator_start_pose_invalid && - lhs.joint_motion_generator_position_limits_violation == - rhs.joint_motion_generator_position_limits_violation && - lhs.joint_motion_generator_velocity_limits_violation == - rhs.joint_motion_generator_velocity_limits_violation && - lhs.joint_motion_generator_velocity_discontinuity == - rhs.joint_motion_generator_velocity_discontinuity && - lhs.joint_motion_generator_acceleration_discontinuity == - rhs.joint_motion_generator_acceleration_discontinuity && - lhs.cartesian_position_motion_generator_start_pose_invalid == - rhs.cartesian_position_motion_generator_start_pose_invalid && - lhs.cartesian_motion_generator_elbow_limit_violation == - rhs.cartesian_motion_generator_elbow_limit_violation && - lhs.cartesian_motion_generator_velocity_limits_violation == - rhs.cartesian_motion_generator_velocity_limits_violation && - lhs.cartesian_motion_generator_velocity_discontinuity == - rhs.cartesian_motion_generator_velocity_discontinuity && - lhs.cartesian_motion_generator_acceleration_discontinuity == - rhs.cartesian_motion_generator_acceleration_discontinuity && - lhs.cartesian_motion_generator_elbow_sign_inconsistent == - rhs.cartesian_motion_generator_elbow_sign_inconsistent && - lhs.cartesian_motion_generator_start_elbow_invalid == - rhs.cartesian_motion_generator_start_elbow_invalid && - lhs.cartesian_motion_generator_joint_position_limits_violation == - rhs.cartesian_motion_generator_joint_position_limits_violation && - lhs.cartesian_motion_generator_joint_velocity_limits_violation == - rhs.cartesian_motion_generator_joint_velocity_limits_violation && - lhs.cartesian_motion_generator_joint_velocity_discontinuity == - rhs.cartesian_motion_generator_joint_velocity_discontinuity && - lhs.cartesian_motion_generator_joint_acceleration_discontinuity == - rhs.cartesian_motion_generator_joint_acceleration_discontinuity && - lhs.cartesian_position_motion_generator_invalid_frame == - rhs.cartesian_position_motion_generator_invalid_frame && - lhs.force_controller_desired_force_tolerance_violation == - rhs.force_controller_desired_force_tolerance_violation && - lhs.controller_torque_discontinuity == rhs.controller_torque_discontinuity && - lhs.start_elbow_sign_inconsistent == rhs.start_elbow_sign_inconsistent && - lhs.communication_constraints_violation == rhs.communication_constraints_violation && - lhs.power_limit_violation == rhs.power_limit_violation && - lhs.joint_p2p_insufficient_torque_for_planning == - 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; + return lhs == rhs; } } // namespace franka