Populate JointTransmittedWrench from physics

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

@@ -20,6 +20,7 @@
 #include <google/protobuf/message_lite.h>
 #include <ignition/msgs/double_v.pb.h>
 
+#include <array>
 #include <string>
 #include <vector>
 #include <sdf/Sensor.hh>

src/systems/physics/Physics.cc

@@ -22,9 +22,11 @@
 #include <ignition/msgs/entity.pb.h>
 #include <ignition/msgs/Utility.hh>
 
+#include <array>
 #include <algorithm>
 #include <iostream>
 #include <deque>
+#include <functional>
 #include <map>
 #include <string>
 #include <unordered_map>
@@ -116,6 +118,7 @@
 #include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/ParentLinkName.hh"
 #include "ignition/gazebo/components/ExternalWorldWrenchCmd.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"
@@ -418,6 +421,12 @@ class ignition::gazebo::systems::PhysicsPrivate
             physics::DetachJointFeature,
             physics::SetJointTransformFromParentFeature>{};
 
+  //////////////////////////////////////////////////
+  // Joint transmitted wrench
+  /// \brief Feature list for getting joint transmitted wrenches.
+  public: struct JointGetTransmittedWrenchFeatureList : physics::FeatureList<
+            physics::GetJointTransmittedWrench>{};
+
   //////////////////////////////////////////////////
   // Collisions
 
@@ -561,7 +570,8 @@ class ignition::gazebo::systems::PhysicsPrivate
             physics::Joint,
             JointFeatureList,
             DetachableJointFeatureList,
-            JointVelocityCommandFeatureList
+            JointVelocityCommandFeatureList,
+            JointGetTransmittedWrenchFeatureList
             >;
 
   /// \brief A map between joint entity ids in the ECM to Joint Entities in
@@ -2777,6 +2787,44 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
       });
   IGN_PROFILE_END();
 
+  // Update joint transmitteds
+  _ecm.Each<components::Joint, components::JointTransmittedWrench>(
+      [&](const Entity &_entity, components::Joint *,
+          components::JointTransmittedWrench *_wrench) -> bool
+      {
+        auto jointPhys =
+            this->entityJointMap
+                .EntityCast<JointGetTransmittedWrenchFeatureList>(_entity);
+        if (jointPhys)
+        {
+          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 transmitted wrenches, but the "
+                   "physics engine doesn't support this feature. Values in the "
+                   "JointTransmittedWrench component will not be meaningful."
+                << std::endl;
+            informed = true;
+          }
+        }
+        return true;
+      });
+
   // TODO(louise) Skip this if there are no collision features
   this->UpdateCollisions(_ecm);
 }

test/integration/physics_system.cc

@@ -18,6 +18,7 @@
 #include <gtest/gtest.h>
 
 #include <algorithm>
+#include <cmath>
 #include <string>
 #include <vector>
 
@@ -36,6 +37,7 @@
 #include "ignition/gazebo/Server.hh"
 #include "ignition/gazebo/SystemLoader.hh"
 #include "ignition/gazebo/Util.hh"
+#include "ignition/gazebo/components/PoseCmd.hh"
 #include "ignition/gazebo/test_config.hh"  // NOLINT(build/include)
 
 #include "ignition/gazebo/components/AxisAlignedBox.hh"
@@ -44,6 +46,7 @@
 #include "ignition/gazebo/components/Geometry.hh"
 #include "ignition/gazebo/components/Inertial.hh"
 #include "ignition/gazebo/components/Joint.hh"
+#include "ignition/gazebo/components/JointTransmittedWrench.hh"
 #include "ignition/gazebo/components/JointPosition.hh"
 #include "ignition/gazebo/components/JointPositionReset.hh"
 #include "ignition/gazebo/components/JointVelocity.hh"
@@ -1662,3 +1665,122 @@ TEST_F(PhysicsSystemFixture, Heightmap)
   EXPECT_TRUE(checked);
   EXPECT_EQ(1000, maxIt);
 }
+
+/////////////////////////////////////////////////
+// Joint force
+TEST_F(PhysicsSystemFixture, JointTransmittedWrench)
+{
+  common::Console::SetVerbosity(4);
+  ignition::gazebo::ServerConfig serverConfig;
+
+  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+    "/test/worlds/joint_transmitted_wrench.sdf";
+
+  serverConfig.SetSdfFile(sdfFile);
+
+  gazebo::Server server(serverConfig);
+
+  server.SetUpdatePeriod(1us);
+
+  // Create a system that records the poses of the links after physics
+  test::Relay testSystem;
+
+  testSystem.OnPreUpdate(
+      [&](const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm)
+      {
+        if (_info.iterations == 1)
+        {
+          _ecm.Each<components::Joint>(
+              [&](const ignition::gazebo::Entity &_entity,
+                  const components::Joint *) -> bool
+              {
+                _ecm.CreateComponent(_entity,
+                                     components::JointTransmittedWrench());
+                return true;
+              });
+        }
+      });
+
+  const std::size_t totalIters = 1800;
+  std::vector<Wrench> wrenches;
+  wrenches.reserve(totalIters);
+  // Simply collect joint wrenches. We check the values later.
+  testSystem.OnPostUpdate(
+      [&](const gazebo::UpdateInfo &,
+          const gazebo::EntityComponentManager &_ecm)
+      {
+        const auto sensorJointEntity = _ecm.EntityByComponents(
+            components::Joint(), components::Name("sensor_joint"));
+        const auto jointWrench =
+            _ecm.ComponentData<components::JointTransmittedWrench>(
+                sensorJointEntity);
+        if (jointWrench.has_value())
+        {
+          wrenches.push_back(*jointWrench);
+        }
+      });
+  server.AddSystem(testSystem.systemPtr);
+  server.Run(true, totalIters, false);
+
+  ASSERT_EQ(totalIters, wrenches.size());
+
+  const double kWeightScaleContactHeight = 0.05 + 0.25;
+  const double kSensorMass = 0.2;
+  const double kWeightMass = 10;
+  const double kGravity = 9.8;
+  const double kWeightInitialHeight = 0.5;
+  const double dt = 0.001;
+  const double timeOfContact = std::sqrt(
+      2 * (kWeightInitialHeight - kWeightScaleContactHeight) / kGravity);
+  std::size_t iterOfContact =
+      static_cast<std::size_t>(std::round(timeOfContact / dt));
+
+  for (std::size_t i = 0; i < iterOfContact - 10; ++i)
+  {
+    const auto &wrench = wrenches[i];
+    EXPECT_NEAR(0.0, wrench.force.X(), 1e-3);
+    EXPECT_NEAR(0.0, wrench.force.Y(), 1e-3);
+    EXPECT_NEAR(kGravity * kSensorMass, wrench.force.Z(), 1e-3);
+    EXPECT_EQ(math::Vector3d::Zero, wrench.torque);
+  }
+
+  // Wait 300 (determined empirically) iterations for values to stabilize.
+  for (std::size_t i = iterOfContact + 300; i < wrenches.size(); ++i)
+  {
+    const auto &wrench = wrenches[i];
+    EXPECT_NEAR(0.0, wrench.force.X(), 1e-3);
+    EXPECT_NEAR(0.0, wrench.force.Y(), 1e-3);
+    EXPECT_NEAR(kGravity * (kSensorMass + kWeightMass), wrench.force.Z(), 1e-3);
+    EXPECT_EQ(math::Vector3d::Zero, wrench.torque);
+  }
+
+  // Move the weight off center so it generates torque
+  testSystem.OnPreUpdate(
+      [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm)
+      {
+        const auto weightEntity = _ecm.EntityByComponents(
+            components::Model(), components::Name("weight"));
+        _ecm.SetComponentData<components::WorldPoseCmd>(
+            weightEntity, math::Pose3d(0.2, 0.1, 0.5, 0, 0, 0));
+      });
+
+  server.RunOnce();
+  // Reset PreUpdate so it doesn't keep moving the weight
+  testSystem.OnPreUpdate({});
+  wrenches.clear();
+  server.Run(true, totalIters, false);
+  ASSERT_EQ(totalIters, wrenches.size());
+
+  // Wait 300 (determined empirically) iterations for values to stabilize.
+  for (std::size_t i = iterOfContact + 300; i < wrenches.size(); ++i)
+  {
+    const auto &wrench = wrenches[i];
+    EXPECT_NEAR(0.0, wrench.force.X(), 1e-3);
+    EXPECT_NEAR(0.0, wrench.force.Y(), 1e-3);
+    EXPECT_NEAR(kGravity * (kSensorMass + kWeightMass), wrench.force.Z(), 1e-3);
+
+    EXPECT_NEAR(0.1 * kGravity * kWeightMass, wrench.torque.X(), 1e-3);
+    EXPECT_NEAR(-0.2 * kGravity * kWeightMass, wrench.torque.Y(), 1e-3);
+    EXPECT_NEAR(0.0, wrench.torque.Z(), 1e-3);
+  }
+}