From c1fcb2c343b2bf83ec135b90069ac433a08229ad Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Mon, 17 Aug 2020 13:51:51 +0200
Subject: [PATCH 1/7] Absolem: Add relative positional control.

---
 .../launch/vehicle_topics.launch              |  31 ++++-
 .../specifications.md                         |   6 +-
 .../src/flipper_control_plugin.cpp            | 126 +++++++++---------
 3 files changed, 97 insertions(+), 66 deletions(-)

diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch
index 62c2582e..dc792567 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch
@@ -71,6 +71,13 @@
       args="/model/$(arg name)/joint/front_left_flipper_j/cmd_pos@std_msgs/Float64]ignition.msgs.Double">
       <remap from="/model/$(arg name)/joint/front_left_flipper_j/cmd_pos" to="flippers_cmd_pos/front_left"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_front_left_flipper_pos_rel"
+      args="/model/$(arg name)/joint/front_left_flipper_j/cmd_pos_rel@std_msgs/Float64]ignition.msgs.Double">
+      <remap from="/model/$(arg name)/joint/front_left_flipper_j/cmd_pos_rel" to="flippers_cmd_pos_rel/front_left"/>
+    </node>
 
     <node
       pkg="ros_ign_bridge"
@@ -86,6 +93,13 @@
       args="/model/$(arg name)/joint/front_right_flipper_j/cmd_pos@std_msgs/Float64]ignition.msgs.Double">
       <remap from="/model/$(arg name)/joint/front_right_flipper_j/cmd_pos" to="flippers_cmd_pos/front_right"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_front_right_flipper_pos_rel"
+      args="/model/$(arg name)/joint/front_right_flipper_j/cmd_pos_rel@std_msgs/Float64]ignition.msgs.Double">
+      <remap from="/model/$(arg name)/joint/front_right_flipper_j/cmd_pos_rel" to="flippers_cmd_pos_rel/front_right"/>
+    </node>
 
     <node
       pkg="ros_ign_bridge"
@@ -101,6 +115,13 @@
       args="/model/$(arg name)/joint/rear_left_flipper_j/cmd_pos@std_msgs/Float64]ignition.msgs.Double">
       <remap from="/model/$(arg name)/joint/rear_left_flipper_j/cmd_pos" to="flippers_cmd_pos/rear_left"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_rear_left_flipper_pos_rel"
+      args="/model/$(arg name)/joint/rear_left_flipper_j/cmd_pos_rel@std_msgs/Float64]ignition.msgs.Double">
+      <remap from="/model/$(arg name)/joint/rear_left_flipper_j/cmd_pos_rel" to="flippers_cmd_pos_rel/rear_left"/>
+    </node>
 
     <node
       pkg="ros_ign_bridge"
@@ -115,7 +136,15 @@
       name="ros_ign_bridge_rear_right_flipper_pos"
       args="/model/$(arg name)/joint/rear_right_flipper_j/cmd_pos@std_msgs/Float64]ignition.msgs.Double">
       <remap from="/model/$(arg name)/joint/rear_right_flipper_j/cmd_pos" to="flippers_cmd_pos/rear_right"/>
-    </node>   
+    </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_rear_right_flipper_pos_rel"
+      args="/model/$(arg name)/joint/rear_right_flipper_j/cmd_pos_rel@std_msgs/Float64]ignition.msgs.Double">
+      <remap from="/model/$(arg name)/joint/rear_right_flipper_j/cmd_pos_rel" to="flippers_cmd_pos_rel/rear_right"/>
+    </node>
+
     <!-- Laser rotation control -->
     
     <node
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/specifications.md b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/specifications.md
index 16b551c6..59e7eaa5 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/specifications.md
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/specifications.md
@@ -9,9 +9,9 @@ This configuration is based BlueBotics Absolem tracked robot. The robot is equip
 ## Usage Instructions
 The robot motion is controlled via standard `cmd_vel` commands.
 
-Flippers can be velocity-controlled by publishing to topics `flippers_cmd_vel/front_left` (`front_right`, `rear_left`, `rear_right`) (`std_msgs/Float64`) for velocity control or topics `flippers_cmd_pos/front_left`, etc. for position control. Maximum angular velocity of the flippers is `pi/4 rad/s`. The effort limits in the model were set so that the robot can support itself with the flippers, but cannot use them to lift itself on all four flippers. This is how the real flippers work. The current position of the flippers is published to `joint_states` as `front_left_flipper_j` etc. The flippers can continuously rotate.
+Flippers can be controlled by publishing to topics `flippers_cmd_vel/front_left` (`front_right`, `rear_left`, `rear_right`) (`std_msgs/Float64`) for velocity control or topics `flippers_cmd_pos/front_left`, etc. for position control. Relative positional control is available on `flippers_cmd_pos_rel/front_left` etc. Maximum angular velocity of the flippers is `pi/4 rad/s`. The effort limits in the model were set so that the robot can support itself with the flippers, but cannot use them to lift itself on all four flippers. This is how the real flippers work. The current position of the flippers is published to `joint_states` as `front_left_flipper_j` etc. The flippers can continuously rotate.
 
-The laser rotation is velocity-controlled by publishing to topic `scanning_speed_cmd` (`std_msgs/Float64`). The laser has hard stops at `+-2.36 rad` and maximum rotation velocity is `1.2 rad/s`. The laser has an automatic controller that reverses the rotation direction at a given angle (currently ca. `1.6 rad`). The current position of the laser is published to `joint_states` as `laser_j`. The default (zero) position of the laser is such that the scanning plane is levelled with ground.
+The laser rotation is velocity-controlled by publishing to topic `lidar_gimbal/roll_rate_cmd_double` (`std_msgs/Float64`). The laser has hard stops at `+-2.36 rad` and maximum rotation velocity is `1.2 rad/s`. The laser has an automatic controller that reverses the rotation direction at a given angle (currently ca. `1.6 rad`). The current position of the laser is published to `joint_states` as `laser_j`. The default (zero) position of the laser is such that the scanning plane is levelled with ground.
 
 The robot is equipped with two more passive joints which connect the tracks to `base_link`. They are called `left_track_j` and `right_track_j`. These joints are connected via a differential with lockable brake. The differential makes sure that `angle(left_track_j) == -angle(right_track_j)` at all times. This model configuration has the differential brake applied in zero position, which means the tracks cannot move relative to the robot body. 
 
@@ -42,7 +42,7 @@ The following specific sensors are declared payloads of this vehicle:
 ### Control
 This robot is controlled by the DiffDrive plugin.  It accepts twist inputs which drives the vehicle along the x-direction and around the z-axis. We add additional 8 pseudo-wheels where the robot's tracks are to better approximate a track vehicle (flippers are subdivided to 5 pseudo-wheels). Currently, we are not aware of a track-vehicle plugin for ignition-gazebo.  A TrackedVehicle plugin does exist in gazebo8+, but it is not straightforward to port to ignition-gazebo.  We hope to work with other SubT teams and possibly experts among the ignition-gazebo developers to address this in the future.
 
-Flippers provide a velocity control interface, but a positional controller and a higher-level control policy are strongly suggested.
+Flippers provide interfaces for velocity control and absolute and relative positional control. The positional controllers move flippers to the given position using the maximum speed of the flippers.
 
 ### Motion characteristics
 
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
index 5da11bda..5c3621a5 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
@@ -26,16 +26,21 @@ namespace cras
 /// `<topic_vel>`: Custom topic that this system will subscribe to in order to receive velocity commands messages.
 /// This element is optional, and the default value is `/model/{name_of_model}/joints/{joint_name}/cmd_vel`.
 ///
-/// `<topic_pos>`: Custom topic that this system will subscribe to in order to receive position commands messages.
+/// `<topic_pos_abs>`: Custom topic that this system will subscribe to in order to receive position commands messages.
 /// This element is optional, and the default value is `/model/{name_of_model}/joints/{joint_name}/cmd_pos`.
 ///
+/// `<topic_pos_rel>`: Custom topic that this system will subscribe to in order to receive relative position commands
+/// messages. This element is optional, and the default value is
+/// `/model/{name_of_model}/joints/{joint_name}/cmd_pos_rel`.
+///
 /// `<max_velocity>`: Maximum angular velocity of the flipper joint that is used in positional control.
 ///
 /// # Subscriptions
 ///
 /// `{topic_vel}` (`ignition::msgs::Double`): The desired rotation velocity of the flipper.
-/// `{topic_pos}` (`ignition::msgs::Double`): The positional setpoint of the flipper.
-class FlipperControlPlugin : public System, public ISystemConfigure, public ISystemPreUpdate, public ISystemPostUpdate
+/// `{topic_pos_abs}` (`ignition::msgs::Double`): The positional setpoint of the flipper.
+/// `{topic_pos_rel}` (`ignition::msgs::Double`): Relative positional setpoint of the flipper.
+class FlipperControlPlugin : public System, public ISystemConfigure, public ISystemPreUpdate
 {
   public: void Configure(const Entity& _entity, const std::shared_ptr<const sdf::Element>& _sdf,
                          EntityComponentManager& _ecm, EventManager& _eventMgr) override
@@ -69,13 +74,18 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
       topicVel = _sdf->Get<std::string>("topic_vel");
     this->node.Subscribe(topicVel, &FlipperControlPlugin::OnCmdVel, this);
 
-    std::string topicPos {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_pos"};
-    if (_sdf->HasElement("topic_pos"))
-      topicPos = _sdf->Get<std::string>("topic_pos");
-    this->node.Subscribe(topicPos, &FlipperControlPlugin::OnCmdPos, this);
+    std::string topicPosAbs {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_pos"};
+    if (_sdf->HasElement("topic_pos_abs"))
+      topicPosAbs = _sdf->Get<std::string>("topic_pos_abs");
+    this->node.Subscribe(topicPosAbs, &FlipperControlPlugin::OnCmdPosAbs, this);
+
+    std::string topicPosRel {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_pos_rel"};
+    if (_sdf->HasElement("topic_pos_rel"))
+      topicPosRel = _sdf->Get<std::string>("topic_pos_rel");
+    this->node.Subscribe(topicPosRel, &FlipperControlPlugin::OnCmdPosRel, this);
 
     ignmsg << "FlipperControlPlugin subscribing to cmd_vel messages on [" << topicVel << "] and cmd_pos messages on ["
-      << topicPos << "]" << std::endl;
+           << topicPosAbs << "] and cmd_pos_rel messages on [" << topicPosRel << "]" << std::endl;
   }
 
   public: void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override
@@ -102,76 +112,63 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
 
     auto pos = _ecm.Component<components::JointPosition>(this->joint);
     if (!pos)
-      _ecm.CreateComponent(this->joint, components::JointPosition());
-
-    auto vel = _ecm.Component<components::JointVelocityCmd>(this->joint);
-    if (!vel)
-    {
-      _ecm.CreateComponent(this->joint, components::JointVelocityCmd({this->angularSpeed}));
-    }
-    else
     {
-      *vel = components::JointVelocityCmd({this->angularSpeed});
+      _ecm.CreateComponent(this->joint, components::JointPosition());
+      pos = _ecm.Component<components::JointPosition>(this->joint);
     }
 
-    if (this->angularSpeed == 0.0)
-      this->correctStaticAnglePosition(this->joint, this->staticAngle, _ecm);
-  }
-
-  public: void PostUpdate(const UpdateInfo& _info, const EntityComponentManager& _ecm) override
-  {
     if (this->cmdVel.has_value())
     {
       const auto velocity = this->cmdVel.value();
-      if (this->joint != kNullEntity) {
-        if (this->angularSpeed != 0.0 && velocity == 0.0)
-        {
-          auto pos = _ecm.Component<components::JointPosition>(this->joint);
-          if (pos)
-          {
-            this->staticAngle = pos->Data()[0];
-          }
-        }
-        const auto sanitizedVelocity = math::clamp(velocity, -this->maxAngularVelocity, this->maxAngularVelocity);
-        this->angularSpeed = sanitizedVelocity;
+      this->staticAngle.reset();
+      if (this->angularSpeed != 0.0 && velocity == 0.0)
+      {
+        this->staticAngle = pos->Data()[0];
       }
+      this->angularSpeed = velocity;
       this->cmdVel.reset();
-    } else if (this->cmdPos.has_value()) {
-      const auto position = this->cmdPos.value();
-      if (this->joint != kNullEntity) {
-        this->staticAngle = position;
-      }
-      this->cmdPos.reset();
+    } else if (this->cmdPosAbs.has_value()) {
+      const auto position = this->cmdPosAbs.value();
+      this->staticAngle = position;
+      this->cmdPosAbs.reset();
+    } else if (this->cmdPosRel.has_value()) {
+      const auto position = pos->Data()[0] + this->cmdPosRel.value();
+      this->staticAngle = position;
+      this->cmdPosRel.reset();
     }
 
     if (this->cmdTorque.has_value()) {
       this->UpdateMaxTorque(this->cmdTorque.value(), _ecm);
       this->cmdTorque.reset();
     }
+
+    auto velocityCommand = this->angularSpeed;
+    if (this->staticAngle.has_value())
+      velocityCommand = this->correctStaticAnglePosition(this->joint, this->staticAngle.value(), _ecm);
+
+    auto vel = _ecm.Component<components::JointVelocityCmd>(this->joint);
+    if (!vel)
+    {
+      _ecm.CreateComponent(this->joint, components::JointVelocityCmd({velocityCommand}));
+    }
+    else
+    {
+      *vel = components::JointVelocityCmd({velocityCommand});
+    }
   }
 
   // To mitigate integrating small velocity errors, if the flipper is said to be stationary, we check that its position
   // does not drift over time.
-  protected: void correctStaticAnglePosition(const Entity& joint, const math::Angle& staticPos, EntityComponentManager& _ecm) {
+protected: double correctStaticAnglePosition(const Entity& joint, const math::Angle& staticPos, EntityComponentManager& _ecm) {
     auto pos = _ecm.Component<components::JointPosition>(this->joint);
-    if (!pos)
-      return;
-
     const math::Angle currentPos{pos->Data()[0]};
     if (fabs((currentPos - staticPos).Degree()) > 1.0)
     {
       const auto correctingVelocity = this->positionCorrectionGain * (staticPos - currentPos).Radian();
       const auto sanitizedVelocity = math::clamp(correctingVelocity, -this->maxAngularVelocity, this->maxAngularVelocity);
-      auto vel = _ecm.Component<components::JointVelocityCmd>(this->joint);
-      if (!vel)
-      {
-        _ecm.CreateComponent(this->joint, components::JointVelocityCmd({sanitizedVelocity}));
-      }
-      else
-      {
-        *vel = components::JointVelocityCmd({sanitizedVelocity});
-      }
+      return sanitizedVelocity;
     }
+    return 0.0;
   }
 
   protected: void UpdateMaxTorque(const double maxTorque, const EntityComponentManager& _ecm)
@@ -189,15 +186,20 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
     this->cmdVel = _msg.data();
   }
 
-  public: void OnCmdPos(const msgs::Double &_msg)
+  public: void OnCmdPosAbs(const msgs::Double &_msg)
+  {
+    this->cmdPosAbs = _msg.data();
+  }
+
+  public: void OnCmdPosRel(const msgs::Double &_msg)
   {
-    this->cmdPos = _msg.data();
+    this->cmdPosRel = _msg.data();
   }
 
   public: void Reset(EntityComponentManager& _ecm)
   {
     this->angularSpeed = 0;
-    this->staticAngle = ignition::math::Angle(0);
+    this->staticAngle.reset();
 
     if (this->joint != kNullEntity)
     {
@@ -221,13 +223,14 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
   protected: std::string jointName;
   protected: Entity joint{kNullEntity};
   protected: transport::Node node;
-  protected: std::optional<double> cmdPos;
+  protected: std::optional<double> cmdPosAbs;
+  protected: std::optional<double> cmdPosRel;
   protected: std::optional<double> cmdVel;
   protected: std::optional<double> cmdTorque;
-  protected: ignition::math::Angle staticAngle{0.0};
+  protected: std::optional<ignition::math::Angle> staticAngle{0.0};
   protected: double angularSpeed{0.0};
   protected: double maxTorque{30.0};
-  protected: double positionCorrectionGain{0.2};
+  protected: double positionCorrectionGain{20.0};
   protected: double maxAngularVelocity{0.5};
 };
 
@@ -236,7 +239,6 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
 IGNITION_ADD_PLUGIN(cras::FlipperControlPlugin,
                     System,
                     ISystemConfigure,
-                    ISystemPreUpdate,
-                    ISystemPostUpdate)
+                    ISystemPreUpdate)
 
-IGNITION_ADD_PLUGIN_ALIAS(cras::FlipperControlPlugin, "cras::FlipperControlPlugin")
+IGNITION_ADD_PLUGIN_ALIAS(cras::FlipperControlPlugin, "cras::FlipperControlPlugin")
\ No newline at end of file

From 174ddd27a921fd35de7d91d5a91976f5b46f6626 Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Mon, 17 Aug 2020 17:18:15 +0200
Subject: [PATCH 2/7] Absolem: Added a reference to the issue blocking
 implementation of the differential.

---
 .../urdf/nifti_robot.xacro                                     | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/nifti_robot.xacro b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/nifti_robot.xacro
index 18ed7c6b..de2e0e31 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/nifti_robot.xacro
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/nifti_robot.xacro
@@ -1432,7 +1432,8 @@
     </gazebo>
 
     <xacro:if value="${'$(arg rendering_target)' == 'sdf' and $(arg has_differential)}">
-        <xacro:include filename="$(find ctu_cras_norlab_absolem_sensor_config_1)/urdf/differential.xacro" />
+		<!-- No support for closed kinematic chains until https://github.com/ignitionrobotics/ign-physics/issues/25 is fixed -->
+        <!--xacro:include filename="$(find ctu_cras_norlab_absolem_sensor_config_1)/urdf/differential.xacro" /-->
     </xacro:if>
 
     <xacro:if value="$(arg has_flippers)">

From 99be7e570559727886da728968c1cc08972392b7 Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Wed, 10 Feb 2021 16:30:39 +0100
Subject: [PATCH 3/7] Absolem: Read flipper velocity and effort limits from the
 joint limits, too.

---
 .../src/flipper_control_plugin.cpp            | 75 ++++++++++++++-----
 1 file changed, 56 insertions(+), 19 deletions(-)

diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
index 5c3621a5..e249bf27 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
@@ -7,8 +7,11 @@
 
 #include "ignition/gazebo/components/JointPosition.hh"
 #include "ignition/gazebo/components/JointVelocityCmd.hh"
+#include "ignition/gazebo/components/JointAxis.hh"
 #include "ignition/gazebo/Model.hh"
 
+#include <sdf/JointAxis.hh>
+
 using namespace ignition;
 using namespace gazebo;
 using namespace systems;
@@ -33,7 +36,20 @@ namespace cras
 /// messages. This element is optional, and the default value is
 /// `/model/{name_of_model}/joints/{joint_name}/cmd_pos_rel`.
 ///
-/// `<max_velocity>`: Maximum angular velocity of the flipper joint that is used in positional control.
+/// `<max_velocity>`: Maximum angular velocity of the flipper joint that is used in positional and velocity control.
+/// Note that this velocity is also limited by the joint's <limit><velocity> setting. Default is 0.5 rad/s.
+///
+/// `<max_torque>`: Maximum torque the flipper joint can use to reach the given positional or velocity setpoint (or to
+/// hold still in its position when stopped). Note that this torque is also limited by the joint's <limit><effort>
+/// setting. Default is 30 Nm.
+///
+/// `<position_correction_gain>`: The gain used for positional control. The correcting velocity is computed as
+/// gain * (current_position - setpoint) and limited by <max_velocity>. The higher this gain is, the faster will the
+/// flipper reach its positional control setpoint. Default is 20.0.
+///
+/// `<position_correction_tolerance>`: Angular tolerance of positional control (in radians). When the positional error
+/// is lower than this threshold, the controller will stop the flipper and try to keep it at the given position. Default
+/// is 1 degree.
 ///
 /// # Subscriptions
 ///
@@ -60,9 +76,39 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
 
     this->jointName = _sdf->Get<std::string>("joint_name");
 
-    this->maxAngularVelocity = _sdf->Get<double>("max_velocity", this->maxAngularVelocity).first;
-    this->maxTorque = _sdf->Get<double>("max_torque", this->maxTorque).first;
+    this->joint = model.JointByName(_ecm, this->jointName);
+    if (this->joint == kNullEntity)
+    {
+      ignwarn << "FlipperControlPlugin failed to find flipper joint [" << this->jointName << "] for model ["
+              << this->model.Name(_ecm) << "]" << std::endl;
+      return;
+    }
+
+    const auto jointAxisComponent = _ecm.Component<components::JointAxis>(this->joint);
+    const auto jointAxis = jointAxisComponent->Data();
+
+    const auto defaultAngularVelocity = jointAxis.MaxVelocity() > 0 ? jointAxis.MaxVelocity() : this->maxAngularVelocity;
+    this->maxAngularVelocity = _sdf->Get<double>("max_velocity", defaultAngularVelocity).first;
+    if (jointAxis.MaxVelocity() > 0 && this->maxAngularVelocity > jointAxis.MaxVelocity())
+    {
+      ignwarn << "<max_velocity> of joint " << this->jointName << " set to " << this->maxAngularVelocity
+              << " is limited by the joint's <limit><velocity> equal to " << jointAxis.MaxVelocity() << std::endl;
+      this->maxAngularVelocity = jointAxis.MaxVelocity();
+    }
+
+    const auto defaultMaxTorque = jointAxis.Effort() > 0 ? jointAxis.Effort() : this->maxTorque;
+    this->maxTorque = _sdf->Get<double>("max_torque", defaultMaxTorque).first;
+    if (jointAxis.Effort() > 0 && this->maxTorque > jointAxis.Effort())
+    {
+      ignwarn << "<max_torque> of joint " << this->jointName << " set to " << this->maxTorque
+              << " is limited by the joint's <limit><effort> equal to " << jointAxis.Effort() << std::endl;
+      this->maxTorque = jointAxis.Effort();
+    }
+
     this->positionCorrectionGain = _sdf->Get<double>("position_correction_gain", this->positionCorrectionGain).first;
+    this->positionCorrectionTolerance.Radian(
+        _sdf->Get<double>(
+            "position_correction_tolerance", this->positionCorrectionTolerance.Radian()).first);
 
     std::string topicTorque {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_torque"};
     if (_sdf->HasElement("topic_torque"))
@@ -85,7 +131,8 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
     this->node.Subscribe(topicPosRel, &FlipperControlPlugin::OnCmdPosRel, this);
 
     ignmsg << "FlipperControlPlugin subscribing to cmd_vel messages on [" << topicVel << "] and cmd_pos messages on ["
-           << topicPosAbs << "] and cmd_pos_rel messages on [" << topicPosRel << "]" << std::endl;
+           << topicPosAbs << "] and cmd_pos_rel messages on [" << topicPosRel << "]. Maximum joint velocity is "
+           << this->maxAngularVelocity << " rad/s, maximum joint torque is " << this->maxTorque << " Nm." << std::endl;
   }
 
   public: void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override
@@ -99,17 +146,6 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
       return;
     }
 
-    if (this->joint == kNullEntity)
-    {
-      this->joint = this->model.JointByName(_ecm, this->jointName);
-      if (this->joint == kNullEntity)
-      {
-        ignwarn << "Failed to find flipper joint [" << this->jointName << "] for model [" << this->model.Name(_ecm)
-          << "]" << std::endl;
-        return;
-      }
-    }
-
     auto pos = _ecm.Component<components::JointPosition>(this->joint);
     if (!pos)
     {
@@ -145,6 +181,7 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
     auto velocityCommand = this->angularSpeed;
     if (this->staticAngle.has_value())
       velocityCommand = this->correctStaticAnglePosition(this->joint, this->staticAngle.value(), _ecm);
+    velocityCommand = math::clamp(velocityCommand, -this->maxAngularVelocity, this->maxAngularVelocity);
 
     auto vel = _ecm.Component<components::JointVelocityCmd>(this->joint);
     if (!vel)
@@ -159,14 +196,13 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
 
   // To mitigate integrating small velocity errors, if the flipper is said to be stationary, we check that its position
   // does not drift over time.
-protected: double correctStaticAnglePosition(const Entity& joint, const math::Angle& staticPos, EntityComponentManager& _ecm) {
+  protected: double correctStaticAnglePosition(const Entity& joint, const math::Angle& staticPos, EntityComponentManager& _ecm) {
     auto pos = _ecm.Component<components::JointPosition>(this->joint);
     const math::Angle currentPos{pos->Data()[0]};
-    if (fabs((currentPos - staticPos).Degree()) > 1.0)
+    if (fabs((currentPos - staticPos).Radian()) > this->positionCorrectionTolerance.Radian())
     {
       const auto correctingVelocity = this->positionCorrectionGain * (staticPos - currentPos).Radian();
-      const auto sanitizedVelocity = math::clamp(correctingVelocity, -this->maxAngularVelocity, this->maxAngularVelocity);
-      return sanitizedVelocity;
+      return correctingVelocity;
     }
     return 0.0;
   }
@@ -231,6 +267,7 @@ protected: double correctStaticAnglePosition(const Entity& joint, const math::An
   protected: double angularSpeed{0.0};
   protected: double maxTorque{30.0};
   protected: double positionCorrectionGain{20.0};
+  protected: math::Angle positionCorrectionTolerance{math::Angle::Pi / 180.0};  // 1 degree
   protected: double maxAngularVelocity{0.5};
 };
 

From 6dbf214097f79af7c5e3592bf2e549b85b69f8df Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Mon, 17 Aug 2020 18:35:43 +0200
Subject: [PATCH 4/7] Absolem: Preparations for setting max torque in flippers.

---
 .../launch/vehicle_topics.launch              | 28 +++++++++++++++++++
 .../src/flipper_control_plugin.cpp            | 25 +++++++++++++----
 2 files changed, 47 insertions(+), 6 deletions(-)

diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch
index dc792567..15895ca5 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch
@@ -78,6 +78,13 @@
       args="/model/$(arg name)/joint/front_left_flipper_j/cmd_pos_rel@std_msgs/Float64]ignition.msgs.Double">
       <remap from="/model/$(arg name)/joint/front_left_flipper_j/cmd_pos_rel" to="flippers_cmd_pos_rel/front_left"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_front_left_flipper_torque"
+      args="/model/$(arg name)/joint/front_left_flipper_j/cmd_max_torque@std_msgs/Float64]ignition.msgs.Double">
+      <remap from="/model/$(arg name)/joint/front_left_flipper_j/cmd_max_torque" to="flippers_cmd_max_torque/front_left"/>
+    </node>
 
     <node
       pkg="ros_ign_bridge"
@@ -100,6 +107,13 @@
       args="/model/$(arg name)/joint/front_right_flipper_j/cmd_pos_rel@std_msgs/Float64]ignition.msgs.Double">
       <remap from="/model/$(arg name)/joint/front_right_flipper_j/cmd_pos_rel" to="flippers_cmd_pos_rel/front_right"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_front_right_flipper_torque"
+      args="/model/$(arg name)/joint/front_right_flipper_j/cmd_max_torque@std_msgs/Float64]ignition.msgs.Double">
+      <remap from="/model/$(arg name)/joint/front_right_flipper_j/cmd_max_torque" to="flippers_cmd_max_torque/front_right"/>
+    </node>
 
     <node
       pkg="ros_ign_bridge"
@@ -122,6 +136,13 @@
       args="/model/$(arg name)/joint/rear_left_flipper_j/cmd_pos_rel@std_msgs/Float64]ignition.msgs.Double">
       <remap from="/model/$(arg name)/joint/rear_left_flipper_j/cmd_pos_rel" to="flippers_cmd_pos_rel/rear_left"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_rear_left_flipper_torque"
+      args="/model/$(arg name)/joint/rear_left_flipper_j/cmd_max_torque@std_msgs/Float64]ignition.msgs.Double">
+      <remap from="/model/$(arg name)/joint/rear_left_flipper_j/cmd_max_torque" to="flippers_cmd_max_torque/rear_left"/>
+    </node>
 
     <node
       pkg="ros_ign_bridge"
@@ -144,6 +165,13 @@
       args="/model/$(arg name)/joint/rear_right_flipper_j/cmd_pos_rel@std_msgs/Float64]ignition.msgs.Double">
       <remap from="/model/$(arg name)/joint/rear_right_flipper_j/cmd_pos_rel" to="flippers_cmd_pos_rel/rear_right"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_rear_right_flipper_torque"
+      args="/model/$(arg name)/joint/rear_right_flipper_j/cmd_max_torque@std_msgs/Float64]ignition.msgs.Double">
+      <remap from="/model/$(arg name)/joint/rear_right_flipper_j/cmd_max_torque" to="flippers_cmd_max_torque/rear_right"/>
+    </node>
 
     <!-- Laser rotation control -->
     
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
index e249bf27..d67bd4f6 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
@@ -36,12 +36,16 @@ namespace cras
 /// messages. This element is optional, and the default value is
 /// `/model/{name_of_model}/joints/{joint_name}/cmd_pos_rel`.
 ///
+/// `<topic_max_torque>`: Custom topic that this system will subscribe to in order to receive torque limit command
+/// messages. This element is optional, and the default value is
+/// `/model/{name_of_model}/joints/{joint_name}/cmd_max_torque`. This feature is not yet implemented.
+///
 /// `<max_velocity>`: Maximum angular velocity of the flipper joint that is used in positional and velocity control.
 /// Note that this velocity is also limited by the joint's <limit><velocity> setting. Default is 0.5 rad/s.
 ///
 /// `<max_torque>`: Maximum torque the flipper joint can use to reach the given positional or velocity setpoint (or to
 /// hold still in its position when stopped). Note that this torque is also limited by the joint's <limit><effort>
-/// setting. Default is 30 Nm.
+/// setting. Default is 30 Nm. This feature is not yet implemented.
 ///
 /// `<position_correction_gain>`: The gain used for positional control. The correcting velocity is computed as
 /// gain * (current_position - setpoint) and limited by <max_velocity>. The higher this gain is, the faster will the
@@ -56,6 +60,7 @@ namespace cras
 /// `{topic_vel}` (`ignition::msgs::Double`): The desired rotation velocity of the flipper.
 /// `{topic_pos_abs}` (`ignition::msgs::Double`): The positional setpoint of the flipper.
 /// `{topic_pos_rel}` (`ignition::msgs::Double`): Relative positional setpoint of the flipper.
+/// `{topic_max_torque}` (`ignition::msgs::Double`): Maximum torque allowed for the flipper.
 class FlipperControlPlugin : public System, public ISystemConfigure, public ISystemPreUpdate
 {
   public: void Configure(const Entity& _entity, const std::shared_ptr<const sdf::Element>& _sdf,
@@ -110,9 +115,9 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
         _sdf->Get<double>(
             "position_correction_tolerance", this->positionCorrectionTolerance.Radian()).first);
 
-    std::string topicTorque {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_torque"};
-    if (_sdf->HasElement("topic_torque"))
-      topicTorque = _sdf->Get<std::string>("topic_torque");
+    std::string topicTorque {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_max_torque"};
+    if (_sdf->HasElement("topic_max_torque"))
+      topicTorque = _sdf->Get<std::string>("topic_max_torque");
     this->node.Subscribe(topicTorque, &FlipperControlPlugin::OnCmdTorque, this);
 
     std::string topicVel {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_vel"};
@@ -207,9 +212,17 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
     return 0.0;
   }
 
-  protected: void UpdateMaxTorque(const double maxTorque, const EntityComponentManager& _ecm)
+  protected: void UpdateMaxTorque(const double maxTorqueCmd, EntityComponentManager& _ecm)
   {
-    // TODO
+    const auto torque = math::clamp(maxTorqueCmd, 0.0, this->maxTorque);
+    // TODO max effort cannot be changed during run time, waiting for resolution of
+    //  https://github.com/ignitionrobotics/ign-physics/issues/96
+    static bool informed{false};
+    if (!informed)
+    {
+      ignwarn << "FlipperControlPlugin: Max torque commands are not yet supported." << std::endl;
+      informed = true;
+    }
   }
 
   public: void OnCmdTorque(const msgs::Double &_msg)

From fe3369c9a79296080e03193419e80b65704737e5 Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Wed, 10 Feb 2021 17:52:28 +0100
Subject: [PATCH 5/7] Absolem: Simplify interaction with ECM using the new
 APIs.

---
 .../src/ecm_helper.hh                         | 27 ++++++++++
 .../src/flipper_control_plugin.cpp            | 49 ++++++-------------
 .../src/laser_rotate_plugin.cpp               | 36 +++-----------
 3 files changed, 50 insertions(+), 62 deletions(-)
 create mode 100644 submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh

diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh
new file mode 100644
index 00000000..61a7e444
--- /dev/null
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh
@@ -0,0 +1,27 @@
+#ifndef CTU_CRAS_NORLAB_ABSOLEM_SENSOR_CONFIG_1_ECM_HELPER_HH
+#define CTU_CRAS_NORLAB_ABSOLEM_SENSOR_CONFIG_1_ECM_HELPER_HH
+
+#include <ignition/gazebo/EntityComponentManager.hh>
+
+namespace cras
+{
+
+// This is a helper meant to be upstreamed to ign-gazebo
+template<typename ComponentTypeT>
+ComponentTypeT* ComponentDefault(
+  ignition::gazebo::EntityComponentManager& _ecm,
+  const ignition::gazebo::Entity _entity,
+  const typename ComponentTypeT::Type& _default = typename ComponentTypeT::Type())
+{
+  auto comp = _ecm.Component<ComponentTypeT>(_entity);
+  if (!comp)
+  {
+    _ecm.CreateComponent(_entity, ComponentTypeT(_default));
+    comp = _ecm.Component<ComponentTypeT>(_entity);
+  }
+  return comp;
+}
+
+}
+
+#endif //CTU_CRAS_NORLAB_ABSOLEM_SENSOR_CONFIG_1_ECM_HELPER_HH
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
index d67bd4f6..e40ca901 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
@@ -12,6 +12,8 @@
 
 #include <sdf/JointAxis.hh>
 
+#include "ecm_helper.hh"
+
 using namespace ignition;
 using namespace gazebo;
 using namespace systems;
@@ -89,8 +91,7 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
       return;
     }
 
-    const auto jointAxisComponent = _ecm.Component<components::JointAxis>(this->joint);
-    const auto jointAxis = jointAxisComponent->Data();
+    const auto jointAxis = _ecm.Component<components::JointAxis>(this->joint)->Data();
 
     const auto defaultAngularVelocity = jointAxis.MaxVelocity() > 0 ? jointAxis.MaxVelocity() : this->maxAngularVelocity;
     this->maxAngularVelocity = _sdf->Get<double>("max_velocity", defaultAngularVelocity).first;
@@ -135,6 +136,9 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
       topicPosRel = _sdf->Get<std::string>("topic_pos_rel");
     this->node.Subscribe(topicPosRel, &FlipperControlPlugin::OnCmdPosRel, this);
 
+    // cached command for flipper joint velocity; the joint has 1 axis, so this vector needs to hold 1 item
+    this->velocityCommand.push_back(0.0);
+
     ignmsg << "FlipperControlPlugin subscribing to cmd_vel messages on [" << topicVel << "] and cmd_pos messages on ["
            << topicPosAbs << "] and cmd_pos_rel messages on [" << topicPosRel << "]. Maximum joint velocity is "
            << this->maxAngularVelocity << " rad/s, maximum joint torque is " << this->maxTorque << " Nm." << std::endl;
@@ -151,12 +155,7 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
       return;
     }
 
-    auto pos = _ecm.Component<components::JointPosition>(this->joint);
-    if (!pos)
-    {
-      _ecm.CreateComponent(this->joint, components::JointPosition());
-      pos = _ecm.Component<components::JointPosition>(this->joint);
-    }
+    const auto& angle = ComponentDefault<components::JointPosition>(_ecm, this->joint)->Data()[0];
 
     if (this->cmdVel.has_value())
     {
@@ -164,7 +163,7 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
       this->staticAngle.reset();
       if (this->angularSpeed != 0.0 && velocity == 0.0)
       {
-        this->staticAngle = pos->Data()[0];
+        this->staticAngle = angle;
       }
       this->angularSpeed = velocity;
       this->cmdVel.reset();
@@ -173,7 +172,7 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
       this->staticAngle = position;
       this->cmdPosAbs.reset();
     } else if (this->cmdPosRel.has_value()) {
-      const auto position = pos->Data()[0] + this->cmdPosRel.value();
+      const auto position = angle + this->cmdPosRel.value();
       this->staticAngle = position;
       this->cmdPosRel.reset();
     }
@@ -183,20 +182,12 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
       this->cmdTorque.reset();
     }
 
-    auto velocityCommand = this->angularSpeed;
+    this->velocityCommand[0] = this->angularSpeed;
     if (this->staticAngle.has_value())
-      velocityCommand = this->correctStaticAnglePosition(this->joint, this->staticAngle.value(), _ecm);
-    velocityCommand = math::clamp(velocityCommand, -this->maxAngularVelocity, this->maxAngularVelocity);
+      this->velocityCommand[0] = this->correctStaticAnglePosition(this->joint, this->staticAngle.value(), _ecm);
+    this->velocityCommand[0] = math::clamp(this->velocityCommand[0], -this->maxAngularVelocity, this->maxAngularVelocity);
 
-    auto vel = _ecm.Component<components::JointVelocityCmd>(this->joint);
-    if (!vel)
-    {
-      _ecm.CreateComponent(this->joint, components::JointVelocityCmd({velocityCommand}));
-    }
-    else
-    {
-      *vel = components::JointVelocityCmd({velocityCommand});
-    }
+    _ecm.SetComponentData<components::JointVelocityCmd>(this->joint, this->velocityCommand);
   }
 
   // To mitigate integrating small velocity errors, if the flipper is said to be stationary, we check that its position
@@ -252,17 +243,8 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
 
     if (this->joint != kNullEntity)
     {
-      auto pos = _ecm.Component<components::JointPosition>(this->joint);
-      if (pos)
-      {
-        *pos = components::JointPosition({0.0});
-      }
-
-      auto vel = _ecm.Component<components::JointVelocityCmd>(this->joint);
-      if (vel)
-      {
-        *vel = components::JointVelocityCmd({0.0});
-      }
+      _ecm.SetComponentData<components::JointPosition>(this->joint, {0.0});
+      _ecm.SetComponentData<components::JointVelocityCmd>(this->joint, {0.0});
     }
 
     this->UpdateMaxTorque(this->maxTorque, _ecm);
@@ -282,6 +264,7 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
   protected: double positionCorrectionGain{20.0};
   protected: math::Angle positionCorrectionTolerance{math::Angle::Pi / 180.0};  // 1 degree
   protected: double maxAngularVelocity{0.5};
+  protected: std::vector<double> velocityCommand;
 };
 
 }
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/laser_rotate_plugin.cpp b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/laser_rotate_plugin.cpp
index e87f3a96..4998977a 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/laser_rotate_plugin.cpp
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/laser_rotate_plugin.cpp
@@ -8,6 +8,8 @@
 #include "ignition/gazebo/components/JointVelocityCmd.hh"
 #include "ignition/gazebo/Model.hh"
 
+#include "ecm_helper.hh"
+
 using namespace ignition;
 using namespace gazebo;
 using namespace systems;
@@ -89,18 +91,11 @@ class LaserRotatePlugin : public System, public ISystemConfigure, public ISystem
       }
     }
 
-    auto pos = _ecm.Component<components::JointPosition>(this->joint);
-    if (!pos)
-    {
-      _ecm.CreateComponent(this->joint, components::JointPosition());
-      pos = _ecm.Component<components::JointPosition>(this->joint);
-    }
-
-    const auto jointPosition = pos->Data()[0];
+    auto& jointPosition = ComponentDefault<components::JointPosition>(_ecm, this->joint)->Data().at(0);
 
     if (this->rotationVelocitySigned == 0.0)
     {
-      *pos = components::JointPosition({this->staticAngle});
+      jointPosition = this->staticAngle;
     }
     else if (jointPosition >= rotationAngularLimit)
     {
@@ -111,15 +106,7 @@ class LaserRotatePlugin : public System, public ISystemConfigure, public ISystem
       this->rotationVelocitySigned = std::abs(this->rotationVelocitySigned);
     }
 
-    auto vel = _ecm.Component<components::JointVelocityCmd>(this->joint);
-    if (!vel)
-    {
-      _ecm.CreateComponent(this->joint, components::JointVelocityCmd({this->rotationVelocitySigned}));
-    }
-    else
-    {
-       *vel = components::JointVelocityCmd({this->rotationVelocitySigned});
-    }
+    _ecm.SetComponentData<components::JointVelocityCmd>(this->joint, {this->rotationVelocitySigned});
   }
 
   public: void PostUpdate(const UpdateInfo& _info, const EntityComponentManager& _ecm) override
@@ -187,17 +174,8 @@ class LaserRotatePlugin : public System, public ISystemConfigure, public ISystem
 
     if (this->joint != kNullEntity)
     {
-      auto pos = _ecm.Component<components::JointPosition>(this->joint);
-      if (pos)
-      {
-        *pos = components::JointPosition({0.0});
-      }
-
-      auto vel = _ecm.Component<components::JointVelocityCmd>(this->joint);
-      if (vel)
-      {
-        *vel = components::JointVelocityCmd({this->initialScanningSpeed});
-      }
+      _ecm.SetComponentData<components::JointPosition>(this->joint, {0.0});
+      _ecm.SetComponentData<components::JointVelocityCmd>(this->joint, {this->initialScanningSpeed});
     }
   }
 

From 7a51ea109509f4d28ddef304b057ce8cc5304b7f Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Fri, 12 Feb 2021 14:01:59 +0100
Subject: [PATCH 6/7] Absolem: Added TODO referencing a PR in ign-gazebo that
 would allow deleting the ECM shim from this package.

---
 .../ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh    | 1 +
 1 file changed, 1 insertion(+)

diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh
index 61a7e444..1cd8f8cc 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh
@@ -7,6 +7,7 @@ namespace cras
 {
 
 // This is a helper meant to be upstreamed to ign-gazebo
+// TODO(peci1) remove this once https://github.com/ignitionrobotics/ign-gazebo/pull/629 is merged
 template<typename ComponentTypeT>
 ComponentTypeT* ComponentDefault(
   ignition::gazebo::EntityComponentManager& _ecm,

From b33cb5e8f5b468ddb1ee5aaa0341b71589844565 Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Wed, 3 Mar 2021 19:47:13 +0100
Subject: [PATCH 7/7] Simplify the code with
 https://github.com/ignitionrobotics/ign-gazebo/pull/629 .

---
 .../src/ecm_helper.hh                         | 28 -------------------
 .../src/flipper_control_plugin.cpp            |  4 +--
 .../src/laser_rotate_plugin.cpp               |  4 +--
 3 files changed, 2 insertions(+), 34 deletions(-)
 delete mode 100644 submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh

diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh
deleted file mode 100644
index 1cd8f8cc..00000000
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/ecm_helper.hh
+++ /dev/null
@@ -1,28 +0,0 @@
-#ifndef CTU_CRAS_NORLAB_ABSOLEM_SENSOR_CONFIG_1_ECM_HELPER_HH
-#define CTU_CRAS_NORLAB_ABSOLEM_SENSOR_CONFIG_1_ECM_HELPER_HH
-
-#include <ignition/gazebo/EntityComponentManager.hh>
-
-namespace cras
-{
-
-// This is a helper meant to be upstreamed to ign-gazebo
-// TODO(peci1) remove this once https://github.com/ignitionrobotics/ign-gazebo/pull/629 is merged
-template<typename ComponentTypeT>
-ComponentTypeT* ComponentDefault(
-  ignition::gazebo::EntityComponentManager& _ecm,
-  const ignition::gazebo::Entity _entity,
-  const typename ComponentTypeT::Type& _default = typename ComponentTypeT::Type())
-{
-  auto comp = _ecm.Component<ComponentTypeT>(_entity);
-  if (!comp)
-  {
-    _ecm.CreateComponent(_entity, ComponentTypeT(_default));
-    comp = _ecm.Component<ComponentTypeT>(_entity);
-  }
-  return comp;
-}
-
-}
-
-#endif //CTU_CRAS_NORLAB_ABSOLEM_SENSOR_CONFIG_1_ECM_HELPER_HH
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
index e40ca901..4dfd91f4 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
@@ -12,8 +12,6 @@
 
 #include <sdf/JointAxis.hh>
 
-#include "ecm_helper.hh"
-
 using namespace ignition;
 using namespace gazebo;
 using namespace systems;
@@ -155,7 +153,7 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
       return;
     }
 
-    const auto& angle = ComponentDefault<components::JointPosition>(_ecm, this->joint)->Data()[0];
+    const auto& angle = _ecm.ComponentDefault<components::JointPosition>(this->joint)->Data()[0];
 
     if (this->cmdVel.has_value())
     {
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/laser_rotate_plugin.cpp b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/laser_rotate_plugin.cpp
index 4998977a..4c3a8455 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/laser_rotate_plugin.cpp
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/laser_rotate_plugin.cpp
@@ -8,8 +8,6 @@
 #include "ignition/gazebo/components/JointVelocityCmd.hh"
 #include "ignition/gazebo/Model.hh"
 
-#include "ecm_helper.hh"
-
 using namespace ignition;
 using namespace gazebo;
 using namespace systems;
@@ -91,7 +89,7 @@ class LaserRotatePlugin : public System, public ISystemConfigure, public ISystem
       }
     }
 
-    auto& jointPosition = ComponentDefault<components::JointPosition>(_ecm, this->joint)->Data().at(0);
+    auto& jointPosition = _ecm.ComponentDefault<components::JointPosition>(this->joint)->Data().at(0);
 
     if (this->rotationVelocitySigned == 0.0)
     {