Add velocity limits component and method to Joint class (#2664)

Signed-off-by: Mohamed Zaghloul <[email protected]>

Author: mo-zaghloul

.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "optional": "cpp"
+    }
+}

include/gz/sim/Joint.hh

@@ -260,6 +260,12 @@ namespace gz
       public: std::optional<std::vector<double>> Position(
           const EntityComponentManager &_ecm) const;
 
+      /// \brief Get the velocity limits of the joint.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Velocity limits (min, max) or nullopt if not available.
+      public: std::optional<std::vector<gz::math::Vector2d>> VelocityLimits(
+          const EntityComponentManager &_ecm) const;
+
       /// \brief Get the transmitted wrench of the joint
       /// \param[in] _ecm Entity-component manager.
       /// \return Transmitted wrench of the joint or nullopt if transmitted

include/gz/sim/components/JointVelocityLimits.hh

@@ -0,0 +1,55 @@
+/*
+ * 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 GZ_SIM_COMPONENTS_JOINTVELOCITYLIMITS_HH_
+#define GZ_SIM_COMPONENTS_JOINTVELOCITYLIMITS_HH_
+
+#include <vector>
+#include <gz/math/Vector2.hh>
+
+#include <gz/sim/components/Component.hh>
+#include <gz/sim/components/Factory.hh>
+#include <gz/sim/components/Serialization.hh>
+#include <gz/sim/config.hh>
+#include <gz/sim/Export.hh>
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+
+namespace components
+{
+/// \brief Component for recording the current 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.
+using JointVelocityLimits = Component<
+  std::vector<gz::math::Vector2d>,
+  class JointVelocityLimitsTag,
+  serializers::VectorSerializer<gz::math::Vector2d>
+>;
+
+GZ_SIM_REGISTER_COMPONENT(
+  "gz_sim_components.JointVelocityLimits", JointVelocityLimits)
+}
+}
+}
+}
+
+#endif

src/Joint.cc

@@ -28,6 +28,7 @@
 #include "gz/sim/components/JointVelocity.hh"
 #include "gz/sim/components/JointVelocityCmd.hh"
 #include "gz/sim/components/JointVelocityLimitsCmd.hh"
+#include "gz/sim/components/JointVelocityLimits.hh"
 #include "gz/sim/components/JointVelocityReset.hh"
 #include "gz/sim/components/Name.hh"
 #include "gz/sim/components/ParentEntity.hh"
@@ -354,6 +355,14 @@ std::optional<std::vector<double>> Joint::Position(
       this->dataPtr->id);
 }
 
+//////////////////////////////////////////////////
+std::optional<std::vector<gz::math::Vector2d>> Joint::VelocityLimits(
+    const EntityComponentManager &_ecm) const
+{
+  return _ecm.ComponentData<components::JointVelocityLimits>(
+      this->dataPtr->id);
+}
+
 //////////////////////////////////////////////////
 std::optional<std::vector<msgs::Wrench>> Joint::TransmittedWrench(
     const EntityComponentManager &_ecm) const

src/systems/physics/Physics.cc

@@ -122,6 +122,7 @@
 #include "gz/sim/components/JointVelocity.hh"
 #include "gz/sim/components/JointVelocityCmd.hh"
 #include "gz/sim/components/JointVelocityLimitsCmd.hh"
+#include "gz/sim/components/JointVelocityLimits.hh"
 #include "gz/sim/components/JointVelocityReset.hh"
 #include "gz/sim/components/LinearAcceleration.hh"
 #include "gz/sim/components/LinearVelocity.hh"