Fixed position control (#29) (#34)

Signed-off-by: ahcorde <ahcorde@gmail.com>
ros-controls · Feb 10, 2022 · 482987c · 482987c
1 parent c859881
commit 482987c
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 17 deletions.
diff --git a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp
@@ -61,7 +61,8 @@ class IgnitionSystem : public IgnitionSystemInterface
     rclcpp::Node::SharedPtr & model_nh,
     std::map<std::string, ignition::gazebo::Entity> & joints,
     const hardware_interface::HardwareInfo & hardware_info,
-    ignition::gazebo::EntityComponentManager & _ecm) override;
+    ignition::gazebo::EntityComponentManager & _ecm,
+    int & update_rate) override;
 
 private:
   // Register a sensor (for now just IMUs)

diff --git a/ign_ros2_control/include/ign_ros2_control/ign_system_interface.hpp b/ign_ros2_control/include/ign_ros2_control/ign_system_interface.hpp
@@ -82,11 +82,13 @@ class IgnitionSystemInterface
   /// related with the entity in Gazebo
   /// param[in] hardware_info structure with data from URDF.
   /// param[in] _ecm Entity-component manager.
+  /// param[in] update_rate controller update rate
   virtual bool initSim(
     rclcpp::Node::SharedPtr & model_nh,
     std::map<std::string, ignition::gazebo::Entity> & joints,
     const hardware_interface::HardwareInfo & hardware_info,
-    ignition::gazebo::EntityComponentManager & _ecm) = 0;
+    ignition::gazebo::EntityComponentManager & _ecm,
+    int & update_rate) = 0;
 
   // Methods used to control a joint.
   enum ControlMethod_

diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp
@@ -101,6 +101,9 @@ class IgnitionROS2ControlPluginPrivate
 
   /// \brief ECM pointer
   ignition::gazebo::EntityComponentManager * ecm{nullptr};
+
+  /// \brief controller update rate
+  int update_rate;
 };
 
 //////////////////////////////////////////////////
@@ -387,7 +390,8 @@ void IgnitionROS2ControlPlugin::Configure(
         this->dataPtr->node_,
         enabledJoints,
         control_hardware[i],
-        _ecm))
+        _ecm,
+        this->dataPtr->update_rate))
     {
       RCLCPP_FATAL(
         this->dataPtr->node_->get_logger(), "Could not initialize robot simulation interface");
@@ -412,10 +416,11 @@ void IgnitionROS2ControlPlugin::Configure(
     return;
   }
 
-  auto cm_update_rate = this->dataPtr->controller_manager_->get_parameter("update_rate").as_int();
+  this->dataPtr->update_rate =
+    this->dataPtr->controller_manager_->get_parameter("update_rate").as_int();
   this->dataPtr->control_period_ = rclcpp::Duration(
     std::chrono::duration_cast<std::chrono::nanoseconds>(
-      std::chrono::duration<double>(1.0 / static_cast<double>(cm_update_rate))));
+      std::chrono::duration<double>(1.0 / static_cast<double>(this->dataPtr->update_rate))));
 
   this->dataPtr->entity_ = _entity;
 }

diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp
@@ -126,6 +126,9 @@ class ign_ros2_control::IgnitionSystemPrivate
   /// methods, otherwise the app will crash
   ignition::gazebo::EntityComponentManager * ecm;
 
+  /// \brief controller update rate
+  int * update_rate;
+
   /// \brief Ignition communication node.
   ignition::transport::Node node;
 };
@@ -137,15 +140,18 @@ bool IgnitionSystem::initSim(
   rclcpp::Node::SharedPtr & model_nh,
   std::map<std::string, ignition::gazebo::Entity> & enableJoints,
   const hardware_interface::HardwareInfo & hardware_info,
-  ignition::gazebo::EntityComponentManager & _ecm)
+  ignition::gazebo::EntityComponentManager & _ecm,
+  int & update_rate)
 {
   this->dataPtr = std::make_unique<IgnitionSystemPrivate>();
 
   this->nh_ = model_nh;
   this->dataPtr->ecm = &_ecm;
   this->dataPtr->n_dof_ = hardware_info.joints.size();
 
-  RCLCPP_DEBUG(this->nh_->get_logger(), "n_dof_ %d", this->dataPtr->n_dof_);
+  this->dataPtr->update_rate = &update_rate;
+
+  RCLCPP_DEBUG(this->nh_->get_logger(), "n_dof_ %lu", this->dataPtr->n_dof_);
 
   this->dataPtr->joints_.resize(this->dataPtr->n_dof_);
 
@@ -412,18 +418,24 @@ hardware_interface::return_type IgnitionSystem::write()
     }
 
     if (this->dataPtr->joints_[i].joint_control_method & POSITION) {
-      if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointPositionReset>(
-          this->dataPtr->joints_[i].sim_joint))
-      {
+      // Get error in position
+      double error;
+      error = (this->dataPtr->joints_[i].joint_position -
+        this->dataPtr->joints_[i].joint_position_cmd) * *this->dataPtr->update_rate;
+
+      // Calculate target velcity
+      double targetVel = -error;
+
+      auto vel =
+        this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
+        this->dataPtr->joints_[i].sim_joint);
+
+      if (vel == nullptr) {
         this->dataPtr->ecm->CreateComponent(
           this->dataPtr->joints_[i].sim_joint,
-          ignition::gazebo::components::JointPositionReset(
-            {this->dataPtr->joints_[i].joint_position}));
-        const auto jointPosCmd =
-          this->dataPtr->ecm->Component<ignition::gazebo::components::JointPositionReset>(
-          this->dataPtr->joints_[i].sim_joint);
-        *jointPosCmd = ignition::gazebo::components::JointPositionReset(
-          {this->dataPtr->joints_[i].joint_position_cmd});
+          ignition::gazebo::components::JointVelocityCmd({targetVel}));
+      } else if (!vel->Data().empty()) {
+        vel->Data()[0] = targetVel;
       }
     }