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/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 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/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/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index c2a85b35..0d9dae5f 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -1,12 +1,14 @@ #pragma once -#include +#include #include #include #include +#include #include #include #include +#include #include #include #include @@ -15,11 +17,13 @@ #include #include #include +#include #include #include #include #include #include +#include namespace franka_gazebo { @@ -45,6 +49,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. * @@ -125,9 +134,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_; @@ -135,6 +141,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_; @@ -145,6 +152,10 @@ 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_; + ros::ServiceClient service_controller_list_; + ros::ServiceClient service_controller_switch_; + std::unique_ptr> action_recovery_; std::vector lower_force_thresholds_nominal_; std::vector upper_force_thresholds_nominal_; @@ -162,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(); @@ -169,6 +182,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, 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 = "") { std::array x; diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h index 3770f537..a6e9bdea 100644 --- a/franka_gazebo/include/franka_gazebo/joint.h +++ b/franka_gazebo/include/franka_gazebo/joint.h @@ -1,9 +1,12 @@ #pragma once #include +#include +#include #include #include #include +#include #include namespace franka_gazebo { @@ -64,8 +67,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; @@ -94,6 +97,50 @@ struct Joint { /// isInCollision double collision_threshold = std::numeric_limits::infinity(); + /// 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$ @@ -112,6 +159,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/include/franka_gazebo/statemachine.h b/franka_gazebo/include/franka_gazebo/statemachine.h new file mode 100644 index 00000000..4c051c3e --- /dev/null +++ b/franka_gazebo/include/franka_gazebo/statemachine.h @@ -0,0 +1,84 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +namespace franka_gazebo { + +using JointMap = std::map>; + +// States +struct Idle {}; +struct Move {}; +struct UserStopped {}; + +// Events +struct ErrorRecovery {}; +struct SwitchControl {}; +struct UserStop { + bool pressed; +}; + +// 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) { + 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, JointMap& joints) { + ROS_WARN("User 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) { + 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; + } +}; + +struct StateMachine { + auto operator()() const { + using namespace boost::sml; + return make_transition_table( + // clang-format off + *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 + ); + } +}; +} // namespace franka_gazebo 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)"> 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 33b0ae71..b9884ec1 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 @@ -12,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -21,6 +24,11 @@ namespace franka_gazebo { +using actionlib::SimpleActionServer; +using boost::sml::state; + +FrankaHWSim::FrankaHWSim() : sm_(this->robot_state_, this->joints_) {} + bool FrankaHWSim::initSim(const std::string& robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent, @@ -41,9 +49,30 @@ 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); + 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 { + 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(); + #if GAZEBO_MAJOR_VERSION >= 8 gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics(); #else @@ -134,20 +163,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; } @@ -237,6 +261,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_)) { @@ -358,6 +385,47 @@ 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{static_cast(request.data)}); + 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 = static_cast(true); + swtch.request.strictness = controller_manager_msgs::SwitchControllerRequest::STRICT; + 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"); + } } void FrankaHWSim::readSim(ros::Time time, ros::Duration period) { @@ -368,6 +436,36 @@ void FrankaHWSim::readSim(ros::Time time, ros::Duration period) { this->updateRobotState(time); } +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; + 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, const 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_); @@ -376,46 +474,26 @@ void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration period) { // Retrieve effort control command double effort = 0; + + // 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); + } 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); } - auto& control_method = joint->control_method; - if (control_method == EFFORT) { - effort = joint->command + joint->gravity; - } else if (control_method == POSITION) { - // Use position motion generator - 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, joint->desired_position, - kJointLowerLimit, kJointUpperLimit, error); - break; - default: - std::string error_message = - "Only revolute 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( - position_pid_controllers_[joint->name].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) + - joint->gravity; - } + effort += joint->gravity; // Send control effort control command if (not std::isfinite(effort)) { @@ -560,6 +638,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); @@ -568,19 +647,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; @@ -589,6 +659,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 +724,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; }); @@ -664,6 +736,8 @@ void FrankaHWSim::doSwitch(const std::list& joint.desired_position = joint.position; joint.desired_velocity = 0; }); + + this->sm_.process_event(SwitchControl()); } void FrankaHWSim::forControlledJoint( 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(); diff --git a/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp b/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp index e922653b..08166450 100644 --- a/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp +++ b/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp @@ -1,14 +1,39 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include +#include +#include +#include +#include +#include +#include +#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) - 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,8 +51,438 @@ 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); +} + +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 @@ +