Wheel slip system

include/ignition/gazebo/components/SlipComplianceCmd.hh

@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2020 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_SLIPCOMPLIANCECMD_HH_
+#define IGNITION_GAZEBO_COMPONENTS_SLIPCOMPLIANCECMD_HH_
+
+#include <vector>
+
+#include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/Export.hh>
+
+#include <ignition/gazebo/components/Factory.hh>
+#include "ignition/gazebo/components/Component.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+  /// \brief A component type that contains the slip compliance parameters to be
+  /// set on a collision. The 0 and 1 index values correspond to the slip
+  /// compliance parameters in friction direction 1 (fdir1) and friction
+  /// direction 2 (fdir2) respectively.
+  using SlipComplianceCmd =
+    Component<std::vector<double>, class SlipComplianceCmdTag>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SlipComplianceCmd ",
+      SlipComplianceCmd)
+}
+}
+}
+}
+#endif

src/systems/CMakeLists.txt

@@ -97,4 +97,5 @@ add_subdirectory(sensors)
 add_subdirectory(touch_plugin)
 add_subdirectory(triggered_publisher)
 add_subdirectory(user_commands)
+add_subdirectory(wheel_slip)
 add_subdirectory(wind_effects)

src/systems/physics/Physics.cc

@@ -106,6 +106,7 @@
 #include "ignition/gazebo/components/Pose.hh"
 #include "ignition/gazebo/components/PoseCmd.hh"
 #include "ignition/gazebo/components/SelfCollide.hh"
+#include "ignition/gazebo/components/SlipComplianceCmd.hh"
 #include "ignition/gazebo/components/Static.hh"
 #include "ignition/gazebo/components/ThreadPitch.hh"
 #include "ignition/gazebo/components/World.hh"
@@ -125,6 +126,8 @@ class ignition::gazebo::systems::PhysicsPrivate
   /// New features can't be added to this list in minor / patch releases, in
   /// order to maintain backwards compatibility with downstream physics plugins.
   public: using MinimumFeatureList = ignition::physics::FeatureList<
+          // FreeGroup
+
           ignition::physics::FindFreeGroupFeature,
           ignition::physics::SetFreeGroupWorldPose,
           ignition::physics::FreeGroupFrameSemantics,
@@ -271,6 +274,27 @@ class ignition::gazebo::systems::PhysicsPrivate
   /// \brief Environment variable which holds paths to look for engine plugins
   public: std::string pluginPathEnv = "IGN_GAZEBO_PHYSICS_ENGINE_PATH";
 
+  //////////////////////////////////////////////////
+  // Slip Compliance
+
+  /// \brief Feature list to process `FrictionPyramidSlipCompliance` components.
+  public: using FrictionPyramidSlipComplianceFeatureList = physics::FeatureList<
+            MinimumFeatureList,
+            ignition::physics::GetShapeFrictionPyramidSlipCompliance,
+            ignition::physics::SetShapeFrictionPyramidSlipCompliance>;
+
+  /// \brief Shape type with slip compliance features.
+  public: using ShapeSlipParamPtrType = physics::ShapePtr<
+            physics::FeaturePolicy3d, FrictionPyramidSlipComplianceFeatureList>;
+
+  /// \brief A map between shape entity ids in the ECM to Shape Entities in
+  /// ign-physics
+  /// All shapes on this map are also in `entityCollisionMap`. The difference
+  /// is that here they've been casted for
+  /// `FrictionPyramidSlipComplianceFeatureList`.
+  public: std::unordered_map<Entity, ShapeSlipParamPtrType>
+      entityShapeSlipParamMap;
+
   //////////////////////////////////////////////////
   // Detachable joints
 
@@ -1169,6 +1193,42 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         return true;
       });
 
+  // Slip compliance on Collisions
+  _ecm.Each<components::SlipComplianceCmd>(
+      [&](const Entity &_entity,
+          const components::SlipComplianceCmd *_slipCmdComp)
+      {
+        auto shapeIt = this->entityCollisionMap.find(_entity);
+        if (shapeIt == this->entityCollisionMap.end())
+        {
+          ignwarn << "Failed to find shape [" << _entity << "]." << std::endl;
+          return true;
+        }
+
+        auto slipComplianceShape = entityCast(_entity, shapeIt->second,
+            this->entityShapeSlipParamMap);
+
+        if (!slipComplianceShape)
+        {
+          ignwarn << "Can't process Wheel Slip component, physics engine "
+                  << "missing SetShapeFrictionPyramidSlipCompliance"
+                  << std::endl;
+
+          // Break Each call since no SlipCompliances can be processed
+          return false;
+        }
+
+        if (_slipCmdComp->Data().size() == 2)
+        {
+          slipComplianceShape->SetPrimarySlipCompliance(
+              _slipCmdComp->Data()[0]);
+          slipComplianceShape->SetSecondarySlipCompliance(
+              _slipCmdComp->Data()[1]);
+        }
+
+        return true;
+      });
+
   // Clear pending commands
   // Note: Removing components from inside an Each call can be dangerous.
   // Instead, we collect all the entities that have the desired components and
@@ -1597,6 +1657,13 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) const
         return true;
       });
 
+  _ecm.Each<components::SlipComplianceCmd>(
+      [&](const Entity &, components::SlipComplianceCmd *_slip) -> bool
+      {
+        std::fill(_slip->Data().begin(), _slip->Data().end(), 0.0);
+        return true;
+      });
+
   // Update joint positions
   _ecm.Each<components::Joint, components::JointPosition>(
       [&](const Entity &_entity, components::Joint *,

src/systems/wheel_slip/CMakeLists.txt

@@ -0,0 +1,6 @@
+gz_add_system(wheel-slip
+  SOURCES
+  WheelSlip.cc
+  PUBLIC_LINK_LIBS
+    ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+)