Adding Hydraulic Pneumatic Friction plugin

buoy_description/models/mbari_wec/model.sdf

@@ -62,5 +62,9 @@
     </plugin>
     <!-- <debug_prescribed_velocity>true</debug_prescribed_velocity>-->
 
+    <plugin filename="PTOFriction" name="buoy_gazebo::PTOFriction">
+      <PrismaticJointName>HydraulicRam</PrismaticJointName>
+    </plugin>
+
   </model>
 </sdf>

buoy_gazebo/CMakeLists.txt

@@ -142,7 +142,7 @@ if(BUILD_TESTING)
       )
     endif()
     if(gz_add_test_ROS)
-      set(ROS_PKGS rclcpp ${gz_add_test_EXTRA_ROS_PKGS})
+      set(ROS_PKGS rclcpp splinter_ros ${gz_add_test_EXTRA_ROS_PKGS})
       foreach(PKG ${ROS_PKGS})
         find_package(${PKG} REQUIRED)
       endforeach()
@@ -167,6 +167,7 @@ if(BUILD_TESTING)
 
   # Add gtests
   gz_add_gtest(test_stopwatch ROS)
+  gz_add_gtest(test_pto_friction ROS)
 
 endif()
 

buoy_gazebo/src/PTOFriction/CMakeLists.txt

@@ -0,0 +1,8 @@
+gz_add_plugin(PTOFriction
+  SOURCES
+  PTOFriction.cpp
+  INCLUDE_DIRS
+    ..
+  EXTRA_ROS_PKGS
+    splinter_ros
+)

buoy_gazebo/src/PTOFriction/PTOFriction.cpp

@@ -0,0 +1,158 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute
+//
+// 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.
+
+#include "PTOFriction.hpp"
+#include <ignition/common/Profiler.hh>
+#include <ignition/gazebo/components/Name.hh>
+#include <ignition/gazebo/components/JointVelocityCmd.hh>
+#include <ignition/gazebo/components/JointForceCmd.hh>
+#include <ignition/gazebo/components/JointVelocity.hh>
+#include <ignition/gazebo/Model.hh>
+#include <ignition/plugin/Register.hh>
+
+#include <stdio.h>
+
+#include <splinter_ros/splinter1d.hpp>
+
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+
+
+namespace buoy_gazebo
+{
+class PTOFrictionPrivate
+{
+public:
+  /// \brief Piston joint entity
+  ignition::gazebo::Entity PrismaticJointEntity{ignition::gazebo::kNullEntity};
+
+  /// \brief Model interface
+  ignition::gazebo::Model model{ignition::gazebo::kNullEntity};
+
+  /// \brief Piston velocity
+  const std::vector<double> pistonSpeed;  // m/s
+
+  /// \brief Piston mean friction force
+  const std::vector<double> meanFriction;  // N
+
+  /// \brief Construct and approximation of friction model using linear spline
+  splinter_ros::Splinter1d pto_friction_model;
+
+  const splinter_ros::FillMode & fill_mode = splinter_ros::USE_BOUNDS;
+
+  PTOFrictionPrivate()
+  : pistonSpeed{0.4F, 0.1F, -0.1F, -0.4F},
+    meanFriction{1200.0F, 700.0F, -1000.0F, -2900.0F},
+    pto_friction_model(pistonSpeed, meanFriction)
+  {
+  }
+};
+
+//////////////////////////////////////////////////
+PTOFriction::PTOFriction()
+: dataPtr(std::make_unique<PTOFrictionPrivate>())
+{
+}
+
+//////////////////////////////////////////////////
+void PTOFriction::Configure(
+  const ignition::gazebo::Entity & _entity,
+  const std::shared_ptr<const sdf::Element> & _sdf,
+  ignition::gazebo::EntityComponentManager & _ecm,
+  ignition::gazebo::EventManager & /*_eventMgr*/)
+{
+  this->dataPtr->model = ignition::gazebo::Model(_entity);
+  if (!this->dataPtr->model.Valid(_ecm)) {
+    ignerr << "PTOFriction plugin should be attached to a model entity. " <<
+      "Failed to initialize." << std::endl;
+    return;
+  }
+
+
+  // Get params from SDF for Prismatic Joint.
+  auto PrismaticJointName = _sdf->Get<std::string>("PrismaticJointName");
+  if (PrismaticJointName.empty()) {
+    ignerr << "PTOFriction found an empty PrismaticJointName parameter. " <<
+      "Failed to initialize.";
+    return;
+  }
+
+
+  this->dataPtr->PrismaticJointEntity = this->dataPtr->model.JointByName(
+    _ecm,
+    PrismaticJointName);
+  if (this->dataPtr->PrismaticJointEntity == ignition::gazebo::kNullEntity) {
+    ignerr << "Joint with name [" << PrismaticJointName << "] not found. " <<
+      "The PTOFriction may not influence this joint.\n";
+    return;
+  }
+}
+
+//////////////////////////////////////////////////
+void PTOFriction::PreUpdate(
+  const ignition::gazebo::UpdateInfo & _info,
+  ignition::gazebo::EntityComponentManager & _ecm)
+{
+  IGN_PROFILE("PTOFriction::PreUpdate");
+  // Nothing left to do if paused.
+  if (_info.paused) {
+    return;
+  }
+
+  auto SimTime = std::chrono::duration<double>(_info.simTime).count();
+
+  // If the joints haven't been identified yet, the plugin is disabled
+  if (this->dataPtr->PrismaticJointEntity == ignition::gazebo::kNullEntity) {
+    return;
+  }
+
+  // Create joint velocity component for piston if one doesn't exist
+  auto prismaticJointVelComp = _ecm.Component<ignition::gazebo::components::JointVelocity>(
+    this->dataPtr->PrismaticJointEntity);
+  if (prismaticJointVelComp == nullptr) {
+    _ecm.CreateComponent(
+      this->dataPtr->PrismaticJointEntity, ignition::gazebo::components::JointVelocity());
+  }
+  // We just created the joint velocity component, give one iteration for the
+  // physics system to update its size
+  if (prismaticJointVelComp == nullptr || prismaticJointVelComp->Data().empty()) {
+    return;
+  }
+
+  // Interpolate the new friction force based on current joint velocity
+  auto friction_force =
+    this->dataPtr->pto_friction_model.eval(
+    fabs(prismaticJointVelComp->Data().at(0)), this->dataPtr->fill_mode);
+
+  // Create new component for applying force if it doesn't already exist
+  auto forceComp = _ecm.Component<ignition::gazebo::components::JointForceCmd>(
+    this->dataPtr->PrismaticJointEntity);
+  if (forceComp == nullptr) {
+    _ecm.CreateComponent(
+      this->dataPtr->PrismaticJointEntity,
+      ignition::gazebo::components::JointForceCmd({friction_force}));  // Create this iteration
+  } else {
+    forceComp->Data()[0] += friction_force;  // Add friction to existing forces
+  }
+}
+
+}  // namespace buoy_gazebo
+
+IGNITION_ADD_PLUGIN(
+  buoy_gazebo::PTOFriction,
+  ignition::gazebo::System,
+  buoy_gazebo::PTOFriction::ISystemConfigure,
+  buoy_gazebo::PTOFriction::ISystemPreUpdate);

buoy_gazebo/src/PTOFriction/PTOFriction.hpp

@@ -0,0 +1,57 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute
+//
+// 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 PTOFRICTION__PTOFRICTION_HPP_
+#define PTOFRICTION__PTOFRICTION_HPP_
+
+#include <ignition/gazebo/System.hh>
+
+#include <memory>
+#include <vector>
+
+
+namespace buoy_gazebo
+{
+// Forward declaration
+class PTOFrictionPrivate;
+class PTOFriction : public ignition::gazebo::System,
+  public ignition::gazebo::ISystemConfigure,
+  public ignition::gazebo::ISystemPreUpdate
+{
+public:
+  /// \brief Constructor
+  PTOFriction();
+
+  /// \brief Destructor
+  ~PTOFriction() override = default;
+
+  // Documentation inherited
+  void Configure(
+    const ignition::gazebo::Entity & _entity,
+    const std::shared_ptr<const sdf::Element> & _sdf,
+    ignition::gazebo::EntityComponentManager & _ecm,
+    ignition::gazebo::EventManager & _eventMgr) override;
+
+  // Documentation inherited
+  void PreUpdate(
+    const ignition::gazebo::UpdateInfo & _info,
+    ignition::gazebo::EntityComponentManager & _ecm) override;
+
+private:
+  /// \brief Private data pointer
+  std::unique_ptr<PTOFrictionPrivate> dataPtr;
+};
+}  // namespace buoy_gazebo
+
+#endif  // PTOFRICTION__PTOFRICTION_HPP_

buoy_gazebo/test/test_pto_friction.cpp

@@ -0,0 +1,43 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+
+#include <gtest/gtest.h>
+
+#include <splinter_ros/splinter1d.hpp>
+
+#include <vector>
+
+
+// NOLINTNEXTLINE
+class PTOFrictionTest : public ::testing::Test
+{
+public:
+  const std::vector<double> speed;  // m/s
+  const std::vector<double> force;  // N
+  splinter_ros::Splinter1d friction_model;
+
+  PTOFrictionTest()
+  :  speed{0.4F, 0.1F, -0.1F, -0.4F},
+    force{1200.0F, 700.0F, -1000.0F, -2900.0F},
+    friction_model(speed, force)
+  {
+  }
+};
+
+TEST_F(PTOFrictionTest, ModelAccuracy)
+{
+  EXPECT_NEAR(0.0, friction_model.eval(0.0, splinter_ros::USE_BOUNDS), 200.0);
+  EXPECT_NEAR(-2100.0, friction_model.eval(-0.3, splinter_ros::USE_BOUNDS), 200.0);
+}