From 04f430020902af32b5ba76f436a16149e68f22dd Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Mon, 18 Jul 2022 13:49:11 +0200 Subject: [PATCH 01/22] FIX: Move position & velocity controllers into `Joint` class --- .../include/franka_gazebo/franka_hw_sim.h | 4 ---- franka_gazebo/include/franka_gazebo/joint.h | 9 ++++++++ franka_gazebo/src/franka_hw_sim.cpp | 23 +++++++------------ 3 files changed, 17 insertions(+), 19 deletions(-) diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index c2a85b35..69117d2d 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include @@ -125,9 +124,6 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { gazebo::physics::ModelPtr robot_; std::map> joints_; - std::map position_pid_controllers_; - std::map velocity_pid_controllers_; - hardware_interface::JointStateInterface jsi_; hardware_interface::EffortJointInterface eji_; hardware_interface::PositionJointInterface pji_; diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h index 3770f537..81cfd9f8 100644 --- a/franka_gazebo/include/franka_gazebo/joint.h +++ b/franka_gazebo/include/franka_gazebo/joint.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -112,6 +113,14 @@ struct Joint { */ bool isInCollision() const; + /// The PID gains used for the controller, when in "position" control mode. In other modes these + /// gains are ignored. + control_toolbox::Pid position_controller; + + /// The PID gains used for the controller, when in "velocity" control mode. In other modes these + /// gains are ignored. + control_toolbox::Pid velocity_controller; + private: double lastVelocity = std::numeric_limits::quiet_NaN(); double lastAcceleration = std::numeric_limits::quiet_NaN(); diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 33b0ae71..e7c94aca 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -134,20 +134,15 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, } if (k_interface == "hardware_interface/PositionJointInterface") { // Initiate position motion generator (PID controller) - control_toolbox::Pid pid; - pid.initParam(robot_namespace + "/motion_generators/position/gains/" + joint->name); - this->position_pid_controllers_.emplace(joint->name, pid); - + joint->position_controller.initParam(robot_namespace + + "/motion_generators/position/gains/" + joint->name); initPositionCommandHandle(joint); continue; } if (k_interface == "hardware_interface/VelocityJointInterface") { // Initiate velocity motion generator (PID controller) - control_toolbox::Pid pid_velocity; - pid_velocity.initParam(robot_namespace + "/motion_generators/velocity/gains/" + - joint->name); - this->velocity_pid_controllers_.emplace(joint->name, pid_velocity); - + joint->velocity_controller.initParam(robot_namespace + + "/motion_generators/velocity/gains/" + joint->name); initVelocityCommandHandle(joint); continue; } @@ -403,17 +398,15 @@ void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration period) { } const double kEffortLimit = joint->limits.max_effort; - effort = boost::algorithm::clamp( - position_pid_controllers_[joint->name].computeCommand(error, period), - -kEffortLimit, kEffortLimit) + + effort = boost::algorithm::clamp(joint->position_controller.computeCommand(error, period), + -kEffortLimit, kEffortLimit) + joint->gravity; } else if (control_method == VELOCITY) { // Use velocity motion generator const double kError = joint->desired_velocity - joint->velocity; const double kEffortLimit = joint->limits.max_effort; - effort = boost::algorithm::clamp( - velocity_pid_controllers_[joint->name].computeCommand(kError, period), - -kEffortLimit, kEffortLimit) + + effort = boost::algorithm::clamp(joint->velocity_controller.computeCommand(kError, period), + -kEffortLimit, kEffortLimit) + joint->gravity; } From c76c2a31166efcf97f401bb6e83e610f85d450ac Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Mon, 18 Jul 2022 15:11:00 +0200 Subject: [PATCH 02/22] CHANGE: Initialize `robot_mode` to `kIdle` in `franka_gazebo` --- franka_gazebo/src/franka_hw_sim.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index e7c94aca..172490ab 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -232,6 +232,9 @@ void FrankaHWSim::initFrankaStateHandle( " joints were found beneath the tag, but 7 are required."); } + // Initialize robot_mode to "Idle". Once a controller is started, we will switch to "Move" + this->robot_state_.robot_mode = franka::RobotMode::kIdle; + // Check if all joints defined in the actually exist in the URDF for (const auto& joint : transmission.joints_) { if (not urdf.getJoint(joint.name_)) { From 4ace45670ba8f686e7c4208936b91f17ebb7d8ea Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Mon, 18 Jul 2022 15:13:25 +0200 Subject: [PATCH 03/22] ADD: Control state machine to franka_gazebo --- .ci/Dockerfile.melodic | 1 + .ci/Dockerfile.noetic | 1 + franka_gazebo/CMakeLists.txt | 2 + .../include/franka_gazebo/franka_hw_sim.h | 9 ++++ .../include/franka_gazebo/statemachine.h | 51 +++++++++++++++++++ franka_gazebo/package.xml | 1 + franka_gazebo/src/franka_hw_sim.cpp | 4 ++ 7 files changed, 69 insertions(+) create mode 100644 franka_gazebo/include/franka_gazebo/statemachine.h diff --git a/.ci/Dockerfile.melodic b/.ci/Dockerfile.melodic index a6e4fc47..4ade8398 100644 --- a/.ci/Dockerfile.melodic +++ b/.ci/Dockerfile.melodic @@ -7,6 +7,7 @@ RUN apt-get update -y && apt-get install -y \ pycodestyle \ python3-catkin-tools \ ros-melodic-libfranka \ + ros-melodic-boost-sml \ ros-melodic-ros-control \ ros-melodic-eigen-conversions \ ros-melodic-gazebo-dev \ diff --git a/.ci/Dockerfile.noetic b/.ci/Dockerfile.noetic index f5763444..7e2534d0 100644 --- a/.ci/Dockerfile.noetic +++ b/.ci/Dockerfile.noetic @@ -8,6 +8,7 @@ RUN apt-get update -y && apt-get install -y \ liborocos-kdl-dev \ python3-catkin-tools \ ros-noetic-libfranka \ + ros-noetic-boost-sml \ ros-noetic-ros-control \ ros-noetic-eigen-conversions \ ros-noetic-gazebo-dev \ diff --git a/franka_gazebo/CMakeLists.txt b/franka_gazebo/CMakeLists.txt index c58ab1af..3eda3df8 100644 --- a/franka_gazebo/CMakeLists.txt +++ b/franka_gazebo/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs gazebo_ros_control kdl_parser + boost_sml control_toolbox controller_manager controller_interface @@ -49,6 +50,7 @@ catkin_package( std_msgs gazebo_ros_control kdl_parser + boost_sml controller_manager controller_interface control_toolbox diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index 69117d2d..76d3130d 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -14,11 +15,13 @@ #include #include #include +#include #include #include #include #include #include +#include namespace franka_gazebo { @@ -44,6 +47,11 @@ const double kDefaultTauExtLowpassFilter = 1.0; // no filtering per default of */ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { public: + /** + * Create a new FrankaHWSim instance + */ + FrankaHWSim(); + /** * Initialize the simulated robot hardware and parse all supported transmissions. * @@ -131,6 +139,7 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { franka_hw::FrankaStateInterface fsi_; franka_hw::FrankaModelInterface fmi_; + boost::sml::sm> sm_; franka::RobotState robot_state_; std::unique_ptr model_; diff --git a/franka_gazebo/include/franka_gazebo/statemachine.h b/franka_gazebo/include/franka_gazebo/statemachine.h new file mode 100644 index 00000000..07faf4d8 --- /dev/null +++ b/franka_gazebo/include/franka_gazebo/statemachine.h @@ -0,0 +1,51 @@ +#pragma once + +#include +#include +#include + +namespace franka_gazebo { +// States +struct Idle {}; +struct Move {}; +struct UserStopped {}; + +// Events +struct ErrorRecovery {}; +struct UserStop { + bool pressed; +}; +struct StartControl {}; +struct StopControl {}; + +// Guards +auto isPressed = [](const UserStop& event) { return event.pressed; }; +auto isReleased = [](const UserStop& event) { return not event.pressed; }; + +// Actions +auto start = [](franka::RobotState& state) { state.robot_mode = franka::RobotMode::kMove; }; +auto idle = [](franka::RobotState& state) { state.robot_mode = franka::RobotMode::kIdle; }; +auto stop = [](franka::RobotState& state) { + ROS_WARN("E-Stop pressed, stopping robot"); + state.robot_mode = franka::RobotMode::kUserStopped; + state.q_d = state.q; + state.dq_d = {0}; + state.ddq_d = {0}; +}; + +struct StateMachine { + auto operator()() const { + using namespace boost::sml; + return make_transition_table( + // clang-format off + *state + event / start = state, + state + event[isPressed] / stop = state, + state + event / start = state, + state + event / idle = state, + state + event[isPressed] / stop = state, + state + event[isReleased] / idle = state + // clang-format on + ); + } +}; +} // namespace franka_gazebo diff --git a/franka_gazebo/package.xml b/franka_gazebo/package.xml index 01e230d9..0b5b3580 100644 --- a/franka_gazebo/package.xml +++ b/franka_gazebo/package.xml @@ -20,6 +20,7 @@ gazebo_ros_control roscpp std_msgs + boost_sml control_msgs control_toolbox controller_manager diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 172490ab..a5910174 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -21,6 +21,10 @@ namespace franka_gazebo { +using boost::sml::state; + +FrankaHWSim::FrankaHWSim() : sm_(this->robot_state_) {} + bool FrankaHWSim::initSim(const std::string& robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent, From 89d25713b741631ca9f4f417585e7835d160cf4a Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Tue, 19 Jul 2022 10:37:44 +0200 Subject: [PATCH 04/22] CHANGE: Properly distinguish between Position & Internal control When you stop any claiming controller (e.g. joint position controller) then FrankaHWSim will internally still use `ControlMethod::POSITION` as "internal" control to keep the joint holding position. It will still be possible to send `desired_position` commands via a JointPositionInterface. The solution to this is to distinguish between `control_method == POSITION` and `control_method == boost::none` inside of `FrankaHWSim::writeSim()`. In the later case we just use a saved `stop_position` as setpoint of `desired_position` --- .../franka_gazebo/controller_verifier.h | 2 +- franka_gazebo/include/franka_gazebo/joint.h | 7 ++++-- franka_gazebo/src/franka_hw_sim.cpp | 22 +++++++++---------- 3 files changed, 17 insertions(+), 14 deletions(-) diff --git a/franka_gazebo/include/franka_gazebo/controller_verifier.h b/franka_gazebo/include/franka_gazebo/controller_verifier.h index c24949e6..8a34ed5b 100644 --- a/franka_gazebo/include/franka_gazebo/controller_verifier.h +++ b/franka_gazebo/include/franka_gazebo/controller_verifier.h @@ -43,4 +43,4 @@ class ControllerVerifier { static bool hasControlMethodAndValidSize(const hardware_interface::InterfaceResources& resource); }; -} // namespace franka_gazebo \ No newline at end of file +} // namespace franka_gazebo diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h index 81cfd9f8..e1f19127 100644 --- a/franka_gazebo/include/franka_gazebo/joint.h +++ b/franka_gazebo/include/franka_gazebo/joint.h @@ -5,6 +5,7 @@ #include #include #include +#include #include namespace franka_gazebo { @@ -65,8 +66,8 @@ struct Joint { double desired_velocity = 0; /// Decides whether the joint is doing torque control or if the position or velocity should - /// be controlled. - ControlMethod control_method = POSITION; + /// be controlled, or if the joint is entirely uncontrolled + boost::optional control_method = boost::none; /// The currently acting gravity force or torque acting on this joint in \f$N\f$ or \f$Nm\f$ double gravity = 0; @@ -95,6 +96,8 @@ struct Joint { /// isInCollision double collision_threshold = std::numeric_limits::infinity(); + /// Position used as desired position if `control_method` is none + double stop_position = 0; /** * Get the total link mass of this joint's child * @return the mass in \f$kg\f$ diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index a5910174..390d54e6 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -45,7 +45,6 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, this->robot_initialized_pub_ = model_nh.advertise("initialized", 1); std_msgs::Bool msg; msg.data = static_cast(false); - ; this->robot_initialized_pub_.publish(msg); #if GAZEBO_MAJOR_VERSION >= 8 @@ -384,18 +383,16 @@ void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration period) { int i = std::stoi(pair.first.substr(prefix.size())) - 1; joint->gravity = g.at(i); } - auto& control_method = joint->control_method; - if (control_method == EFFORT) { - effort = joint->command + joint->gravity; - } else if (control_method == POSITION) { + if (not joint->control_method or joint->control_method == POSITION) { // Use position motion generator double error; + double setpoint = joint->control_method ? joint->desired_position : joint->stop_position; const double kJointLowerLimit = joint->limits.min_position; const double kJointUpperLimit = joint->limits.max_position; switch (joint->type) { case urdf::Joint::REVOLUTE: - angles::shortest_angular_distance_with_limits(joint->position, joint->desired_position, - kJointLowerLimit, kJointUpperLimit, error); + angles::shortest_angular_distance_with_limits(joint->position, setpoint, kJointLowerLimit, + kJointUpperLimit, error); break; default: std::string error_message = @@ -408,13 +405,15 @@ void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration period) { effort = boost::algorithm::clamp(joint->position_controller.computeCommand(error, period), -kEffortLimit, kEffortLimit) + joint->gravity; - } else if (control_method == VELOCITY) { + } else if (joint->control_method == VELOCITY) { // Use velocity motion generator const double kError = joint->desired_velocity - joint->velocity; const double kEffortLimit = joint->limits.max_effort; effort = boost::algorithm::clamp(joint->velocity_controller.computeCommand(kError, period), -kEffortLimit, kEffortLimit) + joint->gravity; + } else if (joint->control_method == EFFORT) { + effort = joint->command + joint->gravity; } // Send control effort control command @@ -588,7 +587,7 @@ void FrankaHWSim::updateRobotState(ros::Time time) { // first time initialization of the desired position if (not this->robot_initialized_) { - joint->desired_position = joint->position; + joint->stop_position = joint->position; } if (this->robot_initialized_) { @@ -653,8 +652,9 @@ bool FrankaHWSim::prepareSwitch( void FrankaHWSim::doSwitch(const std::list& start_list, const std::list& stop_list) { - forControlledJoint(stop_list, [](franka_gazebo::Joint& joint, const ControlMethod& method) { - joint.control_method = POSITION; + forControlledJoint(stop_list, [](franka_gazebo::Joint& joint, const ControlMethod& /*method*/) { + joint.control_method = boost::none; + joint.stop_position = joint.position; joint.desired_position = joint.position; joint.desired_velocity = 0; }); From 32c2df3caa33e4034a612c44b9204c9a779e6109 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Tue, 19 Jul 2022 10:41:15 +0200 Subject: [PATCH 05/22] ADD: Allow position control for prismatic joints --- franka_gazebo/src/franka_hw_sim.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 390d54e6..eb2b4320 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -394,9 +394,13 @@ void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration period) { angles::shortest_angular_distance_with_limits(joint->position, setpoint, kJointLowerLimit, kJointUpperLimit, error); break; + case urdf::Joint::PRISMATIC: + error = boost::algorithm::clamp(setpoint - joint->position, kJointLowerLimit, + kJointUpperLimit); + break; default: std::string error_message = - "Only revolute joints are allowed for position control right now"; + "Only revolute or prismatic joints are allowed for position control right now"; ROS_FATAL("%s", error_message.c_str()); throw std::invalid_argument(error_message); } From 4177b5ff9f0f088d7c25addd42ee81df275e092d Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Tue, 19 Jul 2022 10:42:26 +0200 Subject: [PATCH 06/22] ADD: FrankaHWSim offers `set_user_stop` service --- .../include/franka_gazebo/franka_hw_sim.h | 1 + franka_gazebo/src/franka_hw_sim.cpp | 27 +++++++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index 76d3130d..bf6100ee 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -150,6 +150,7 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { ros::ServiceServer service_set_k_; ros::ServiceServer service_set_load_; ros::ServiceServer service_collision_behavior_; + ros::ServiceServer service_user_stop_; std::vector lower_force_thresholds_nominal_; std::vector upper_force_thresholds_nominal_; diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index eb2b4320..341c6c9a 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -359,6 +360,13 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) { response.success = true; return true; }); + this->service_user_stop_ = + nh.advertiseService( + "set_user_stop", [&](auto& request, auto& response) { + this->sm_.process_event(UserStop{request.data}); + response.success = true; + return true; + }); } void FrankaHWSim::readSim(ros::Time time, ros::Duration period) { @@ -668,6 +676,25 @@ void FrankaHWSim::doSwitch(const std::list& joint.desired_position = joint.position; joint.desired_velocity = 0; }); + + auto contains = [&](const auto& haystack, const auto& needle) { + return haystack.find(needle) != std::string::npos; + }; + + bool is_any_arm_joint_controlled = false; + for (auto& joint : joints_) { + if (contains(joint.first, "_finger_joint")) { + continue; + } + is_any_arm_joint_controlled |= joint.second->control_method != boost::none; + } + if (is_any_arm_joint_controlled) { + ROS_INFO("Starting control"); + this->sm_.process_event(StartControl()); + } else { + ROS_INFO("Stopping control"); + this->sm_.process_event(StopControl()); + } } void FrankaHWSim::forControlledJoint( From fd3d0e83360756d05cb76969f7cc36bdb5a3c578 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Thu, 21 Jul 2022 14:26:41 +0200 Subject: [PATCH 07/22] CHANGE: Set `control_method` in statemachine actions --- .../include/franka_gazebo/statemachine.h | 51 +++++++++++++++---- franka_gazebo/src/franka_hw_sim.cpp | 21 +------- 2 files changed, 43 insertions(+), 29 deletions(-) diff --git a/franka_gazebo/include/franka_gazebo/statemachine.h b/franka_gazebo/include/franka_gazebo/statemachine.h index 07faf4d8..80c07076 100644 --- a/franka_gazebo/include/franka_gazebo/statemachine.h +++ b/franka_gazebo/include/franka_gazebo/statemachine.h @@ -1,10 +1,18 @@ #pragma once #include +#include + #include #include +#include +#include +#include namespace franka_gazebo { + +using JointMap = std::map>; + // States struct Idle {}; struct Move {}; @@ -12,25 +20,48 @@ struct UserStopped {}; // Events struct ErrorRecovery {}; +struct SwitchControl {}; struct UserStop { bool pressed; }; -struct StartControl {}; -struct StopControl {}; // Guards auto isPressed = [](const UserStop& event) { return event.pressed; }; -auto isReleased = [](const UserStop& event) { return not event.pressed; }; +auto isReleased = [](const UserStop& event, const JointMap& joints) { return not event.pressed; }; +auto isStarting = [](const SwitchControl& event, const JointMap& joints) { + auto contains = [](const auto& haystack, const auto& needle) { + return haystack.find(needle) != std::string::npos; + }; + for (auto& joint : joints) { + if (contains(joint.first, "_finger_joint")) { + continue; + } + if (joint.second->control_method) { + return true; + } + } + return false; +}; +auto isStopping = [](const SwitchControl& event, const JointMap& joints) { + return not isStarting(event, joints); +}; // Actions auto start = [](franka::RobotState& state) { state.robot_mode = franka::RobotMode::kMove; }; auto idle = [](franka::RobotState& state) { state.robot_mode = franka::RobotMode::kIdle; }; -auto stop = [](franka::RobotState& state) { +auto stop = [](franka::RobotState& state, JointMap& joints) { ROS_WARN("E-Stop pressed, stopping robot"); state.robot_mode = franka::RobotMode::kUserStopped; state.q_d = state.q; state.dq_d = {0}; state.ddq_d = {0}; + + for (auto& joint : joints) { + joint.second->control_method = boost::none; + joint.second->stop_position = joint.second->position; + joint.second->desired_position = joint.second->position; + joint.second->desired_velocity = 0; + } }; struct StateMachine { @@ -38,12 +69,12 @@ struct StateMachine { using namespace boost::sml; return make_transition_table( // clang-format off - *state + event / start = state, - state + event[isPressed] / stop = state, - state + event / start = state, - state + event / idle = state, - state + event[isPressed] / stop = state, - state + event[isReleased] / idle = state + *state + event[isStarting] / start = state, + state + event[isPressed] / stop = state, + state + event / start = state, + state + event[isStopping] / idle = state, + state + event[isPressed] / stop = state, + state + event[isReleased] / idle = state // clang-format on ); } diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 341c6c9a..cddee8c1 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -24,7 +24,7 @@ namespace franka_gazebo { using boost::sml::state; -FrankaHWSim::FrankaHWSim() : sm_(this->robot_state_) {} +FrankaHWSim::FrankaHWSim() : sm_(this->robot_state_, this->joints_) {} bool FrankaHWSim::initSim(const std::string& robot_namespace, ros::NodeHandle model_nh, @@ -677,24 +677,7 @@ void FrankaHWSim::doSwitch(const std::list& joint.desired_velocity = 0; }); - auto contains = [&](const auto& haystack, const auto& needle) { - return haystack.find(needle) != std::string::npos; - }; - - bool is_any_arm_joint_controlled = false; - for (auto& joint : joints_) { - if (contains(joint.first, "_finger_joint")) { - continue; - } - is_any_arm_joint_controlled |= joint.second->control_method != boost::none; - } - if (is_any_arm_joint_controlled) { - ROS_INFO("Starting control"); - this->sm_.process_event(StartControl()); - } else { - ROS_INFO("Stopping control"); - this->sm_.process_event(StopControl()); - } + this->sm_.process_event(SwitchControl()); } void FrankaHWSim::forControlledJoint( From 51c4c7e5dfcf82905bbf707d1417de6f99436df2 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Thu, 21 Jul 2022 14:27:18 +0200 Subject: [PATCH 08/22] CHANGE: Extract motion generators into separate functions --- .../include/franka_gazebo/franka_hw_sim.h | 3 + franka_gazebo/src/franka_hw_sim.cpp | 80 ++++++++++--------- 2 files changed, 47 insertions(+), 36 deletions(-) diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index bf6100ee..0397afd4 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -175,6 +175,9 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { void guessEndEffector(const ros::NodeHandle& nh, const urdf::Model& urdf); + static double positionControl(Joint& joint, double setpoint, ros::Duration period); + static double velocityControl(Joint& joint, double setpoint, ros::Duration period); + template std::array readArray(std::string param, std::string name = "") { std::array x; diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index cddee8c1..ec2ea4ad 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -377,6 +377,36 @@ void FrankaHWSim::readSim(ros::Time time, ros::Duration period) { this->updateRobotState(time); } +double FrankaHWSim::positionControl(Joint& joint, double setpoint, ros::Duration period) { + double error; + const double kJointLowerLimit = joint.limits.min_position; + const double kJointUpperLimit = joint.limits.max_position; + switch (joint.type) { + case urdf::Joint::REVOLUTE: + angles::shortest_angular_distance_with_limits(joint.position, setpoint, kJointLowerLimit, + kJointUpperLimit, error); + break; + case urdf::Joint::PRISMATIC: + error = + boost::algorithm::clamp(setpoint - joint.position, kJointLowerLimit, kJointUpperLimit); + break; + default: + std::string error_message = + "Only revolute or prismatic joints are allowed for position control right now"; + ROS_FATAL("%s", error_message.c_str()); + throw std::invalid_argument(error_message); + } + + return boost::algorithm::clamp(joint.position_controller.computeCommand(error, period), + -joint.limits.max_effort, joint.limits.max_effort); +} + +double FrankaHWSim::velocityControl(Joint& joint, double setpoint, ros::Duration period) { + return boost::algorithm::clamp( + joint.velocity_controller.computeCommand(setpoint - joint.velocity, period), + -joint.limits.max_effort, joint.limits.max_effort); +} + void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration period) { auto g = this->model_->gravity(this->robot_state_, this->gravity_earth_); @@ -385,48 +415,25 @@ void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration period) { // Retrieve effort control command double effort = 0; + + if (not sm_.is(state)) { + effort = positionControl(*joint, joint->stop_position, period); + } else if (joint->control_method == POSITION) { + effort = positionControl(*joint, joint->desired_position, period); + } else if (joint->control_method == VELOCITY) { + velocityControl(*joint, joint->desired_velocity, period); + } else if (joint->control_method == EFFORT) { + // Feed-forward commands in effort control + effort = joint->command; + } + // Check if this joint is affected by gravity compensation std::string prefix = this->arm_id_ + "_joint"; if (pair.first.rfind(prefix, 0) != std::string::npos) { int i = std::stoi(pair.first.substr(prefix.size())) - 1; joint->gravity = g.at(i); } - if (not joint->control_method or joint->control_method == POSITION) { - // Use position motion generator - double error; - double setpoint = joint->control_method ? joint->desired_position : joint->stop_position; - const double kJointLowerLimit = joint->limits.min_position; - const double kJointUpperLimit = joint->limits.max_position; - switch (joint->type) { - case urdf::Joint::REVOLUTE: - angles::shortest_angular_distance_with_limits(joint->position, setpoint, kJointLowerLimit, - kJointUpperLimit, error); - break; - case urdf::Joint::PRISMATIC: - error = boost::algorithm::clamp(setpoint - joint->position, kJointLowerLimit, - kJointUpperLimit); - break; - default: - std::string error_message = - "Only revolute or prismatic joints are allowed for position control right now"; - ROS_FATAL("%s", error_message.c_str()); - throw std::invalid_argument(error_message); - } - - const double kEffortLimit = joint->limits.max_effort; - effort = boost::algorithm::clamp(joint->position_controller.computeCommand(error, period), - -kEffortLimit, kEffortLimit) + - joint->gravity; - } else if (joint->control_method == VELOCITY) { - // Use velocity motion generator - const double kError = joint->desired_velocity - joint->velocity; - const double kEffortLimit = joint->limits.max_effort; - effort = boost::algorithm::clamp(joint->velocity_controller.computeCommand(kError, period), - -kEffortLimit, kEffortLimit) + - joint->gravity; - } else if (joint->control_method == EFFORT) { - effort = joint->command + joint->gravity; - } + effort += joint->gravity; // Send control effort control command if (not std::isfinite(effort)) { @@ -599,6 +606,7 @@ void FrankaHWSim::updateRobotState(ros::Time time) { // first time initialization of the desired position if (not this->robot_initialized_) { + joint->desired_position = joint->position; joint->stop_position = joint->position; } From 0586707e5416525727004e26fd0991b02843c6ff Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Thu, 21 Jul 2022 14:51:22 +0200 Subject: [PATCH 09/22] ADD: `error_recovery` action to FrankaHWSim --- franka_gazebo/include/franka_gazebo/franka_hw_sim.h | 3 +++ franka_gazebo/src/franka_hw_sim.cpp | 11 +++++++++++ 2 files changed, 14 insertions(+) diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index 0397afd4..4bd88302 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -7,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -151,6 +153,7 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { ros::ServiceServer service_set_load_; ros::ServiceServer service_collision_behavior_; ros::ServiceServer service_user_stop_; + std::unique_ptr> action_recovery_; std::vector lower_force_thresholds_nominal_; std::vector upper_force_thresholds_nominal_; diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index ec2ea4ad..c853ead6 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -22,6 +22,7 @@ namespace franka_gazebo { +using actionlib::SimpleActionServer; using boost::sml::state; FrankaHWSim::FrankaHWSim() : sm_(this->robot_state_, this->joints_) {} @@ -48,6 +49,16 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, msg.data = static_cast(false); this->robot_initialized_pub_.publish(msg); + this->action_recovery_ = std::make_unique>( + model_nh, "franka_control/error_recovery", + [&](const franka_msgs::ErrorRecoveryGoalConstPtr& goal) { + ROS_INFO_NAMED("franka_hw_sim", "Recovered from error"); + this->sm_.process_event(ErrorRecovery()); + this->action_recovery_->setSucceeded(); + }, + false); + this->action_recovery_->start(); + #if GAZEBO_MAJOR_VERSION >= 8 gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics(); #else From 5508c425d2aa4ffa378fcd6aebcaca6cf0e21a5c Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Mon, 25 Jul 2022 09:17:56 +0200 Subject: [PATCH 10/22] FIX: Don't switch control method on user stop When the user stop is pressed, we don't want to "forget" in which control method each joint was, so that we can recover from the user stop error and continue the controller --- franka_gazebo/include/franka_gazebo/statemachine.h | 1 - 1 file changed, 1 deletion(-) diff --git a/franka_gazebo/include/franka_gazebo/statemachine.h b/franka_gazebo/include/franka_gazebo/statemachine.h index 80c07076..8a04248d 100644 --- a/franka_gazebo/include/franka_gazebo/statemachine.h +++ b/franka_gazebo/include/franka_gazebo/statemachine.h @@ -57,7 +57,6 @@ auto stop = [](franka::RobotState& state, JointMap& joints) { state.ddq_d = {0}; for (auto& joint : joints) { - joint.second->control_method = boost::none; joint.second->stop_position = joint.second->position; joint.second->desired_position = joint.second->position; joint.second->desired_velocity = 0; From 60294b65bf45d51c287e21e3c143dbf0a14481cd Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Mon, 25 Jul 2022 10:02:28 +0200 Subject: [PATCH 11/22] CHANGE: Extract `getDesiredXXX()` logic into Joint class --- franka_gazebo/include/franka_gazebo/joint.h | 43 +++++++++++++++++++++ franka_gazebo/src/franka_hw_sim.cpp | 18 +++------ franka_gazebo/src/joint.cpp | 41 ++++++++++++++++++++ 3 files changed, 89 insertions(+), 13 deletions(-) diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h index e1f19127..a6e9bdea 100644 --- a/franka_gazebo/include/franka_gazebo/joint.h +++ b/franka_gazebo/include/franka_gazebo/joint.h @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -98,6 +99,48 @@ struct Joint { /// Position used as desired position if `control_method` is none double stop_position = 0; + + /** + * Decide what the desired position of this joint is based on: + * 1. If a reflex is present, return `position` + * 2. ...otherwise if the control method is POSITION, return `desired_position` + * 3. ...otherwise if the control method is EFFORT return `desired_position` + * 3. ...otherwise return `position` + * @param[in] mode - the current mode the robot is in + * @return either `position` or `desired_position` + */ + double getDesiredPosition(const franka::RobotMode& mode) const; + + /** + * Decide what the desired velocity of this joint is based on: + * 1. If a reflex is present, return `velocity` + * 2. ...otherwise if the control method is not VELOCITY, return `velocity` + * 3. ...otherwise return `desired_velocity` + * @param[in] mode - the current mode the robot is in + * @return either `velocity` or `desired_velocity` + */ + double getDesiredVelocity(const franka::RobotMode& mode) const; + + /** + * Decide what the desired acceleration of this joint is based on: + * 1. If a reflex is present, return `acceleration` + * 2. ...otherwise if the control method is EFFORT, return 0 + * 3. ...otherwise return `acceleration` + * @param[in] mode - the current mode the robot is in + * @return either `acceleration` or `0` + */ + double getDesiredAcceleration(const franka::RobotMode& mode) const; + + /** + * Decide what the desired torque of this joint is: + * 1. If a reflex is present, return 0 + * 2. ... otherwise if the control method is not EFFORT return 0 + * 3. ... otherwise return `command` + * @param[in] mode - the current mode the robot is in + * @return either `command` or zero + */ + double getDesiredTorque(const franka::RobotMode& mode) const; + /** * Get the total link mass of this joint's child * @return the mass in \f$kg\f$ diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index c853ead6..30244727 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -589,6 +589,7 @@ void FrankaHWSim::updateRobotState(ros::Time time) { // This is ensured, because a FrankaStateInterface checks for at least seven joints in the URDF assert(this->joints_.size() >= 7); + auto mode = this->robot_state_.robot_mode; for (int i = 0; i < 7; i++) { std::string name = this->arm_id_ + "_joint" + std::to_string(i + 1); const auto& joint = this->joints_.at(name); @@ -597,19 +598,10 @@ void FrankaHWSim::updateRobotState(ros::Time time) { this->robot_state_.tau_J[i] = joint->effort; this->robot_state_.dtau_J[i] = joint->jerk; - if (joint->control_method == EFFORT) { - this->robot_state_.q_d[i] = joint->desired_position; - this->robot_state_.dq_d[i] = 0; - this->robot_state_.ddq_d[i] = 0; - this->robot_state_.tau_J_d[i] = joint->command; - } else { - this->robot_state_.q_d[i] = - joint->control_method == POSITION ? joint->desired_position : joint->position; - this->robot_state_.dq_d[i] = - joint->control_method == VELOCITY ? joint->desired_velocity : joint->velocity; - this->robot_state_.ddq_d[i] = joint->acceleration; - this->robot_state_.tau_J_d[i] = 0; - } + this->robot_state_.q_d[i] = joint->getDesiredPosition(mode); + this->robot_state_.dq_d[i] = joint->getDesiredVelocity(mode); + this->robot_state_.ddq_d[i] = joint->getDesiredAcceleration(mode); + this->robot_state_.tau_J_d[i] = joint->getDesiredTorque(mode); // For now we assume no flexible joints this->robot_state_.theta[i] = joint->position; diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp index d733539d..186f1192 100644 --- a/franka_gazebo/src/joint.cpp +++ b/franka_gazebo/src/joint.cpp @@ -52,6 +52,47 @@ void Joint::update(const ros::Duration& dt) { this->lastAcceleration = this->acceleration; } +double Joint::getDesiredPosition(const franka::RobotMode& mode) const { + if (mode != franka::RobotMode::kMove) { + return this->position; + } + if (this->control_method == ControlMethod::POSITION or + this->control_method != ControlMethod::EFFORT) { + return this->desired_position; + } + return this->position; +} + +double Joint::getDesiredVelocity(const franka::RobotMode& mode) const { + if (mode != franka::RobotMode::kMove) { + return this->velocity; + } + if (this->control_method != ControlMethod::VELOCITY) { + return this->velocity; + } + return this->desired_velocity; +} + +double Joint::getDesiredAcceleration(const franka::RobotMode& mode) const { + if (mode != franka::RobotMode::kMove) { + return this->acceleration; + } + if (this->control_method == ControlMethod::EFFORT) { + return 0; + } + return this->acceleration; +} + +double Joint::getDesiredTorque(const franka::RobotMode& mode) const { + if (mode != franka::RobotMode::kMove) { + return 0; + } + if (this->control_method != ControlMethod::EFFORT) { + return 0; + } + return command; +} + double Joint::getLinkMass() const { if (not this->handle) { return std::numeric_limits::quiet_NaN(); From f965948e9a0d6b55c24cdcf1ae74950efbe5d319 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Mon, 25 Jul 2022 13:26:02 +0200 Subject: [PATCH 12/22] CHANGE: Restart all running controllers on error_recovery This is similar to how `franka_control` does it. It it necessary since we (franka_control & franka_hw_sim) keep the controllers running even if the user stop is pressed in order for "read-only" controllers (such as FrankaStateController) to keep updating. However, when the user stop error was recovered (i.e. the controller's commands are fed through again) their will be a large jump in desired vs actual control signals. This is mitigated by restarting all controllers via the `switch_controller` service call of the controller manager. --- .../include/franka_gazebo/franka_hw_sim.h | 4 ++ franka_gazebo/src/franka_hw_sim.cpp | 47 +++++++++++++++++-- 2 files changed, 48 insertions(+), 3 deletions(-) diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index 4bd88302..d627b0f4 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -153,6 +153,8 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { ros::ServiceServer service_set_load_; ros::ServiceServer service_collision_behavior_; ros::ServiceServer service_user_stop_; + ros::ServiceClient service_controller_list_; + ros::ServiceClient service_controller_switch_; std::unique_ptr> action_recovery_; std::vector lower_force_thresholds_nominal_; @@ -171,6 +173,8 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { double singularity_threshold); void initServices(ros::NodeHandle& nh); + void restartControllers(); + void updateRobotState(ros::Time time); void updateRobotStateDynamics(); diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 30244727..4b965ade 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -1,4 +1,6 @@ #include +#include +#include #include #include #include @@ -52,9 +54,15 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, this->action_recovery_ = std::make_unique>( model_nh, "franka_control/error_recovery", [&](const franka_msgs::ErrorRecoveryGoalConstPtr& goal) { - ROS_INFO_NAMED("franka_hw_sim", "Recovered from error"); - this->sm_.process_event(ErrorRecovery()); - this->action_recovery_->setSucceeded(); + try { + restartControllers(); + ROS_INFO_NAMED("franka_hw_sim", "Recovered from error"); + this->sm_.process_event(ErrorRecovery()); + this->action_recovery_->setSucceeded(); + } catch (const std::runtime_error& e) { + ROS_WARN_STREAM_NAMED("franka_hw_sim", "Error recovery failed: " << e.what()); + this->action_recovery_->setAborted(); + } }, false); this->action_recovery_->start(); @@ -378,6 +386,39 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) { response.success = true; return true; }); + this->service_controller_list_ = nh.serviceClient( + "controller_manager/list_controllers"); + this->service_controller_switch_ = nh.serviceClient( + "controller_manager/switch_controller"); +} + +void FrankaHWSim::restartControllers() { + // Restart controllers by stopping and starting all running ones + auto name = this->service_controller_list_.getService(); + if (not this->service_controller_list_.waitForExistence(ros::Duration(3))) { + throw std::runtime_error("Cannot find service '" + name + + "'. Is the controller_manager running?"); + } + + controller_manager_msgs::ListControllers list; + if (not this->service_controller_list_.call(list)) { + throw std::runtime_error("Service call '" + name + "' failed"); + } + + controller_manager_msgs::SwitchController swtch; + for (const auto& controller : list.response.controller) { + if (controller.state != "running") { + continue; + } + swtch.request.stop_controllers.push_back(controller.name); + swtch.request.start_controllers.push_back(controller.name); + } + swtch.request.start_asap = true; + swtch.request.strictness = controller_manager_msgs::SwitchControllerRequest::STRICT; + if (not this->service_controller_switch_.call(swtch) or not swtch.response.ok) { + throw std::runtime_error("Service call '" + this->service_controller_switch_.getService() + + "' failed"); + } } void FrankaHWSim::readSim(ros::Time time, ros::Duration period) { From a8a77dd3613a2f3e4e35982ae3b3905789f84b9d Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Mon, 25 Jul 2022 13:33:00 +0200 Subject: [PATCH 13/22] ADD: Warning to FrankaHWSim::error_recovery action --- franka_gazebo/src/franka_hw_sim.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 4b965ade..3556f5e9 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -55,6 +55,9 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, model_nh, "franka_control/error_recovery", [&](const franka_msgs::ErrorRecoveryGoalConstPtr& goal) { try { + ROS_WARN_STREAM_COND_NAMED( + this->robot_state_.robot_mode == franka::RobotMode::kUserStopped, "franka_hw_sim", + "Cannot recover errors since the user stop seems still pressed"); restartControllers(); ROS_INFO_NAMED("franka_hw_sim", "Recovered from error"); this->sm_.process_event(ErrorRecovery()); From 8e8d970a3a53cb71945d8a083732366d5bfdb347 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Mon, 25 Jul 2022 16:25:16 +0200 Subject: [PATCH 14/22] ADD: Statemachine tests --- .../test/franka_hw_sim_gazebo_test.cpp | 257 +++++++++++++++++- 1 file changed, 254 insertions(+), 3 deletions(-) diff --git a/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp b/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp index e922653b..5ab21af3 100644 --- a/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp +++ b/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp @@ -1,14 +1,25 @@ +#include +#include +#include #include #include #include +#include +#include +#include +#include +#include +using namespace franka_gazebo; +namespace sml = boost::sml; TEST( #if ROS_VERSION_MINIMUM(1, 15, 13) - TestSuite, /* noetic & newer */ + FrankaHWSim, /* noetic & newer */ #else - DISABLED_TestSuite, /* melodic */ + DISABLED_FrankaHWSim, /* melodic */ #endif - DISABLED_franka_hw_sim_compensates_gravity_on_F_ext) { // TODO(goll_th) enable again when not flaky anymore + DISABLED_franka_hw_sim_compensates_gravity_on_F_ext) { // TODO(goll_th) enable again when not + // flaky anymore ros::NodeHandle n; for (int i = 0; i < 50; i++) { @@ -26,6 +37,246 @@ TEST( } } +class StateMachineFixture : public ::testing::Test { + protected: + franka::RobotState state; + std::map> joints; + std::unique_ptr> sm; + + virtual void SetUp() { + sm = std::make_unique>(state, joints); + for (int i = 1; i <= 7; i++) { + joints.emplace(armJoint(i), std::make_unique()); + } + joints.emplace("panda_finger_joint1", std::make_unique()); + joints.emplace("panda_finger_joint2", std::make_unique()); + } + + std::string armJoint(int i) { return "panda_joint" + std::to_string(i); } +}; + +TEST_F(StateMachineFixture, statemachine_initializes_in_Idle) { + ASSERT_TRUE(sm->is(sml::state)); +} + +TEST_F(StateMachineFixture, statemachine_switches_from_Idle_to_Move_on_error_recovery_event) { + sm->set_current_states(sml::state); + sm->process_event(ErrorRecovery()); + EXPECT_TRUE(sm->is(sml::state)); +} + +TEST_F(StateMachineFixture, + statemachine_switches_from_Idle_to_UserStopped_if_user_stop_is_pressed) { + sm->set_current_states(sml::state); + sm->process_event(UserStop{true}); + EXPECT_TRUE(sm->is(sml::state)); +} + +TEST_F(StateMachineFixture, statemachine_switches_from_Move_to_UserStopped_on_user_stop_event) { + sm->set_current_states(sml::state); + sm->process_event(UserStop{true}); + EXPECT_TRUE(sm->is(sml::state)); +} + +TEST_F(StateMachineFixture, statemachine_stays_in_Move_on_user_stop_event) { + sm->set_current_states(sml::state); + sm->process_event(UserStop{false}); + EXPECT_TRUE(sm->is(sml::state)); +} + +TEST_F(StateMachineFixture, statemachine_stays_in_Move_on_error_recovery_event) { + sm->set_current_states(sml::state); + sm->process_event(ErrorRecovery()); + EXPECT_TRUE(sm->is(sml::state)); +} + +TEST_F(StateMachineFixture, statemachine_stays_in_UserStopped_if_user_stop_still_pressed) { + sm->set_current_states(sml::state); + sm->process_event(UserStop{true}); + EXPECT_TRUE(sm->is(sml::state)); +} + +TEST_F(StateMachineFixture, + statemachine_switches_from_UserStopped_to_Idle_if_user_stop_is_released) { + sm->set_current_states(sml::state); + sm->process_event(UserStop{false}); + EXPECT_TRUE(sm->is(sml::state)); +} + +TEST_F(StateMachineFixture, statemachine_switches_from_Idle_to_Move_if_joints_are_controlled) { + // Arange + sm->set_current_states(sml::state); + + // Act + for (int i = 1; i <= 7; i++) { + joints.at(armJoint(i))->control_method = ControlMethod::POSITION; + } + sm->process_event(SwitchControl()); + + // Assert + EXPECT_TRUE(sm->is(sml::state)); +} + +TEST_F(StateMachineFixture, statemachine_switches_from_Move_to_Idle_if_joints_are_uncontrolled) { + // Arange + sm->set_current_states(sml::state); + + // Act + for (int i = 1; i <= 7; i++) { + joints.at(armJoint(i))->control_method = boost::none; + } + sm->process_event(SwitchControl()); + + // Assert + EXPECT_TRUE(sm->is(sml::state)); +} + +TEST_F(StateMachineFixture, statemachine_stays_in_Idle_if_only_finger_joints_are_controlled) { + // Arange + sm->set_current_states(sml::state); + + // Act + joints.at("panda_finger_joint1")->control_method = ControlMethod::POSITION; + joints.at("panda_finger_joint2")->control_method = ControlMethod::POSITION; + sm->process_event(SwitchControl()); + + // Assert + EXPECT_TRUE(sm->is(sml::state)); +} + +TEST_F(StateMachineFixture, statemachine_stays_in_Move_if_only_finger_joints_are_uncontrolled) { + // Arange + sm->set_current_states(sml::state); + for (int i = 1; i <= 7; i++) { + joints.at(armJoint(i))->control_method = ControlMethod::VELOCITY; + } + + // Act + joints.at("panda_finger_joint1")->control_method = boost::none; + joints.at("panda_finger_joint2")->control_method = boost::none; + sm->process_event(SwitchControl()); + + // Assert + EXPECT_TRUE(sm->is(sml::state)); +} + +TEST_F(StateMachineFixture, statemachine_updates_robot_mode_when_switching_from_Idle_to_Move) { + // Arange + sm->set_current_states(sml::state); + state.robot_mode = franka::RobotMode::kIdle; + + // Act + for (int i = 1; i <= 7; i++) { + joints.at(armJoint(i))->control_method = ControlMethod::VELOCITY; + } + sm->process_event(SwitchControl()); + + // Assert + ASSERT_EQ(state.robot_mode, franka::RobotMode::kMove); +} + +TEST_F(StateMachineFixture, statemachine_updates_robot_mode_when_switching_from_Move_to_Idle) { + // Arange + sm->set_current_states(sml::state); + state.robot_mode = franka::RobotMode::kMove; + + // Act + for (int i = 1; i <= 7; i++) { + joints.at(armJoint(i))->control_method = boost::none; + } + sm->process_event(SwitchControl()); + + // Assert + ASSERT_EQ(state.robot_mode, franka::RobotMode::kIdle); +} + +TEST_F(StateMachineFixture, + statemachine_updates_robot_mode_when_switching_from_Idle_to_UserStopped) { + sm->set_current_states(sml::state); + state.robot_mode = franka::RobotMode::kIdle; + + sm->process_event(UserStop{true}); + ASSERT_EQ(state.robot_mode, franka::RobotMode::kUserStopped); +} + +TEST_F(StateMachineFixture, + statemachine_updates_robot_mode_when_switching_from_Move_to_UserStopped) { + sm->set_current_states(sml::state); + state.robot_mode = franka::RobotMode::kMove; + + sm->process_event(UserStop{true}); + ASSERT_EQ(state.robot_mode, franka::RobotMode::kUserStopped); +} + +TEST_F(StateMachineFixture, + statemachine_updates_robot_mode_when_switching_from_UserStopped_to_Idle) { + sm->set_current_states(sml::state); + state.robot_mode = franka::RobotMode::kUserStopped; + + sm->process_event(UserStop{false}); + ASSERT_EQ(state.robot_mode, franka::RobotMode::kIdle); +} + +TEST_F(StateMachineFixture, + statemachine_resets_desired_control_signals_when_switching_from_Idle_to_UserStopped) { + // Arange + sm->set_current_states(sml::state); + for (int i = 1; i <= 7; i++) { + auto& joint = joints.at(armJoint(i)); + joint->position = 100 + i; + joint->desired_position = 42 + i; + joint->desired_velocity = 84 + i; + joint->stop_position = 0; + } + + // Act + sm->process_event(UserStop{true}); + + // Assert + for (int i = 1; i <= 7; i++) { + auto& joint = joints.at(armJoint(i)); + ASSERT_DOUBLE_EQ(joint->position, 100 + i) << "Joint position changed but expected it didn't"; + ASSERT_DOUBLE_EQ(joint->stop_position, joint->position); + ASSERT_DOUBLE_EQ(joint->desired_position, joint->position); + ASSERT_DOUBLE_EQ(joint->desired_velocity, 0); + } + + std::array zeros = {0}; + ASSERT_EQ(state.q_d, state.q); + ASSERT_EQ(state.dq_d, zeros); + ASSERT_EQ(state.ddq_d, zeros); +} + +TEST_F(StateMachineFixture, + statemachine_resets_desired_control_signals_when_switching_from_Move_to_UserStopped) { + // Arange + sm->set_current_states(sml::state); + for (int i = 1; i <= 7; i++) { + auto& joint = joints.at(armJoint(i)); + joint->position = 100 + i; + joint->desired_position = 42 + i; + joint->desired_velocity = 84 + i; + joint->stop_position = 0; + } + + // Act + sm->process_event(UserStop{true}); + + // Assert + for (int i = 1; i <= 7; i++) { + auto& joint = joints.at(armJoint(i)); + ASSERT_DOUBLE_EQ(joint->position, 100 + i) << "Joint position changed but expected it didn't"; + ASSERT_DOUBLE_EQ(joint->stop_position, joint->position); + ASSERT_DOUBLE_EQ(joint->desired_position, joint->position); + ASSERT_DOUBLE_EQ(joint->desired_velocity, 0); + } + + std::array zeros = {0}; + ASSERT_EQ(state.q_d, state.q); + ASSERT_EQ(state.dq_d, zeros); + ASSERT_EQ(state.ddq_d, zeros); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "franka_hw_sim_test"); From 0ffee47fc7d219bd23ec632b7a9828a9de297203 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Tue, 26 Jul 2022 10:21:48 +0200 Subject: [PATCH 15/22] FIX: Make linter happy --- franka_gazebo/include/franka_gazebo/franka_hw_sim.h | 4 ++-- franka_gazebo/src/franka_hw_sim.cpp | 11 ++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index d627b0f4..0d9dae5f 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -182,8 +182,8 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { void guessEndEffector(const ros::NodeHandle& nh, const urdf::Model& urdf); - static double positionControl(Joint& joint, double setpoint, ros::Duration period); - static double velocityControl(Joint& joint, double setpoint, ros::Duration period); + static double positionControl(Joint& joint, double setpoint, const ros::Duration& period); + static double velocityControl(Joint& joint, double setpoint, const ros::Duration& period); template std::array readArray(std::string param, std::string name = "") { diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 3556f5e9..b5094117 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -385,7 +385,7 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) { this->service_user_stop_ = nh.advertiseService( "set_user_stop", [&](auto& request, auto& response) { - this->sm_.process_event(UserStop{request.data}); + this->sm_.process_event(UserStop{static_cast(request.data)}); response.success = true; return true; }); @@ -416,9 +416,10 @@ void FrankaHWSim::restartControllers() { swtch.request.stop_controllers.push_back(controller.name); swtch.request.start_controllers.push_back(controller.name); } - swtch.request.start_asap = true; + swtch.request.start_asap = static_cast(true); swtch.request.strictness = controller_manager_msgs::SwitchControllerRequest::STRICT; - if (not this->service_controller_switch_.call(swtch) or not swtch.response.ok) { + if (not this->service_controller_switch_.call(swtch) or + not static_cast(swtch.response.ok)) { throw std::runtime_error("Service call '" + this->service_controller_switch_.getService() + "' failed"); } @@ -432,7 +433,7 @@ void FrankaHWSim::readSim(ros::Time time, ros::Duration period) { this->updateRobotState(time); } -double FrankaHWSim::positionControl(Joint& joint, double setpoint, ros::Duration period) { +double FrankaHWSim::positionControl(Joint& joint, double setpoint, const ros::Duration& period) { double error; const double kJointLowerLimit = joint.limits.min_position; const double kJointUpperLimit = joint.limits.max_position; @@ -456,7 +457,7 @@ double FrankaHWSim::positionControl(Joint& joint, double setpoint, ros::Duration -joint.limits.max_effort, joint.limits.max_effort); } -double FrankaHWSim::velocityControl(Joint& joint, double setpoint, ros::Duration period) { +double FrankaHWSim::velocityControl(Joint& joint, double setpoint, const ros::Duration& period) { return boost::algorithm::clamp( joint.velocity_controller.computeCommand(setpoint - joint.velocity, period), -joint.limits.max_effort, joint.limits.max_effort); From 8b0fdfa7415e74b9ceba12476008d42ffdf6f03f Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Tue, 26 Jul 2022 10:47:48 +0200 Subject: [PATCH 16/22] ADD: CHANGELOG entry --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0ab84ced..85c4c724 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,8 @@ * **BREAKING**: `gripper_action` goes now to the commanded gripper position when `max_effort` is zero * `franka_gazebo`: Drop `delayed_controller_spawner.py` script in favor of `--wait-for TOPIC` flag from controller_manager * `franka_gazebo`: Properly calculate inertial properties of `world/stone/model.sdf` + * `franka_gazebo`: `set_user_stop` service to simulate User stop in Gazebo + * `franka_gazebo`: `error_recovery` action similar to `franka_control` ## 0.9.0 - 2022-03-29 From c34b1310a08e6af95f17d1d61ce264d3a570fe58 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Tue, 26 Jul 2022 10:58:13 +0200 Subject: [PATCH 17/22] FIX: Don't recovery from errors when user stop still pressed --- franka_gazebo/src/franka_hw_sim.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index b5094117..81094b8d 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -54,10 +54,13 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, this->action_recovery_ = std::make_unique>( model_nh, "franka_control/error_recovery", [&](const franka_msgs::ErrorRecoveryGoalConstPtr& goal) { + if (this->robot_state_.robot_mode == franka::RobotMode::kUserStopped) { + ROS_WARN_STREAM_NAMED("franka_hw_sim", + "Cannot recover errors since the user stop seems still pressed"); + this->action_recovery_->setSucceeded(); + return; + } try { - ROS_WARN_STREAM_COND_NAMED( - this->robot_state_.robot_mode == franka::RobotMode::kUserStopped, "franka_hw_sim", - "Cannot recover errors since the user stop seems still pressed"); restartControllers(); ROS_INFO_NAMED("franka_hw_sim", "Recovered from error"); this->sm_.process_event(ErrorRecovery()); From 8c314271a53955c44df39edd2096b800c4db71d9 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Wed, 27 Jul 2022 09:57:18 +0200 Subject: [PATCH 18/22] FIX: ROS_WARN message on user stop --- franka_gazebo/include/franka_gazebo/statemachine.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/franka_gazebo/include/franka_gazebo/statemachine.h b/franka_gazebo/include/franka_gazebo/statemachine.h index 8a04248d..e0e8b760 100644 --- a/franka_gazebo/include/franka_gazebo/statemachine.h +++ b/franka_gazebo/include/franka_gazebo/statemachine.h @@ -50,7 +50,7 @@ auto isStopping = [](const SwitchControl& event, const JointMap& joints) { auto start = [](franka::RobotState& state) { state.robot_mode = franka::RobotMode::kMove; }; auto idle = [](franka::RobotState& state) { state.robot_mode = franka::RobotMode::kIdle; }; auto stop = [](franka::RobotState& state, JointMap& joints) { - ROS_WARN("E-Stop pressed, stopping robot"); + ROS_WARN("User stop pressed, stopping robot"); state.robot_mode = franka::RobotMode::kUserStopped; state.q_d = state.q; state.dq_d = {0}; From 09be037ad0ae7293426253dd85e8c3fa3e6af2a6 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Wed, 27 Jul 2022 11:08:01 +0200 Subject: [PATCH 19/22] FIX: Don't stop finger joints on error recovery --- franka_gazebo/include/franka_gazebo/statemachine.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/franka_gazebo/include/franka_gazebo/statemachine.h b/franka_gazebo/include/franka_gazebo/statemachine.h index e0e8b760..4c051c3e 100644 --- a/franka_gazebo/include/franka_gazebo/statemachine.h +++ b/franka_gazebo/include/franka_gazebo/statemachine.h @@ -26,12 +26,12 @@ struct UserStop { }; // Guards +auto contains = [](const auto& haystack, const auto& needle) { + return haystack.find(needle) != std::string::npos; +}; auto isPressed = [](const UserStop& event) { return event.pressed; }; auto isReleased = [](const UserStop& event, const JointMap& joints) { return not event.pressed; }; auto isStarting = [](const SwitchControl& event, const JointMap& joints) { - auto contains = [](const auto& haystack, const auto& needle) { - return haystack.find(needle) != std::string::npos; - }; for (auto& joint : joints) { if (contains(joint.first, "_finger_joint")) { continue; @@ -57,6 +57,9 @@ auto stop = [](franka::RobotState& state, JointMap& joints) { state.ddq_d = {0}; for (auto& joint : joints) { + if (contains(joint.first, "_finger_joint")) { + continue; + } joint.second->stop_position = joint.second->position; joint.second->desired_position = joint.second->position; joint.second->desired_velocity = 0; From c74af0e3ae848f132f4d80429ead97053651068b Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Wed, 27 Jul 2022 11:08:14 +0200 Subject: [PATCH 20/22] FIX: Allow finger joints to still be controlled while not in Move --- franka_gazebo/src/franka_hw_sim.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 81094b8d..b9884ec1 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -475,7 +475,8 @@ void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration period) { // Retrieve effort control command double effort = 0; - if (not sm_.is(state)) { + // Finger joints must still be controllable from franka_gripper_sim controller + if (not sm_.is(state) and not contains(pair.first, "finger_joint")) { effort = positionControl(*joint, joint->stop_position, period); } else if (joint->control_method == POSITION) { effort = positionControl(*joint, joint->desired_position, period); From 5da709657f98434cec152de4e029bd5d13ebeb8e Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Wed, 27 Jul 2022 13:59:15 +0200 Subject: [PATCH 21/22] ADD: Make interactive marker configurable in `panda.launch` In some scenarios we want to start the cartesian_impedance_example controller but command the equilibrium pose by ourselves. A running `interactive_marker` node will keep publishing onto the topic, making it compete with our publisher. To avoid breaking changes we make the presence of the node also configurable. By default it will still be started whenever the cartesian impedance controller is started but this can be deliberately overwritten by the user. --- franka_gazebo/launch/panda.launch | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch index e67cc8e8..6e7cbb55 100644 --- a/franka_gazebo/launch/panda.launch +++ b/franka_gazebo/launch/panda.launch @@ -31,6 +31,7 @@ -J $(arg arm_id)_finger_joint1 0.001 -J $(arg arm_id)_finger_joint2 0.001" /> + @@ -90,7 +91,7 @@ + if="$(arg interactive_marker)"> From 249c593c8ff4d3f476dd74d5e7bfbf67d0a9d206 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Wed, 27 Jul 2022 15:06:01 +0200 Subject: [PATCH 22/22] ADD: Integration test verifying that user stops stops joint in Gazebo --- .../test/franka_hw_sim_gazebo_test.cpp | 204 ++++++++++++++++++ .../test/launch/franka_hw_sim_gazebo.test | 1 + 2 files changed, 205 insertions(+) diff --git a/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp b/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp index 5ab21af3..08166450 100644 --- a/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp +++ b/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp @@ -1,9 +1,17 @@ +#include #include +#include #include #include +#include +#include +#include +#include #include #include #include +#include +#include #include #include #include @@ -11,7 +19,13 @@ #include using namespace franka_gazebo; +using franka::RobotMode; namespace sml = boost::sml; + +// NOTE: Keep a global node handle in memory in order for all tests to be executed sequentially +// If each test or fixture declares its own node handle only the first test will be executed correctly +std::unique_ptr nh; + TEST( #if ROS_VERSION_MINIMUM(1, 15, 13) FrankaHWSim, /* noetic & newer */ @@ -277,8 +291,198 @@ TEST_F(StateMachineFixture, ASSERT_EQ(state.ddq_d, zeros); } +struct FrankaHWSimFixture : public ::testing::Test { + private: + double start_; + + public: + ros::ServiceClient user_stop; + std::unique_ptr> gripper; + std::unique_ptr> error_recovery; + + virtual void SetUp() { + start_ = ros::Time::now().toSec(); + user_stop = nh->serviceClient("set_user_stop"); + gripper = std::make_unique>( + "/franka_gripper/move"); + error_recovery = + std::make_unique>( + "/franka_control/error_recovery"); + } + + double now() { return ros::Time::now().toSec() - start_; } + + void openGripper() { + franka_gripper::MoveGoal goal; + goal.speed = 0.1; + goal.width = franka_gazebo::kMaxFingerWidth; + gripper->sendGoalAndWait(goal, ros::Duration(5)); + auto result = gripper->getResult(); + ASSERT_TRUE(result->success) << "Failed to open the gripper, error: " << result->error; + } + void closeGripper() { + franka_gripper::MoveGoal goal; + goal.speed = 0.1; + goal.width = 0.0; + gripper->sendGoalAndWait(goal, ros::Duration(5)); + auto result = gripper->getResult(); + ASSERT_TRUE(result->success) << "Failed to close the gripper, error: " << result->error; + } + + void errorRecovery() { + error_recovery->sendGoalAndWait(franka_msgs::ErrorRecoveryGoal(), ros::Duration(5)); + } + + std_srvs::SetBool::Response setUserStop(bool pressed) { + std_srvs::SetBool service; + service.request.data = pressed; + user_stop.call(service); + return service.response; + } + + virtual void TearDown() { + closeGripper(); + setUserStop(false); + errorRecovery(); + } +}; + +TEST_F(FrankaHWSimFixture, pressing_user_stop_stops_the_arm_joints) { + // Test procedure: + // Command a circular motion to the impedance controller to get the joints moving + // After some time (user_stop_time) press the user stop, let the joints stop as fast as possible + // After again some time (assert_start_time) measure the velocities of all joints + // After again some time (assert_end_time) verify if the average of the velocities is below some + // threshold. Have to filter because velocity signal is noisy + + double radius = 0.10; + double frequency = 0.5; + double user_stop_time = 3; + double assert_start_time = 3.5; + double assert_end_time = 4.5; + double stop_speed_tolerance = 1e-2; // [rad/s] + + ros::Duration timeout(5); + + geometry_msgs::PoseStamped target; + target.pose.position.z = 0.6; + target.pose.orientation.x = 1; + target.header.frame_id = "panda_link0"; + size_t samples = 0; + std::map velocities; + auto pub = nh->advertise( + "/cartesian_impedance_example_controller/equilibrium_pose", 1); + ros::Rate rate(30); + + bool user_stop_pressed = false; + while (ros::ok()) { + rate.sleep(); + double t = now(); + + target.pose.position.x = radius * std::sin(2 * M_PI * frequency * t) + 0.3; + target.pose.position.y = radius * std::cos(2 * M_PI * frequency * t); + pub.publish(target); + + if (t < user_stop_time) { + continue; + } + // After some time, we press the user stop + if (not user_stop_pressed) { + setUserStop(true); + user_stop_pressed = true; + } + if (t < assert_start_time) { + continue; + } + + auto state = ros::topic::waitForMessage( + "/franka_state_controller/franka_states", *nh, timeout); + ASSERT_TRUE(state != nullptr) + << "No message on /franka_state_controller/franka_states received within " + << timeout.toSec() << "s"; + EXPECT_EQ(static_cast(state->robot_mode), RobotMode::kUserStopped); + + // A little while later we check if the joints are still moving + auto msg = ros::topic::waitForMessage("/joint_states", *nh, timeout); + ASSERT_TRUE(msg != nullptr) << "No message on /joint_states received within " << timeout.toSec() + << "s"; + for (int i = 0; i < msg->name.size(); i++) { + auto name = msg->name.at(i); + if (contains(name, "finger_joint")) { + continue; + } + velocities.emplace(name, 0); // insert if not yet exist + velocities.at(name) += msg->velocity.at(i); + } + samples++; + + if (t < assert_end_time) { + continue; + } + + for (auto pair : velocities) { + auto name = pair.first; + auto velocity = pair.second / samples; + EXPECT_NEAR(velocity, 0, stop_speed_tolerance) << "Joint " << name << " is still moving!"; + } + + break; // finish test + } +} + +TEST_F(FrankaHWSimFixture, pressing_user_stop_lets_finger_joints_still_move) { + // Test Procedure: + // Arange by closing the gripper after (close_gripper_time) + // After some time (user_stop_time) the user stop is pressed + // Then after some time (open_gripper_time) make a move action call to open the fingers + // When the action is finished, check if the finger positions are actually where we commanded them + + double user_stop_time = 1; + double open_gripper_time = 1.5; + double position_tolerance = 5e-3; // [m] + + bool pressed_user_stop = false; + ros::Rate rate(30); + ros::Duration timeout(5); + + while (ros::ok()) { + rate.sleep(); + + double t = now(); + + if (t < user_stop_time) { + continue; + } + + if (not pressed_user_stop) { + pressed_user_stop = true; + setUserStop(true); + } + + if (t < open_gripper_time) { + continue; + } + openGripper(); + + auto msg = ros::topic::waitForMessage("/franka_gripper/joint_states", + *nh, timeout); + ASSERT_TRUE(msg != nullptr) << "No message on /franka_gripper/joint_states received within " + << timeout.toSec() << "s"; + for (int i = 0; i < msg->name.size(); i++) { + auto name = msg->name.at(i); + if (not contains(name, "finger_joint")) { + continue; + } + EXPECT_NEAR(msg->position.at(i), franka_gazebo::kMaxFingerWidth / 2., position_tolerance); + } + + break; // finish test + } +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "franka_hw_sim_test"); + nh = std::make_unique(); return RUN_ALL_TESTS(); } diff --git a/franka_gazebo/test/launch/franka_hw_sim_gazebo.test b/franka_gazebo/test/launch/franka_hw_sim_gazebo.test index 7edc56c4..b9b86102 100644 --- a/franka_gazebo/test/launch/franka_hw_sim_gazebo.test +++ b/franka_gazebo/test/launch/franka_hw_sim_gazebo.test @@ -3,6 +3,7 @@ +