Add hold_joints parameter (backport #251) (#354)

Co-authored-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

doc/index.rst

@@ -213,6 +213,7 @@ The *gazebo_ros2_control* ``<plugin>`` tag also has the following optional child
 * ``<robot_param>``: The location of the ``robot_description`` (URDF) on the parameter server, defaults to ``robot_description``
 * ``<robot_param_node>``: Name of the node where the ``robot_param`` is located, defaults to ``robot_state_publisher``
 * ``<parameters>``: YAML file with the configuration of the controllers
+* ``<hold_joints>``: if set to true (default), it will hold the joints' position if their interface was not claimed, e.g., the controller hasn't been activated yet.
 
 Default gazebo_ros2_control Behavior
 -----------------------------------------------------------

gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp

@@ -184,6 +184,12 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
   } else {
     impl_->robot_description_node_ = "robot_state_publisher";  // default
   }
+  // Hold joints if no control mode is active?
+  bool hold_joints = true;  // default
+  if (sdf->HasElement("hold_joints")) {
+    hold_joints =
+      sdf->GetElement("hold_joints")->Get<bool>();
+  }
 
   // Read urdf from ros parameter server
   std::string urdf_string;
@@ -326,6 +332,23 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
     RCLCPP_DEBUG(
       impl_->model_nh_->get_logger(), "Loaded hardware interface %s!",
       robot_hw_sim_type_str_.c_str());
+    try {
+      node_ros2->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints));
+    } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
+      RCLCPP_ERROR(
+        impl_->model_nh_->get_logger(), "Parameter 'hold_joints' has already been declared, %s",
+        e.what());
+    } catch (const rclcpp::exceptions::InvalidParametersException & e) {
+      RCLCPP_ERROR(
+        impl_->model_nh_->get_logger(), "Parameter 'hold_joints' has invalid name, %s", e.what());
+    } catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
+      RCLCPP_ERROR(
+        impl_->model_nh_->get_logger(), "Parameter 'hold_joints' value is invalid, %s", e.what());
+    } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
+      RCLCPP_ERROR(
+        impl_->model_nh_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s",
+        e.what());
+    }
     if (!gazeboSystem->initSim(
         node_ros2,
         impl_->parent_model_,

gazebo_ros2_control/src/gazebo_system.cpp

@@ -116,6 +116,9 @@ class gazebo_ros2_control::GazeboSystemPrivate
 
   /// \brief mapping of mimicked joints to index of joint they mimic
   std::vector<MimicJoint> mimic_joints_;
+
+  // Should hold the joints if no control_mode is active
+  bool hold_joints_ = true;
 };
 
 namespace gazebo_ros2_control
@@ -141,6 +144,30 @@ bool GazeboSystem::initSim(
     return false;
   }
 
+  try {
+    this->dataPtr->hold_joints_ = this->nh_->get_parameter("hold_joints").as_bool();
+  } catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'hold_joints' not initialized, with error %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
+  } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'hold_joints' not declared, with error %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
+  } catch (rclcpp::ParameterTypeException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'hold_joints' has wrong type: %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
+  }
+  RCLCPP_DEBUG_STREAM(
+    this->nh_->get_logger(), "hold_joints (system): " << this->dataPtr->hold_joints_ << std::endl);
+
   registerJoints(hardware_info, parent_model);
   registerSensors(hardware_info, parent_model);
 
@@ -739,25 +766,21 @@ hardware_interface::return_type GazeboSystem::write(
         double cmd = this->dataPtr->joint_position_cmd_[j];
         this->dataPtr->sim_joints_[j]->SetPosition(0, cmd, true);
         this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
-
       } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) {
         this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]);
-
       } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) {
         this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]);
-
       } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY_PID) {
         double vel_goal = this->dataPtr->joint_velocity_cmd_[j];
         double vel = this->dataPtr->sim_joints_[j]->GetVelocity(0);
         double cmd = this->dataPtr->vel_pid[j].computeCommand(vel_goal - vel, dt);
         this->dataPtr->sim_joints_[j]->SetForce(0, cmd);
-
       } else if (this->dataPtr->joint_control_methods_[j] & POSITION_PID) {
         double pos_goal = this->dataPtr->joint_position_cmd_[j];
         double pos = this->dataPtr->sim_joints_[j]->Position(0);
         double cmd = this->dataPtr->pos_pid[j].computeCommand(pos_goal - pos, dt);
         this->dataPtr->sim_joints_[j]->SetForce(0, cmd);
-      } else if (this->dataPtr->is_joint_actuated_[j]) {
+      } else if (this->dataPtr->is_joint_actuated_[j] && this->dataPtr->hold_joints_) {
         // Fallback case is a velocity command of zero (only for actuated joints)
         this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
       }