Change Wrench type, add test

include/ignition/gazebo/components/JointTransmittedWrench.hh

@@ -0,0 +1,96 @@
+/*
+ * 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_JOINTTRANSMITTEDWRENCH_HH_
+#define IGNITION_GAZEBO_COMPONENTS_JOINTTRANSMITTEDWRENCH_HH_
+
+#include <array>
+
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Serialization.hh>
+#include <ignition/gazebo/config.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+
+/// \brief Data structure that contains torque and force.
+struct Wrench
+{
+  math::Vector3d torque;
+  math::Vector3d force;
+
+  /// \brief Equality comparison operator
+  /// \param[in] _other The other Wrench against which this wrench is compared.
+  /// \return True if both the torque and force values are equal.
+  bool operator==(const Wrench &_other) const
+  {
+    return (this->torque == _other.torque) && (this->force == _other.force);
+  }
+};
+
+namespace serializers
+{
+class WrenchSerializer
+{
+  /// \brief Serialization for `Wrench`.
+  /// \param[in] _out Output stream.
+  /// \param[in] _wrench Wrench to stream
+  /// \return The stream.
+  public: static std::ostream &Serialize(std::ostream &_out,
+                                         const Wrench &_wrench)
+  {
+    _out << _wrench.torque << " " << _wrench.torque;
+    return _out;
+  }
+
+  /// \brief Deserialization for `Wrench`.
+  /// \param[in] _in Input stream.
+  /// \param[out] _Wrench Wrench to populate
+  /// \return The stream.
+  public: static std::istream &Deserialize(std::istream &_in, Wrench &_wrench)
+  {
+    _in >> _wrench.torque;
+    _in >> _wrench.force;
+    return _in;
+  }
+};
+}  // namespace serializers
+
+namespace components
+{
+/// \brief Joint Transmitted wrench in SI units (Nm for revolute, N for
+/// prismatic). The first set of 3 values coorespond to the torque while the
+/// second set of 3 correspond to the force. The wrench is expressed in the
+/// Joint frame and the force component is applied at the joint origin.
+/// \note The term Wrench is used here to mean a 6D vector formed by stacking
+/// torque and force vectors. This is different from the Wrench used in screw
+/// theory.
+using JointTransmittedWrench =
+    Component<Wrench, class JointTransmittedWrenchTag,
+              serializers::WrenchSerializer>;
+IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.JointTransmittedWrench",
+                              JointTransmittedWrench)
+}  // namespace components
+}
+}
+}
+
+#endif

include/ignition/gazebo/components/Serialization.hh

@@ -132,43 +132,6 @@ namespace serializers
       return _in;
     }
   };
-  /// \brief Serializer for components that hold `std::array<double, N>`.
-  template <std::size_t N>
-  class ArrayDoubleSerializer
-  {
-    /// \brief Serialization
-    /// \param[in] _out Output stream.
-    /// \param[in] _arr Array to stream
-    /// \return The stream.
-    public: static std::ostream &Serialize(std::ostream &_out,
-                                           const std::array<double, N> &_arr)
-    {
-      ignition::msgs::Double_V msg;
-      *msg.mutable_data() = {_arr.begin(), _arr.end()};
-      msg.SerializeToOstream(&_out);
-      return _out;
-    }
-
-    /// \brief Deserialization
-    /// \param[in] _in Input stream.
-    /// \param[in] _arr Array to populate
-    /// \return The stream.
-    public: static std::istream &Deserialize(std::istream &_in,
-                                             std::array<double, N> &_arr)
-    {
-      ignition::msgs::Double_V msg;
-      msg.ParseFromIstream(&_in);
-      if (static_cast<std::size_t>(msg.data_size()) > N)
-      {
-        ignerr << "Size of input array exceeds " << N << ". Cannot deserialize"
-               << std::endl;
-        return _in;
-      }
-
-      std::copy(msg.data().begin(), msg.data().end(), _arr.begin());
-      return _in;
-    }
-  };
 
   /// \brief Serializer for components that hold protobuf messages.
   class MsgSerializer

src/systems/physics/Physics.cc

@@ -26,6 +26,7 @@
 #include <algorithm>
 #include <iostream>
 #include <deque>
+#include <functional>
 #include <map>
 #include <string>
 #include <unordered_map>
@@ -117,7 +118,7 @@
 #include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/ParentLinkName.hh"
 #include "ignition/gazebo/components/ExternalWorldWrenchCmd.hh"
-#include "ignition/gazebo/components/JointConstraintWrench.hh"
+#include "ignition/gazebo/components/JointTransmittedWrench.hh"
 #include "ignition/gazebo/components/JointForceCmd.hh"
 #include "ignition/gazebo/components/Physics.hh"
 #include "ignition/gazebo/components/PhysicsEnginePlugin.hh"
@@ -421,10 +422,10 @@ class ignition::gazebo::systems::PhysicsPrivate
             physics::SetJointTransformFromParentFeature>{};
 
   //////////////////////////////////////////////////
-  // Joint constraint wrench
-  /// \brief Feature list for getting joint constraint wrenches.
-  public: struct JointGetConstraintWrenchFeatureList : physics::FeatureList<
-            physics::GetJointConstraintWrench>{};
+  // Joint transmitted wrench
+  /// \brief Feature list for getting joint transmitted wrenches.
+  public: struct JointGetTransmittedWrenchFeatureList : physics::FeatureList<
+            physics::GetJointTransmittedWrench>{};
 
   //////////////////////////////////////////////////
   // Collisions
@@ -570,7 +571,7 @@ class ignition::gazebo::systems::PhysicsPrivate
             JointFeatureList,
             DetachableJointFeatureList,
             JointVelocityCommandFeatureList,
-            JointGetConstraintWrenchFeatureList
+            JointGetTransmittedWrenchFeatureList
             >;
 
   /// \brief A map between joint entity ids in the ECM to Joint Entities in
@@ -2786,31 +2787,37 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
       });
   IGN_PROFILE_END();
 
-  // Update joint constraints
-  _ecm.Each<components::Joint, components::JointConstraintWrench>(
+  // Update joint transmitteds
+  _ecm.Each<components::Joint, components::JointTransmittedWrench>(
       [&](const Entity &_entity, components::Joint *,
-          components::JointConstraintWrench *_wrench) -> bool
+          components::JointTransmittedWrench *_wrench) -> bool
       {
         auto jointPhys =
             this->entityJointMap
-                .EntityCast<JointGetConstraintWrenchFeatureList>(_entity);
+                .EntityCast<JointGetTransmittedWrenchFeatureList>(_entity);
         if (jointPhys)
         {
-          const auto &jointWrench = jointPhys->GetConstraintWrench();
-          std::copy(jointWrench.data(), jointWrench.data() + jointWrench.size(),
-                    std::begin(_wrench->Data()));
-          _ecm.SetChanged(_entity, components::JointConstraintWrench::typeId,
-                          ComponentState::PeriodicChange);
+          const auto &jointWrench = jointPhys->GetTransmittedWrench();
+
+          gazebo::Wrench wrenchData;
+          wrenchData.torque = math::eigen3::convert(jointWrench.torque);
+          wrenchData.force = math::eigen3::convert(jointWrench.force);
+          const auto state =
+              _wrench->SetData(wrenchData, std::equal_to<gazebo::Wrench>())
+                  ? ComponentState::PeriodicChange
+                  : ComponentState::NoChange;
+          _ecm.SetChanged(_entity, components::JointTransmittedWrench::typeId,
+                          state);
         }
         else
         {
           static bool informed{false};
           if (!informed)
           {
             igndbg
-                << "Attempting to get joint constraint wrenches, but the "
+                << "Attempting to get joint transmitted wrenches, but the "
                    "physics engine doesn't support this feature. Values in the "
-                   "JointConstraintWrench component will not be meaningful."
+                   "JointTransmittedWrench component will not be meaningful."
                 << std::endl;
             informed = true;
           }