From 1136e8ca1d158395f546c0011b6365699b67c469 Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Wed, 2 Jun 2021 18:40:10 +0200
Subject: [PATCH 1/6] Added components to dynamically set joint limits.

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
---
 .../gazebo/components/JointEffortLimitsCmd.hh |  60 +++
 .../components/JointPositionLimitsCmd.hh      |  58 +++
 .../components/JointVelocityLimitsCmd.hh      |  58 +++
 .../gazebo/components/Serialization.hh        |  34 ++
 src/systems/physics/Physics.cc                | 156 ++++++-
 test/integration/physics_system.cc            | 386 ++++++++++++++++++
 test/worlds/revolute_joint_equilibrium.sdf    | 193 +++++++++
 7 files changed, 944 insertions(+), 1 deletion(-)
 create mode 100644 include/ignition/gazebo/components/JointEffortLimitsCmd.hh
 create mode 100644 include/ignition/gazebo/components/JointPositionLimitsCmd.hh
 create mode 100644 include/ignition/gazebo/components/JointVelocityLimitsCmd.hh
 create mode 100644 test/worlds/revolute_joint_equilibrium.sdf

diff --git a/include/ignition/gazebo/components/JointEffortLimitsCmd.hh b/include/ignition/gazebo/components/JointEffortLimitsCmd.hh
new file mode 100644
index 0000000000..b8b9217974
--- /dev/null
+++ b/include/ignition/gazebo/components/JointEffortLimitsCmd.hh
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTEFFORTLIMITSCMD_HH_
+#define IGNITION_GAZEBO_COMPONENTS_JOINTEFFORTLIMITSCMD_HH_
+
+#include <vector>
+#include <ignition/math/Vector2.hh>
+
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Serialization.hh>
+#include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/Export.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+
+namespace components
+{
+
+/// \brief Command for setting effort limits of a joint. Data are a vector
+/// with a Vector2 for each DOF. The X() component of the Vector2 specifies
+/// the minimum effort limit, the Y() component stands for maximum limit.
+/// Set to +-infinity to disable the limits.
+/// \note It is expected that the physics plugin reads this component and
+/// sets the limit to the dynamics engine. After setting it, the data of this
+/// component will be cleared (i.e. the vector will have length zero).
+using JointEffortLimitsCmd = Component<
+  std::vector<ignition::math::Vector2d>,
+  class JointEffortLimitsCmdTag,
+  serializers::VectorSerializer<ignition::math::Vector2d>
+>;
+
+IGN_GAZEBO_REGISTER_COMPONENT(
+  "ign_gazebo_components.JointEffortLimitsCmd", JointEffortLimitsCmd)
+}
+
+}
+}
+}
+
+#endif
diff --git a/include/ignition/gazebo/components/JointPositionLimitsCmd.hh b/include/ignition/gazebo/components/JointPositionLimitsCmd.hh
new file mode 100644
index 0000000000..775937fbff
--- /dev/null
+++ b/include/ignition/gazebo/components/JointPositionLimitsCmd.hh
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONLIMITSCMD_HH_
+#define IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONLIMITSCMD_HH_
+
+#include <vector>
+#include <ignition/math/Vector2.hh>
+
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Serialization.hh>
+#include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/Export.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+
+namespace components
+{
+/// \brief Command for setting position limits of a joint. Data are a vector
+/// with a Vector2 for each DOF. The X() component of the Vector2 specifies
+/// the minimum positional limit, the Y() component stands for maximum limit.
+/// Set to +-infinity to disable the limits.
+/// \note It is expected that the physics plugin reads this component and
+/// sets the limit to the dynamics engine. After setting it, the data of this
+/// component will be cleared (i.e. the vector will have length zero).
+using JointPositionLimitsCmd = Component<
+  std::vector<ignition::math::Vector2d>,
+  class JointPositionLimitsCmdTag,
+  serializers::VectorSerializer<ignition::math::Vector2d>
+>;
+
+IGN_GAZEBO_REGISTER_COMPONENT(
+    "ign_gazebo_components.JointPositionLimitsCmd", JointPositionLimitsCmd)
+}
+}
+}
+}
+
+#endif
diff --git a/include/ignition/gazebo/components/JointVelocityLimitsCmd.hh b/include/ignition/gazebo/components/JointVelocityLimitsCmd.hh
new file mode 100644
index 0000000000..e85905095f
--- /dev/null
+++ b/include/ignition/gazebo/components/JointVelocityLimitsCmd.hh
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYLIMITSCMD_HH_
+#define IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYLIMITSCMD_HH_
+
+#include <vector>
+#include <ignition/math/Vector2.hh>
+
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Serialization.hh>
+#include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/Export.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+
+namespace components
+{
+/// \brief Command for setting velocity limits of a joint. Data are a vector
+/// with a Vector2 for each DOF. The X() component of the Vector2 specifies
+/// the minimum velocity limit, the Y() component stands for maximum limit.
+/// Set to +-infinity to disable the limits.
+/// \note It is expected that the physics plugin reads this component and
+/// sets the limit to the dynamics engine. After setting it, the data of this
+/// component will be cleared (i.e. the vector will have length zero).
+using JointVelocityLimitsCmd = Component<
+  std::vector<ignition::math::Vector2d>,
+  class JointVelocityLimitsCmdTag,
+  serializers::VectorSerializer<ignition::math::Vector2d>
+>;
+
+IGN_GAZEBO_REGISTER_COMPONENT(
+  "ign_gazebo_components.JointVelocityLimitsCmd", JointVelocityLimitsCmd)
+}
+}
+}
+}
+
+#endif
diff --git a/include/ignition/gazebo/components/Serialization.hh b/include/ignition/gazebo/components/Serialization.hh
index 7fd3ee7765..e400d01675 100644
--- a/include/ignition/gazebo/components/Serialization.hh
+++ b/include/ignition/gazebo/components/Serialization.hh
@@ -183,6 +183,40 @@ namespace serializers
       return _in;
     }
   };
+
+  template <typename T>
+  class VectorSerializer
+  {
+    /// \brief Serialization for `std::vector<T>` with serializable T.
+    /// \param[in] _out Output stream.
+    /// \param[in] _data The data to stream.
+    /// \return The stream.
+    public: static std::ostream &Serialize(std::ostream &_out,
+      const std::vector<T> &_data)
+    {
+      _out << _data.size();
+      for (const auto& datum : _data)
+        _out << datum;
+      return _out;
+    }
+
+    /// \brief Deserialization for `std::vector<T>` with serializable T.
+    /// \param[in] _in Input stream.
+    /// \param[out] _data The data to populate.
+    /// \return The stream.
+    public: static std::istream &Deserialize(std::istream &_in,
+      std::vector<T> &_data)
+    {
+      size_t size;
+      _in >> size;
+      _data.resize(size);
+      for (size_t i = 0; i < size; ++i)
+      {
+        _in >> _data[i];
+      }
+      return _in;
+    }
+  };
 }
 }
 }
diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 710a001682..20f0435b24 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -94,11 +94,14 @@
 #include "ignition/gazebo/components/DetachableJoint.hh"
 #include "ignition/gazebo/components/Joint.hh"
 #include "ignition/gazebo/components/JointAxis.hh"
+#include "ignition/gazebo/components/JointEffortLimitsCmd.hh"
 #include "ignition/gazebo/components/JointPosition.hh"
+#include "ignition/gazebo/components/JointPositionLimitsCmd.hh"
 #include "ignition/gazebo/components/JointPositionReset.hh"
 #include "ignition/gazebo/components/JointType.hh"
 #include "ignition/gazebo/components/JointVelocity.hh"
 #include "ignition/gazebo/components/JointVelocityCmd.hh"
+#include "ignition/gazebo/components/JointVelocityLimitsCmd.hh"
 #include "ignition/gazebo/components/JointVelocityReset.hh"
 #include "ignition/gazebo/components/LinearAcceleration.hh"
 #include "ignition/gazebo/components/LinearVelocity.hh"
@@ -337,6 +340,28 @@ class ignition::gazebo::systems::PhysicsPrivate
   public: struct JointVelocityCommandFeatureList : physics::FeatureList<
             physics::SetJointVelocityCommandFeature>{};
 
+
+  //////////////////////////////////////////////////
+  // Joint position limits command
+  /// \brief Feature list for setting joint position limits.
+  public: struct JointPositionLimitsCommandFeatureList : physics::FeatureList<
+            physics::SetJointPositionLimitsCommandFeature>{};
+
+
+  //////////////////////////////////////////////////
+  // Joint velocity limits command
+  /// \brief Feature list for setting joint velocity limits.
+  public: struct JointVelocityLimitsCommandFeatureList : physics::FeatureList<
+            physics::SetJointVelocityLimitsCommandFeature>{};
+
+
+  //////////////////////////////////////////////////
+  // Joint effort limits command
+  /// \brief Feature list for setting joint effort limits.
+  public: struct JointEffortLimitsCommandFeatureList : physics::FeatureList<
+            physics::SetJointEffortLimitsCommandFeature>{};
+
+
   //////////////////////////////////////////////////
   // World velocity command
   public: struct WorldVelocityCommandFeatureList :
@@ -404,7 +429,10 @@ class ignition::gazebo::systems::PhysicsPrivate
             physics::Joint,
             JointFeatureList,
             DetachableJointFeatureList,
-            JointVelocityCommandFeatureList
+            JointVelocityCommandFeatureList,
+            JointPositionLimitsCommandFeatureList,
+            JointVelocityLimitsCommandFeatureList,
+            JointEffortLimitsCommandFeatureList
             >;
 
   /// \brief A map between joint entity ids in the ECM to Joint Entities in
@@ -1227,6 +1255,18 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         if (nullptr == jointPhys)
           return true;
 
+        auto jointPosLimitsFeature =
+          this->entityJointMap.EntityCast<JointPositionLimitsCommandFeatureList>
+              (_entity);
+
+        auto jointVelLimitsFeature =
+          this->entityJointMap.EntityCast<JointVelocityLimitsCommandFeatureList>
+              (_entity);
+
+        auto jointEffLimitsFeature =
+          this->entityJointMap.EntityCast<JointEffortLimitsCommandFeatureList>(
+              _entity);
+
         // Model is out of battery
         if (this->entityOffMap[_ecm.ParentEntity(_entity)])
         {
@@ -1238,6 +1278,99 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
           return true;
         }
 
+        auto posLimits = _ecm.Component<components::JointPositionLimitsCmd>(
+            _entity);
+        if (posLimits && !posLimits->Data().empty())
+        {
+          auto limits = posLimits->Data();
+
+          if (limits.size() != jointPhys->GetDegreesOfFreedom())
+          {
+            ignwarn << "There is a mismatch in the degrees of freedom "
+            << "between Joint [" << _name->Data() << "(Entity="
+            << _entity << ")] and its JointPositionLimitsCmd "
+            << "component. The joint has "
+            << jointPhys->GetDegreesOfFreedom()
+            << " while the component has "
+            << limits.size() << ".\n";
+          }
+
+          if (jointPosLimitsFeature)
+          {
+            std::size_t nDofs = std::min(
+              limits.size(),
+              jointPhys->GetDegreesOfFreedom());
+
+            for (std::size_t i = 0; i < nDofs; ++i)
+            {
+              jointPosLimitsFeature->SetMinPositionCommand(i, limits[i].X());
+              jointPosLimitsFeature->SetMaxPositionCommand(i, limits[i].Y());
+            }
+          }
+        }
+
+        auto velLimits = _ecm.Component<components::JointVelocityLimitsCmd>(
+            _entity);
+        if (velLimits && !velLimits->Data().empty())
+        {
+          auto limits = velLimits->Data();
+
+          if (limits.size() != jointPhys->GetDegreesOfFreedom())
+          {
+            ignwarn << "There is a mismatch in the degrees of freedom "
+            << "between Joint [" << _name->Data() << "(Entity="
+            << _entity << ")] and its JointVelocityLimitsCmd "
+            << "component. The joint has "
+            << jointPhys->GetDegreesOfFreedom()
+            << " while the component has "
+            << limits.size() << ".\n";
+          }
+
+          if (jointVelLimitsFeature)
+          {
+            std::size_t nDofs = std::min(
+              limits.size(),
+              jointPhys->GetDegreesOfFreedom());
+
+            for (std::size_t i = 0; i < nDofs; ++i)
+            {
+              jointVelLimitsFeature->SetMinVelocityCommand(i, limits[i].X());
+              jointVelLimitsFeature->SetMaxVelocityCommand(i, limits[i].Y());
+            }
+          }
+        }
+
+        auto effLimits = _ecm.Component<components::JointEffortLimitsCmd>(
+            _entity);
+        if (effLimits && !effLimits->Data().empty())
+        {
+          auto limits = effLimits->Data();
+
+          if (limits.size() != jointPhys->GetDegreesOfFreedom())
+          {
+            ignwarn << "There is a mismatch in the degrees of freedom "
+            << "between Joint [" << _name->Data() << "(Entity="
+            << _entity << ")] and its JointEffortLimitsCmd "
+            << "component. The joint has "
+            << jointPhys->GetDegreesOfFreedom()
+            << " while the component has "
+            << limits.size() << ".\n";
+          }
+
+          if (jointEffLimitsFeature)
+          {
+            std::size_t nDofs = std::min(
+              limits.size(),
+              jointPhys->GetDegreesOfFreedom());
+
+            for (std::size_t i = 0; i < nDofs; ++i)
+            {
+              jointEffLimitsFeature->SetMinEffortCommand(i, limits[i].X());
+              jointEffLimitsFeature->SetMaxEffortCommand(i, limits[i].Y());
+            }
+          }
+        }
+
         auto posReset = _ecm.Component<components::JointPositionReset>(
             _entity);
         auto velReset = _ecm.Component<components::JointVelocityReset>(
@@ -2169,6 +2302,27 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
         return true;
       });
 
+  _ecm.Each<components::JointPositionLimitsCmd>(
+      [&](const Entity &, components::JointPositionLimitsCmd *_limits) -> bool
+      {
+        _limits->Data().clear();
+        return true;
+      });
+
+  _ecm.Each<components::JointVelocityLimitsCmd>(
+      [&](const Entity &, components::JointVelocityLimitsCmd *_limits) -> bool
+      {
+        _limits->Data().clear();
+        return true;
+      });
+
+  _ecm.Each<components::JointEffortLimitsCmd>(
+      [&](const Entity &, components::JointEffortLimitsCmd *_limits) -> bool
+      {
+        _limits->Data().clear();
+        return true;
+      });
+
   _ecm.Each<components::JointVelocityCmd>(
       [&](const Entity &, components::JointVelocityCmd *_vel) -> bool
       {
diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc
index f6d31c8436..8c8ca3f627 100644
--- a/test/integration/physics_system.cc
+++ b/test/integration/physics_system.cc
@@ -41,9 +41,14 @@
 #include "ignition/gazebo/components/Geometry.hh"
 #include "ignition/gazebo/components/Inertial.hh"
 #include "ignition/gazebo/components/Joint.hh"
+#include "ignition/gazebo/components/JointEffortLimitsCmd.hh"
+#include "ignition/gazebo/components/JointForceCmd.hh"
 #include "ignition/gazebo/components/JointPosition.hh"
+#include "ignition/gazebo/components/JointPositionLimitsCmd.hh"
 #include "ignition/gazebo/components/JointPositionReset.hh"
 #include "ignition/gazebo/components/JointVelocity.hh"
+#include "ignition/gazebo/components/JointVelocityCmd.hh"
+#include "ignition/gazebo/components/JointVelocityLimitsCmd.hh"
 #include "ignition/gazebo/components/JointVelocityReset.hh"
 #include "ignition/gazebo/components/Link.hh"
 #include "ignition/gazebo/components/LinearVelocity.hh"
@@ -806,6 +811,387 @@ TEST_F(PhysicsSystemFixture, ResetVelocityComponent)
   EXPECT_NEAR(vel0, velocities[1], 0.05);
 }
 
+/////////////////////////////////////////////////
+/// Test joint position limit command component
+TEST_F(PhysicsSystemFixture, JointPositionLimitsCommandComponent)
+{
+  ignition::gazebo::ServerConfig serverConfig;
+
+  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+    "/test/worlds/revolute_joint.sdf";
+
+  sdf::Root root;
+  root.Load(sdfFile);
+  const sdf::World *world = root.WorldByIndex(0);
+  ASSERT_TRUE(nullptr != world);
+
+  serverConfig.SetSdfFile(sdfFile);
+
+  gazebo::Server server(serverConfig);
+
+  server.SetUpdatePeriod(1ms);
+
+  const std::string rotatingJointName{"j2"};
+
+  test::Relay testSystem;
+
+  // cppcheck-suppress variableScope
+  size_t iteration = 0u;
+
+  // The system is not in equilibrium at the beginning, so normally, joint j2
+  // would move. For the first 50 ms, we set position limits to 1e-6 so that
+  // it can't freely move, and we check it did not. For the other 50 ms, we
+  // remove the position limit and check that the joint has moved at least by
+  // 1e-2. Between times 30 and 40 ms we also add a 100 N force to the joint to
+  // check that the limit is held even in presence of force commands. Between
+  // times 40 and 50 ms, we add a velocity command to check that velocity
+  // commands do not break the positional limit.
+
+  testSystem.OnPreUpdate(
+    [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm)
+    {
+      _ecm.Each<components::Joint, components::Name>(
+        [&](const ignition::gazebo::Entity &_entity,
+            const components::Joint *, components::Name *_name) -> bool
+        {
+          if (_name->Data() == rotatingJointName)
+          {
+            auto limitComp =
+                _ecm.Component<components::JointPositionLimitsCmd>(_entity);
+
+            if (iteration == 0u)
+            {
+              EXPECT_EQ(nullptr, limitComp);
+              _ecm.CreateComponent(_entity,
+                components::JointPositionLimitsCmd ({{-1e-6, 1e-6}}));
+              _ecm.CreateComponent(_entity, components::JointPosition());
+            }
+            else if (iteration == 50u)
+            {
+              EXPECT_NE(nullptr, limitComp);
+              limitComp->Data() = {{-1e6, 1e6}};
+            }
+            else
+            {
+              EXPECT_NE(nullptr, limitComp);
+              if (limitComp)
+              {
+                EXPECT_EQ(0u, limitComp->Data().size());
+              }
+              if (iteration >= 30u && iteration < 40u)
+              {
+                if (iteration == 30u)
+                  _ecm.CreateComponent(_entity, components::JointForceCmd(
+                    std::vector<double>({100.0})));
+                else
+                  _ecm.Component<components::JointForceCmd>(_entity)->Data() =
+                    {100.0};
+              }
+              else if (iteration >= 40u && iteration < 50u)
+              {
+                if (iteration == 40u)
+                  _ecm.CreateComponent(_entity, components::JointVelocityCmd(
+                    std::vector<double>({1.0})));
+                else
+                  _ecm.Component<components::JointVelocityCmd>(
+                    _entity)->Data() = {1.0};
+              }
+            }
+            ++iteration;
+          }
+          return true;
+        });
+    });
+
+  std::vector<double> positions;
+
+  testSystem.OnPostUpdate([&](
+    const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm)
+    {
+      _ecm.Each<components::Joint,
+                components::Name,
+                components::JointPosition>(
+        [&](const ignition::gazebo::Entity &,
+            const components::Joint *,
+            const components::Name *_name,
+            const components::JointPosition *_pos)
+        {
+          if (_name->Data() == rotatingJointName)
+          {
+            positions.push_back(_pos->Data()[0]);
+          }
+          return true;
+        });
+    });
+
+  server.AddSystem(testSystem.systemPtr);
+  server.Run(true, 100, false);
+
+  ASSERT_EQ(positions.size(), 100ul);
+  // The 1e-6 limit is slightly overcome, but not very much
+  EXPECT_NEAR(positions[0], positions[20], 2e-5);
+  EXPECT_NEAR(positions[0], positions[30], 3e-5);
+  EXPECT_NEAR(positions[0], positions[40], 3e-5);
+  EXPECT_NEAR(positions[0], positions[49], 3e-5);
+  EXPECT_LT(std::abs(positions[50]) + 1e-2, std::abs(positions[99]));
+}
+
+/////////////////////////////////////////////////
+/// Test joint velocity limit command component
+TEST_F(PhysicsSystemFixture, JointVelocityLimitsCommandComponent)
+{
+  ignition::gazebo::ServerConfig serverConfig;
+
+  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+    "/test/worlds/revolute_joint.sdf";
+
+  sdf::Root root;
+  root.Load(sdfFile);
+  const sdf::World *world = root.WorldByIndex(0);
+  ASSERT_TRUE(nullptr != world);
+
+  serverConfig.SetSdfFile(sdfFile);
+
+  gazebo::Server server(serverConfig);
+
+  server.SetUpdatePeriod(1ms);
+
+  const std::string rotatingJointName{"j2"};
+
+  test::Relay testSystem;
+
+  // cppcheck-suppress variableScope
+  size_t iteration = 0u;
+
+  // The system is not in equilibrium at the beginning, so normally, joint j2
+  // would move. For the first 50 ms, we set velocity limits to 0.1 so that
+  // it can't move very fast, and we check it does not. For the other 50 ms, we
+  // remove the velocity limit and check that the joint has moved faster.
+  // Between times 30 and 40 ms we also add a 100 N force to the joint to
+  // check that the limit is held even in presence of force commands. Between
+  // times 40 and 50 ms, we add a velocity command to check that velocity
+  // commands do not break the velocity limit.
+
+  testSystem.OnPreUpdate(
+    [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm)
+    {
+      _ecm.Each<components::Joint, components::Name>(
+        [&](const ignition::gazebo::Entity &_entity,
+            const components::Joint *, components::Name *_name) -> bool
+        {
+          if (_name->Data() == rotatingJointName)
+          {
+            auto limitComp =
+                _ecm.Component<components::JointVelocityLimitsCmd>(_entity);
+
+            if (iteration == 0u)
+            {
+              EXPECT_EQ(nullptr, limitComp);
+              _ecm.CreateComponent(_entity,
+                components::JointVelocityLimitsCmd ({{-0.1, 0.1}}));
+              _ecm.CreateComponent(_entity, components::JointVelocity());
+            }
+            else if (iteration == 50u)
+            {
+              EXPECT_NE(nullptr, limitComp);
+              limitComp->Data() = {{-1e6, 1e6}};
+            }
+            else
+            {
+              EXPECT_NE(nullptr, limitComp);
+              if (limitComp)
+              {
+                EXPECT_EQ(0u, limitComp->Data().size());
+              }
+              if (iteration >= 30u && iteration < 40u)
+              {
+                if (iteration == 30u)
+                  _ecm.CreateComponent(_entity, components::JointForceCmd(
+                    std::vector<double>({100.0})));
+                else
+                  _ecm.Component<components::JointForceCmd>(_entity)->Data() =
+                    {100.0};
+              }
+              else if (iteration >= 40u && iteration < 50u)
+              {
+                if (iteration == 40u)
+                  _ecm.CreateComponent(_entity, components::JointVelocityCmd(
+                    std::vector<double>({1.0})));
+                else
+                  _ecm.Component<components::JointVelocityCmd>(
+                    _entity)->Data() = {1.0};
+              }
+            }
+            ++iteration;
+          }
+          return true;
+        });
+    });
+
+  std::vector<double> velocities;
+
+  testSystem.OnPostUpdate([&](
+    const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm)
+    {
+      _ecm.Each<components::Joint,
+                components::Name,
+                components::JointVelocity>(
+        [&](const ignition::gazebo::Entity &,
+            const components::Joint *,
+            const components::Name *_name,
+            const components::JointVelocity *_vel)
+        {
+          if (_name->Data() == rotatingJointName)
+          {
+            velocities.push_back(_vel->Data()[0]);
+          }
+          return true;
+        });
+    });
+
+  server.AddSystem(testSystem.systemPtr);
+  server.Run(true, 100, false);
+
+  ASSERT_EQ(velocities.size(), 100ul);
+  // The 0.1 limit is slightly overcome, but not very much
+  EXPECT_NEAR(0.1, velocities[20], 1e-2);
+  EXPECT_NEAR(0.1, velocities[30], 1e-2);
+  EXPECT_NEAR(0.1, velocities[40], 1e-2);
+  EXPECT_NEAR(0.1, velocities[49], 1e-2);
+  EXPECT_LT(0.5, std::abs(velocities[99]));
+}
+
+
+/////////////////////////////////////////////////
+/// Test joint effort limit command component
+TEST_F(PhysicsSystemFixture, JointEffortLimitsCommandComponent)
+{
+  ignition::gazebo::ServerConfig serverConfig;
+
+  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+    "/test/worlds/revolute_joint_equilibrium.sdf";
+
+  sdf::Root root;
+  root.Load(sdfFile);
+  const sdf::World *world = root.WorldByIndex(0);
+  ASSERT_TRUE(nullptr != world);
+
+  serverConfig.SetSdfFile(sdfFile);
+
+  gazebo::Server server(serverConfig);
+
+  server.SetUpdatePeriod(1ms);
+
+  const std::string rotatingJointName{"j2"};
+
+  test::Relay testSystem;
+
+  // cppcheck-suppress variableScope
+  size_t iteration = 0u;
+
+  // The system is in equilibrium at the beginning.
+  // For the first 50 ms, we set effort limits to 1e-6 so that
+  // it can't move, and we check it does not. For the other 50 ms, we
+  // remove the effort limit and check that the joint has moved.
+  // Between times 30 and 40 ms we also add a 100 N force to the joint to
+  // check that the limit is held even in presence of force commands. Between
+  // times 40 and 50 ms, we add a velocity command to check that velocity
+  // commands do not break the effort limit.
+
+  testSystem.OnPreUpdate(
+    [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm)
+    {
+      _ecm.Each<components::Joint, components::Name>(
+        [&](const ignition::gazebo::Entity &_entity,
+            const components::Joint *, components::Name *_name) -> bool
+        {
+          if (_name->Data() == rotatingJointName)
+          {
+            auto limitComp =
+                _ecm.Component<components::JointEffortLimitsCmd>(_entity);
+
+            if (iteration == 0u)
+            {
+              EXPECT_EQ(nullptr, limitComp);
+              _ecm.CreateComponent(_entity,
+                components::JointEffortLimitsCmd ({{-1e-6, 1e-6}}));
+              _ecm.CreateComponent(_entity, components::JointPosition());
+            }
+            else if (iteration == 50u)
+            {
+              EXPECT_NE(nullptr, limitComp);
+              limitComp->Data() = {{-1e9, 1e9}};
+            }
+            else
+            {
+              EXPECT_NE(nullptr, limitComp);
+              if (limitComp)
+              {
+                EXPECT_EQ(0u, limitComp->Data().size());
+              }
+              if (iteration >= 30u && iteration < 40u)
+              {
+                if (iteration == 30u)
+                  _ecm.CreateComponent(_entity, components::JointForceCmd(
+                    std::vector<double>({100.0})));
+                else
+                  _ecm.Component<components::JointForceCmd>(_entity)->Data() =
+                    {100.0};
+              }
+              else if (iteration >= 40u && iteration < 50u)
+              {
+                if (iteration == 40u)
+                  _ecm.CreateComponent(_entity, components::JointVelocityCmd(
+                    std::vector<double>({1.0})));
+                else
+                  _ecm.Component<components::JointVelocityCmd>(
+                    _entity)->Data() = {1.0};
+              }
+              else if (iteration >= 50u)
+              {
+                _ecm.Component<components::JointForceCmd>(_entity)->Data() =
+                  {1000.0};
+              }
+            }
+            ++iteration;
+          }
+          return true;
+        });
+    });
+
+  std::vector<double> positions;
+
+  testSystem.OnPostUpdate([&](
+    const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm)
+    {
+    _ecm.Each<components::Joint,
+    components::Name,
+    components::JointPosition>(
+      [&](const ignition::gazebo::Entity &,
+        const components::Joint *,
+        const components::Name *_name,
+        const components::JointPosition *_pos)
+        {
+        if (_name->Data() == rotatingJointName)
+        {
+          positions.push_back(_pos->Data()[0]);
+        }
+        return true;
+        });
+    });
+
+  server.AddSystem(testSystem.systemPtr);
+  server.Run(true, 100, false);
+
+  ASSERT_EQ(positions.size(), 100ul);
+  // The 1e-6 limit is slightly overcome, but not very much
+  EXPECT_NEAR(positions[0], positions[20], 2e-5);
+  EXPECT_NEAR(positions[0], positions[30], 3e-5);
+  EXPECT_NEAR(positions[0], positions[40], 3e-5);
+  EXPECT_NEAR(positions[0], positions[49], 3e-5);
+  EXPECT_LT(std::abs(positions[50]) + 1e-2, std::abs(positions[99]));
+}
+
 /////////////////////////////////////////////////
 TEST_F(PhysicsSystemFixture, GetBoundingBox)
 {
diff --git a/test/worlds/revolute_joint_equilibrium.sdf b/test/worlds/revolute_joint_equilibrium.sdf
new file mode 100644
index 0000000000..cf967f17b3
--- /dev/null
+++ b/test/worlds/revolute_joint_equilibrium.sdf
@@ -0,0 +1,193 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+
+    <gui fullscreen="0">
+
+      <!-- 3D scene -->
+      <plugin filename="GzScene3D" name="3D View">
+        <ignition-gui>
+          <title>3D View</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="z">0</property>
+          <anchor line="right" target="window" target_line="right"/>
+          <anchor line="left" target="window" target_line="left"/>
+          <anchor line="top" target="window" target_line="top"/>
+          <anchor line="bottom" target="window" target_line="bottom"/>
+        </ignition-gui>
+
+        <engine>ogre</engine>
+        <scene>scene</scene>
+        <ambient_light>0.4 0.4 0.4</ambient_light>
+        <background_color>0.8 0.8 0.8</background_color>
+        <camera_pose>-1 0 1 0 0.5 0</camera_pose>
+      </plugin>
+
+      <!-- World control -->
+      <plugin filename="WorldControl" name="World control">
+        <ignition-gui>
+          <title>World control</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">72</property>
+          <property type="double" key="width">121</property>
+          <property type="double" key="z">1</property>
+          <anchor line="left" target="window" target_line="left"/>
+          <anchor line="bottom" target="window" target_line="bottom"/>
+        </ignition-gui>
+
+        <play_pause>true</play_pause>
+        <step>true</step>
+        <start_paused>true</start_paused>
+
+      </plugin>
+    </gui>
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+    <model name="revolute_demo">
+      <pose>0 0 0 0 0 0</pose>
+      <link name="base_link">
+        <pose>0.0 0.0 0.0 0 0 0</pose>
+        <inertial>
+          <inertia>
+            <ixx>2.501</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>2.501</iyy>
+            <iyz>0</iyz>
+            <izz>5</izz>
+          </inertia>
+          <mass>120.0</mass>
+        </inertial>
+        <visual name="base_visual">
+          <pose>0.0 0.0 0.0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.5 0.5 0.01</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name="base_collision">
+          <pose>0.0 0.0 0.0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.5 0.5 0.01</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+      <link name="link1">
+        <pose>0.0 0.0 -0.2 0 0 0</pose>
+        <inertial>
+          <inertia>
+            <ixx>0.032</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.032</iyy>
+            <iyz>0</iyz>
+            <izz>0.00012</izz>
+          </inertia>
+          <mass>0.6</mass>
+        </inertial>
+        <visual name="visual1">
+          <geometry>
+            <cylinder>
+              <length>0.4</length>
+              <radius>0.02</radius>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>1 1 1 1</ambient>
+            <diffuse>1 1 1 1</diffuse>
+            <specular>1 1 1 1</specular>
+          </material>
+        </visual>
+        <collision name="collision">
+          <geometry>
+            <cylinder>
+              <length>0.4</length>
+              <radius>0.02</radius>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+      <joint name="j1" type="fixed">
+        <parent>base_link</parent>
+        <child>link1</child>
+      </joint>
+      <link name="link2">
+        <pose>0 0 -0.55 0 0 0</pose>
+        <inertial>
+          <pose>0.0 0.0 0.0 0 0 0</pose>
+          <inertia>
+            <ixx>0.032</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.032</iyy>
+            <iyz>0</iyz>
+            <izz>0.00012</izz>
+          </inertia>
+          <mass>0.6</mass>
+        </inertial>
+        <visual name="visual">
+          <geometry>
+            <cylinder>
+              <length>0.3</length>
+              <radius>0.02</radius>
+            </cylinder>
+          </geometry>
+        </visual>
+        <collision name="collision">
+          <geometry>
+            <cylinder>
+              <length>0.3</length>
+              <radius>0.02</radius>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <joint name="j2" type="revolute">
+        <pose>0 0 0.15 0 0 0</pose>
+        <parent>link1</parent>
+        <child>link2</child>
+        <axis>
+          <xyz>1 0 0</xyz>
+        </axis>
+      </joint>
+    </model>
+  </world>
+</sdf>

From ead9f031b92bfa6845117ba7b6e9b3ffccfe07c1 Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Tue, 13 Jul 2021 10:48:48 +0200
Subject: [PATCH 2/6] Fixed issues from review.

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
---
 .../gazebo/components/Serialization.hh        |   2 +-
 src/systems/physics/Physics.cc                |  24 ++--
 test/integration/components.cc                | 105 ++++++++++++++++++
 test/integration/physics_system.cc            |  90 +++++++--------
 4 files changed, 160 insertions(+), 61 deletions(-)

diff --git a/include/ignition/gazebo/components/Serialization.hh b/include/ignition/gazebo/components/Serialization.hh
index e400d01675..151af91adc 100644
--- a/include/ignition/gazebo/components/Serialization.hh
+++ b/include/ignition/gazebo/components/Serialization.hh
@@ -196,7 +196,7 @@ namespace serializers
     {
       _out << _data.size();
       for (const auto& datum : _data)
-        _out << datum;
+        _out << " " << datum;
       return _out;
     }
 
diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 20f0435b24..0b570793fe 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -345,21 +345,21 @@ class ignition::gazebo::systems::PhysicsPrivate
   // Joint position limits command
   /// \brief Feature list for setting joint position limits.
   public: struct JointPositionLimitsCommandFeatureList : physics::FeatureList<
-            physics::SetJointPositionLimitsCommandFeature>{};
+            physics::SetJointPositionLimitsFeature>{};
 
 
   //////////////////////////////////////////////////
   // Joint velocity limits command
   /// \brief Feature list for setting joint velocity limits.
   public: struct JointVelocityLimitsCommandFeatureList : physics::FeatureList<
-            physics::SetJointVelocityLimitsCommandFeature>{};
+            physics::SetJointVelocityLimitsFeature>{};
 
 
   //////////////////////////////////////////////////
   // Joint effort limits command
   /// \brief Feature list for setting joint effort limits.
   public: struct JointEffortLimitsCommandFeatureList : physics::FeatureList<
-            physics::SetJointEffortLimitsCommandFeature>{};
+            physics::SetJointEffortLimitsFeature>{};
 
 
   //////////////////////////////////////////////////
@@ -1282,7 +1282,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
             _entity);
         if (posLimits && !posLimits->Data().empty())
         {
-          auto limits = posLimits->Data();
+          const auto& limits = posLimits->Data();
 
           if (limits.size() != jointPhys->GetDegreesOfFreedom())
           {
@@ -1303,8 +1303,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
 
             for (std::size_t i = 0; i < nDofs; ++i)
             {
-              jointPosLimitsFeature->SetMinPositionCommand(i, limits[i].X());
-              jointPosLimitsFeature->SetMaxPositionCommand(i, limits[i].Y());
+              jointPosLimitsFeature->SetMinPosition(i, limits[i].X());
+              jointPosLimitsFeature->SetMaxPosition(i, limits[i].Y());
             }
           }
         }
@@ -1313,7 +1313,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
             _entity);
         if (velLimits && !velLimits->Data().empty())
         {
-          auto limits = velLimits->Data();
+          const auto& limits = velLimits->Data();
 
           if (limits.size() != jointPhys->GetDegreesOfFreedom())
           {
@@ -1334,8 +1334,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
 
             for (std::size_t i = 0; i < nDofs; ++i)
             {
-              jointVelLimitsFeature->SetMinVelocityCommand(i, limits[i].X());
-              jointVelLimitsFeature->SetMaxVelocityCommand(i, limits[i].Y());
+              jointVelLimitsFeature->SetMinVelocity(i, limits[i].X());
+              jointVelLimitsFeature->SetMaxVelocity(i, limits[i].Y());
             }
           }
         }
@@ -1344,7 +1344,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
             _entity);
         if (effLimits && !effLimits->Data().empty())
         {
-          auto limits = effLimits->Data();
+          const auto& limits = effLimits->Data();
 
           if (limits.size() != jointPhys->GetDegreesOfFreedom())
           {
@@ -1365,8 +1365,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
 
             for (std::size_t i = 0; i < nDofs; ++i)
             {
-              jointEffLimitsFeature->SetMinEffortCommand(i, limits[i].X());
-              jointEffLimitsFeature->SetMaxEffortCommand(i, limits[i].Y());
+              jointEffLimitsFeature->SetMinEffort(i, limits[i].X());
+              jointEffLimitsFeature->SetMaxEffort(i, limits[i].Y());
             }
           }
         }
diff --git a/test/integration/components.cc b/test/integration/components.cc
index 3991468b6b..4349044950 100644
--- a/test/integration/components.cc
+++ b/test/integration/components.cc
@@ -45,6 +45,9 @@
 #include "ignition/gazebo/components/Inertial.hh"
 #include "ignition/gazebo/components/Joint.hh"
 #include "ignition/gazebo/components/JointAxis.hh"
+#include "ignition/gazebo/components/JointEffortLimitsCmd.hh"
+#include "ignition/gazebo/components/JointPositionLimitsCmd.hh"
+#include "ignition/gazebo/components/JointVelocityLimitsCmd.hh"
 #include "ignition/gazebo/components/JointType.hh"
 #include "ignition/gazebo/components/JointVelocity.hh"
 #include "ignition/gazebo/components/JointVelocityCmd.hh"
@@ -530,6 +533,108 @@ TEST_F(ComponentsTest, JointAxis)
   EXPECT_EQ(comp3.Data().XyzExpressedIn(), "__model__");
 }
 
+/////////////////////////////////////////////////
+TEST_F(ComponentsTest, JointEffortLimitsCmd)
+{
+  // Create components
+  auto comp1 = components::JointEffortLimitsCmd();
+  auto comp2 = components::JointEffortLimitsCmd();
+
+  // Equality operators
+  EXPECT_EQ(comp1, comp2);
+  EXPECT_TRUE(comp1 == comp2);
+  EXPECT_FALSE(comp1 != comp2);
+
+  // Stream operators
+  std::ostringstream ostr;
+  comp1.Serialize(ostr);
+  EXPECT_EQ("0", ostr.str());
+
+  comp2.Data().push_back(math::Vector2d(-1.0, 1.0));
+
+  std::ostringstream ostr2;
+  comp2.Serialize(ostr2);
+  EXPECT_EQ("1 -1 1", ostr2.str());
+
+  comp2.Data().push_back(math::Vector2d(-2.5, 2.5));
+
+  std::ostringstream ostr3;
+  comp2.Serialize(ostr3);
+  EXPECT_EQ("2 -1 1 -2.5 2.5", ostr3.str());
+
+  std::istringstream istr("ignored");
+  components::JointEffortLimitsCmd comp3;
+  comp3.Deserialize(istr);
+}
+
+/////////////////////////////////////////////////
+TEST_F(ComponentsTest, JointPositionLimitsCmd)
+{
+  // Create components
+  auto comp1 = components::JointPositionLimitsCmd();
+  auto comp2 = components::JointPositionLimitsCmd();
+
+  // Equality operators
+  EXPECT_EQ(comp1, comp2);
+  EXPECT_TRUE(comp1 == comp2);
+  EXPECT_FALSE(comp1 != comp2);
+
+  // Stream operators
+  std::ostringstream ostr;
+  comp1.Serialize(ostr);
+  EXPECT_EQ("0", ostr.str());
+
+  comp2.Data().push_back(math::Vector2d(-1.0, 1.0));
+
+  std::ostringstream ostr2;
+  comp2.Serialize(ostr2);
+  EXPECT_EQ("1 -1 1", ostr2.str());
+
+  comp2.Data().push_back(math::Vector2d(-2.5, 2.5));
+
+  std::ostringstream ostr3;
+  comp2.Serialize(ostr3);
+  EXPECT_EQ("2 -1 1 -2.5 2.5", ostr3.str());
+
+  std::istringstream istr("ignored");
+  components::JointPositionLimitsCmd comp3;
+  comp3.Deserialize(istr);
+}
+
+/////////////////////////////////////////////////
+TEST_F(ComponentsTest, JointVelocityLimitsCmd)
+{
+  // Create components
+  auto comp1 = components::JointVelocityLimitsCmd();
+  auto comp2 = components::JointVelocityLimitsCmd();
+
+  // Equality operators
+  EXPECT_EQ(comp1, comp2);
+  EXPECT_TRUE(comp1 == comp2);
+  EXPECT_FALSE(comp1 != comp2);
+
+  // Stream operators
+  std::ostringstream ostr;
+  comp1.Serialize(ostr);
+  EXPECT_EQ("0", ostr.str());
+
+  comp2.Data().push_back(math::Vector2d(-1.0, 1.0));
+
+  std::ostringstream ostr2;
+  comp2.Serialize(ostr2);
+  EXPECT_EQ("1 -1 1", ostr2.str());
+
+  comp2.Data().push_back(math::Vector2d(-2.5, 2.5));
+
+  std::ostringstream ostr3;
+  comp2.Serialize(ostr3);
+  EXPECT_EQ("2 -1 1 -2.5 2.5", ostr3.str());
+
+  std::istringstream istr("ignored");
+  components::JointVelocityLimitsCmd comp3;
+  comp3.Deserialize(istr);
+}
+
 /////////////////////////////////////////////////
 TEST_F(ComponentsTest, JointType)
 {
diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc
index 8c8ca3f627..1837bb37b5 100644
--- a/test/integration/physics_system.cc
+++ b/test/integration/physics_system.cc
@@ -856,11 +856,10 @@ TEST_F(PhysicsSystemFixture, JointPositionLimitsCommandComponent)
         {
           if (_name->Data() == rotatingJointName)
           {
-            auto limitComp =
-                _ecm.Component<components::JointPositionLimitsCmd>(_entity);
-
             if (iteration == 0u)
             {
+              auto limitComp =
+                _ecm.Component<components::JointPositionLimitsCmd>(_entity);
               EXPECT_EQ(nullptr, limitComp);
               _ecm.CreateComponent(_entity,
                 components::JointPositionLimitsCmd ({{-1e-6, 1e-6}}));
@@ -868,11 +867,18 @@ TEST_F(PhysicsSystemFixture, JointPositionLimitsCommandComponent)
             }
             else if (iteration == 50u)
             {
+              auto limitComp =
+                _ecm.Component<components::JointPositionLimitsCmd>(_entity);
               EXPECT_NE(nullptr, limitComp);
-              limitComp->Data() = {{-1e6, 1e6}};
+              if (limitComp)
+              {
+                limitComp->Data() = {{-1e6, 1e6}};
+              }
             }
             else
             {
+              auto limitComp =
+                _ecm.Component<components::JointPositionLimitsCmd>(_entity);
               EXPECT_NE(nullptr, limitComp);
               if (limitComp)
               {
@@ -880,21 +886,13 @@ TEST_F(PhysicsSystemFixture, JointPositionLimitsCommandComponent)
               }
               if (iteration >= 30u && iteration < 40u)
               {
-                if (iteration == 30u)
-                  _ecm.CreateComponent(_entity, components::JointForceCmd(
-                    std::vector<double>({100.0})));
-                else
-                  _ecm.Component<components::JointForceCmd>(_entity)->Data() =
-                    {100.0};
+                _ecm.SetComponentData<components::JointForceCmd>(
+                  _entity, {100.0});
               }
               else if (iteration >= 40u && iteration < 50u)
               {
-                if (iteration == 40u)
-                  _ecm.CreateComponent(_entity, components::JointVelocityCmd(
-                    std::vector<double>({1.0})));
-                else
-                  _ecm.Component<components::JointVelocityCmd>(
-                    _entity)->Data() = {1.0};
+                _ecm.SetComponentData<components::JointVelocityCmd>(
+                  _entity, {1.0});
               }
             }
             ++iteration;
@@ -981,11 +979,10 @@ TEST_F(PhysicsSystemFixture, JointVelocityLimitsCommandComponent)
         {
           if (_name->Data() == rotatingJointName)
           {
-            auto limitComp =
-                _ecm.Component<components::JointVelocityLimitsCmd>(_entity);
-
             if (iteration == 0u)
             {
+              auto limitComp =
+                _ecm.Component<components::JointVelocityLimitsCmd>(_entity);
               EXPECT_EQ(nullptr, limitComp);
               _ecm.CreateComponent(_entity,
                 components::JointVelocityLimitsCmd ({{-0.1, 0.1}}));
@@ -993,11 +990,18 @@ TEST_F(PhysicsSystemFixture, JointVelocityLimitsCommandComponent)
             }
             else if (iteration == 50u)
             {
+              auto limitComp =
+                _ecm.Component<components::JointVelocityLimitsCmd>(_entity);
               EXPECT_NE(nullptr, limitComp);
-              limitComp->Data() = {{-1e6, 1e6}};
+              if (limitComp)
+              {
+                limitComp->Data() = {{-1e6, 1e6}};
+              }
             }
             else
             {
+              auto limitComp =
+                _ecm.Component<components::JointVelocityLimitsCmd>(_entity);
               EXPECT_NE(nullptr, limitComp);
               if (limitComp)
               {
@@ -1005,21 +1009,13 @@ TEST_F(PhysicsSystemFixture, JointVelocityLimitsCommandComponent)
               }
               if (iteration >= 30u && iteration < 40u)
               {
-                if (iteration == 30u)
-                  _ecm.CreateComponent(_entity, components::JointForceCmd(
-                    std::vector<double>({100.0})));
-                else
-                  _ecm.Component<components::JointForceCmd>(_entity)->Data() =
-                    {100.0};
+                _ecm.SetComponentData<components::JointForceCmd>(
+                  _entity, {100.0});
               }
               else if (iteration >= 40u && iteration < 50u)
               {
-                if (iteration == 40u)
-                  _ecm.CreateComponent(_entity, components::JointVelocityCmd(
-                    std::vector<double>({1.0})));
-                else
-                  _ecm.Component<components::JointVelocityCmd>(
-                    _entity)->Data() = {1.0};
+                _ecm.SetComponentData<components::JointVelocityCmd>(
+                  _entity, {1.0});
               }
             }
             ++iteration;
@@ -1107,11 +1103,10 @@ TEST_F(PhysicsSystemFixture, JointEffortLimitsCommandComponent)
         {
           if (_name->Data() == rotatingJointName)
           {
-            auto limitComp =
-                _ecm.Component<components::JointEffortLimitsCmd>(_entity);
-
             if (iteration == 0u)
             {
+              auto limitComp =
+                _ecm.Component<components::JointEffortLimitsCmd>(_entity);
               EXPECT_EQ(nullptr, limitComp);
               _ecm.CreateComponent(_entity,
                 components::JointEffortLimitsCmd ({{-1e-6, 1e-6}}));
@@ -1119,11 +1114,18 @@ TEST_F(PhysicsSystemFixture, JointEffortLimitsCommandComponent)
             }
             else if (iteration == 50u)
             {
+              auto limitComp =
+                _ecm.Component<components::JointEffortLimitsCmd>(_entity);
               EXPECT_NE(nullptr, limitComp);
-              limitComp->Data() = {{-1e9, 1e9}};
+              if (limitComp)
+              {
+                limitComp->Data() = {{-1e9, 1e9}};
+              }
             }
             else
             {
+              auto limitComp =
+                _ecm.Component<components::JointEffortLimitsCmd>(_entity);
               EXPECT_NE(nullptr, limitComp);
               if (limitComp)
               {
@@ -1131,21 +1133,13 @@ TEST_F(PhysicsSystemFixture, JointEffortLimitsCommandComponent)
               }
               if (iteration >= 30u && iteration < 40u)
               {
-                if (iteration == 30u)
-                  _ecm.CreateComponent(_entity, components::JointForceCmd(
-                    std::vector<double>({100.0})));
-                else
-                  _ecm.Component<components::JointForceCmd>(_entity)->Data() =
-                    {100.0};
+                _ecm.SetComponentData<components::JointForceCmd>(
+                  _entity, {100.0});
               }
               else if (iteration >= 40u && iteration < 50u)
               {
-                if (iteration == 40u)
-                  _ecm.CreateComponent(_entity, components::JointVelocityCmd(
-                    std::vector<double>({1.0})));
-                else
-                  _ecm.Component<components::JointVelocityCmd>(
-                    _entity)->Data() = {1.0};
+                _ecm.SetComponentData<components::JointVelocityCmd>(
+                  _entity, {1.0});
               }
               else if (iteration >= 50u)
               {

From 82019c3709365e9ed27c5081ffcc2bcd27048cda Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Fri, 23 Jul 2021 18:15:51 +0200
Subject: [PATCH 3/6] Added deserialize test

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
---
 test/integration/components.cc | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/test/integration/components.cc b/test/integration/components.cc
index 4349044950..8e0bc599dd 100644
--- a/test/integration/components.cc
+++ b/test/integration/components.cc
@@ -573,6 +573,7 @@ TEST_F(ComponentsTest, JointPositionLimitsCmd)
   // Create components
   auto comp1 = components::JointPositionLimitsCmd();
   auto comp2 = components::JointPositionLimitsCmd();
+  components::JointPositionLimitsCmd comp3;
 
   // Equality operators
   EXPECT_EQ(comp1, comp2);
@@ -584,21 +585,33 @@ TEST_F(ComponentsTest, JointPositionLimitsCmd)
   comp1.Serialize(ostr);
   EXPECT_EQ("0", ostr.str());
 
+  auto istr = std::istringstream(ostr.str());
+  comp3.Deserialize(istr);
+  EXPECT_EQ(comp1, comp3);
+
   comp2.Data().push_back(math::Vector2d(-1.0, 1.0));
 
   std::ostringstream ostr2;
   comp2.Serialize(ostr2);
   EXPECT_EQ("1 -1 1", ostr2.str());
 
+  istr = std::istringstream(ostr2.str());
+  comp3.Deserialize(istr);
+  EXPECT_EQ(comp2, comp3);
+
   comp2.Data().push_back(math::Vector2d(-2.5, 2.5));
 
   std::ostringstream ostr3;
   comp2.Serialize(ostr3);
   EXPECT_EQ("2 -1 1 -2.5 2.5", ostr3.str());
 
-  std::istringstream istr("ignored");
-  components::JointPositionLimitsCmd comp3;
+  istr = std::istringstream(ostr3.str());
   comp3.Deserialize(istr);
+  EXPECT_EQ(comp2, comp3);
+
+  istr = std::istringstream("ignored");
+  comp3.Deserialize(istr);
+  EXPECT_EQ(comp1, comp3);
 }
 
 /////////////////////////////////////////////////

From 676112599fbe08649ac68dd87ccbd6a748755885 Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Sun, 31 Oct 2021 22:18:08 +0100
Subject: [PATCH 4/6] Increased required ign-physics version.

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3175b0e533..1f74655efe 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -96,7 +96,7 @@ ign_find_package (Qt5
 
 #--------------------------------------
 # Find ignition-physics
-ign_find_package(ignition-physics2 VERSION 2.3
+ign_find_package(ignition-physics2 VERSION 2.5
   COMPONENTS
     mesh
     sdf

From 6b89ccb6f0fed576e0bae601ed7cb9ee4add86e2 Mon Sep 17 00:00:00 2001
From: "Addisu Z. Taddese" <addisu@openrobotics.org>
Date: Tue, 9 Nov 2021 15:53:33 -0600
Subject: [PATCH 5/6] Disable tests not supported by version of DART

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
---
 test/integration/CMakeLists.txt    | 10 ++++++++++
 test/integration/physics_system.cc | 23 ++++++++++++++++++++---
 2 files changed, 30 insertions(+), 3 deletions(-)

diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 1aebf4bd2d..922069f596 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -82,3 +82,13 @@ ign_build_tests(TYPE INTEGRATION
   LIB_DEPS
     ${EXTRA_TEST_LIB_DEPS}
 )
+
+# For INTEGRATION_physics_system, we need to check what version of DART is
+# available so that we can disable tests that are unsupported by the particular
+# version of physics engine
+ign_find_package(DART QUIET)
+if (DART_FOUND)
+  # Only adding include directories, no need to link against DART to check version
+  target_include_directories(INTEGRATION_physics_system SYSTEM PRIVATE ${DART_INCLUDE_DIRS})
+  target_compile_definitions(INTEGRATION_physics_system PRIVATE HAVE_DART)
+endif()
diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc
index 1837bb37b5..c9a519084b 100644
--- a/test/integration/physics_system.cc
+++ b/test/integration/physics_system.cc
@@ -20,6 +20,9 @@
 #include <algorithm>
 #include <vector>
 
+#ifdef HAVE_DART
+#include <dart/config.hpp>
+#endif
 #include <ignition/common/Console.hh>
 #include <ignition/common/Util.hh>
 #include <sdf/Collision.hh>
@@ -72,6 +75,20 @@ class PhysicsSystemFixture : public InternalFixture<::testing::Test>
 {
 };
 
+class PhysicsSystemFixtureWithDart6_10 : public PhysicsSystemFixture
+{
+  protected: void SetUp() override
+  {
+#ifndef HAVE_DART
+    GTEST_SKIP();
+#elif !DART_VERSION_AT_LEAST(6, 10, 0)
+    GTEST_SKIP();
+#endif
+
+    PhysicsSystemFixture::SetUp();
+  }
+};
+
 /////////////////////////////////////////////////
 TEST_F(PhysicsSystemFixture, CreatePhysicsWorld)
 {
@@ -813,7 +830,7 @@ TEST_F(PhysicsSystemFixture, ResetVelocityComponent)
 
 /////////////////////////////////////////////////
 /// Test joint position limit command component
-TEST_F(PhysicsSystemFixture, JointPositionLimitsCommandComponent)
+TEST_F(PhysicsSystemFixtureWithDart6_10, JointPositionLimitsCommandComponent)
 {
   ignition::gazebo::ServerConfig serverConfig;
 
@@ -936,7 +953,7 @@ TEST_F(PhysicsSystemFixture, JointPositionLimitsCommandComponent)
 
 /////////////////////////////////////////////////
 /// Test joint velocity limit command component
-TEST_F(PhysicsSystemFixture, JointVelocityLimitsCommandComponent)
+TEST_F(PhysicsSystemFixtureWithDart6_10, JointVelocityLimitsCommandComponent)
 {
   ignition::gazebo::ServerConfig serverConfig;
 
@@ -1060,7 +1077,7 @@ TEST_F(PhysicsSystemFixture, JointVelocityLimitsCommandComponent)
 
 /////////////////////////////////////////////////
 /// Test joint effort limit command component
-TEST_F(PhysicsSystemFixture, JointEffortLimitsCommandComponent)
+TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent)
 {
   ignition::gazebo::ServerConfig serverConfig;
 

From c84d2dc318dcc2e2f0bd2ba0892b130140124631 Mon Sep 17 00:00:00 2001
From: "Addisu Z. Taddese" <addisu@openrobotics.org>
Date: Tue, 9 Nov 2021 16:12:09 -0600
Subject: [PATCH 6/6] Add NOLINT

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
---
 src/systems/physics/Physics.cc | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 0b570793fe..2fcfdc7c0e 100644
--- a/src/systems/physics/Physics.cc
+++ b/src/systems/physics/Physics.cc
@@ -1883,7 +1883,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
 
         return true;
       });
-}
+}  // NOLINT readability/fn_size
+// TODO (azeey) Reduce size of function and remove the NOLINT above
 
 //////////////////////////////////////////////////
 void PhysicsPrivate::Step(const std::chrono::steady_clock::duration &_dt)