Populate JointConstraintWrench from physics (gazebosim#989)

Signed-off-by: Addisu Z. Taddese <[email protected]>
Signed-off-by: William Lew <[email protected]>

include/ignition/gazebo/components/JointTransmittedWrench.hh

@@ -0,0 +1,56 @@
+/*
+ * 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 <ignition/msgs/wrench.pb.h>
+
+#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 {
+
+namespace components
+{
+/// \brief Joint Transmitted wrench in SI units (Nm for torque, N for 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 pair of 3D vectors representing
+/// torque and force quantities expessed in a given frame and where the force is
+/// applied at the origin of the frame. This is different from the Wrench used
+/// in screw theory.
+/// \note The value of force_offset in msgs::Wrench is ignored for this
+/// component. The force is assumed to be applied at the origin of the joint
+/// frame.
+using JointTransmittedWrench =
+    Component<msgs::Wrench, class JointTransmittedWrenchTag,
+              serializers::MsgSerializer>;
+IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.JointTransmittedWrench",
+                              JointTransmittedWrench)
+}  // namespace components
+}
+}
+}
+
+#endif

src/systems/physics/Physics.cc

@@ -116,6 +116,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"
@@ -379,6 +380,19 @@ class ignition::gazebo::systems::PhysicsPrivate
                       }
                       return true;
                     }};
+  /// \brief msgs::Contacts equality comparison function.
+  public: std::function<bool(const msgs::Wrench &, const msgs::Wrench &)>
+          wrenchEql{
+          [](const msgs::Wrench &_a, const msgs::Wrench &_b)
+          {
+            return math::equal(_a.torque().x(), _b.torque().x(), 1e-6) &&
+                   math::equal(_a.torque().y(), _b.torque().y(), 1e-6) &&
+                   math::equal(_a.torque().z(), _b.torque().z(), 1e-6) &&
+
+                   math::equal(_a.force().x(), _b.force().x(), 1e-6) &&
+                   math::equal(_a.force().y(), _b.force().y(), 1e-6) &&
+                   math::equal(_a.force().z(), _b.force().z(), 1e-6);
+          }};
 
   /// \brief Environment variable which holds paths to look for engine plugins
   public: std::string pluginPathEnv = "IGN_GAZEBO_PHYSICS_ENGINE_PATH";
@@ -418,6 +432,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 +581,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 +2798,46 @@ 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();
+
+          msgs::Wrench wrenchData;
+          msgs::Set(wrenchData.mutable_torque(),
+                    math::eigen3::convert(jointWrench.torque));
+          msgs::Set(wrenchData.mutable_force(),
+                    math::eigen3::convert(jointWrench.force));
+          const auto state =
+              _wrench->SetData(wrenchData, this->wrenchEql)
+                  ? 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/components.cc

@@ -18,6 +18,8 @@
 #include <gtest/gtest.h>
 
 #include <ignition/msgs/particle_emitter.pb.h>
+#include <ignition/msgs/wrench.pb.h>
+#include <ignition/msgs/Utility.hh>
 
 #include <chrono>
 
@@ -48,6 +50,7 @@
 #include "ignition/gazebo/components/Inertial.hh"
 #include "ignition/gazebo/components/Joint.hh"
 #include "ignition/gazebo/components/JointAxis.hh"
+#include "ignition/gazebo/components/JointTransmittedWrench.hh"
 #include "ignition/gazebo/components/JointType.hh"
 #include "ignition/gazebo/components/JointVelocity.hh"
 #include "ignition/gazebo/components/JointVelocityCmd.hh"
@@ -1661,3 +1664,25 @@ TEST_F(ComponentsTest, ParticleEmitterCmd)
   EXPECT_EQ(comp1.Data().emitting().data(), comp3.Data().emitting().data());
   EXPECT_EQ(comp1.Data().name(), comp3.Data().name());
 }
+
+//////////////////////////////////////////////////
+TEST_F(ComponentsTest, JointTransmittedWrench)
+{
+  msgs::Wrench wrench;
+  msgs::Set(wrench.mutable_torque(), {10, 20, 30});
+  msgs::Set(wrench.mutable_force(), {1, 2, 3});
+
+  // // Create components.
+  auto comp1 = components::JointTransmittedWrench(wrench);
+
+  // Stream operators.
+  std::ostringstream ostr;
+  comp1.Serialize(ostr);
+
+  std::istringstream istr(ostr.str());
+  components::JointTransmittedWrench comp2;
+  comp2.Deserialize(istr);
+  EXPECT_EQ(msgs::Convert(comp2.Data().force()), msgs::Convert(wrench.force()));
+  EXPECT_EQ(msgs::Convert(comp2.Data().torque()),
+            msgs::Convert(wrench.torque()));
+}

test/integration/physics_system.cc

@@ -23,6 +23,7 @@
 
 #include <ignition/common/Console.hh>
 #include <ignition/common/Util.hh>
+#include <ignition/msgs/Utility.hh>
 #include <sdf/Collision.hh>
 #include <sdf/Cylinder.hh>
 #include <sdf/Geometry.hh>
@@ -44,6 +45,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"
@@ -56,6 +58,7 @@
 #include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/Physics.hh"
 #include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/PoseCmd.hh"
 #include "ignition/gazebo/components/Static.hh"
 #include "ignition/gazebo/components/Visual.hh"
 #include "ignition/gazebo/components/World.hh"
@@ -1662,3 +1665,124 @@ 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<msgs::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, msgs::Convert(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, msgs::Convert(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);
+  }
+}