From b318a33fc94b3a01566fa39b4c4d9ad76b82118f Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Tue, 4 Oct 2022 11:08:10 -0700 Subject: [PATCH 01/10] plugin framework Signed-off-by: Dharini Dutia --- buoy_description/models/mbari_wec/model.sdf | 4 + buoy_gazebo/CMakeLists.txt | 3 +- .../HydraulicPneumaticFriction/CMakeLists.txt | 8 + .../HydraulicPneumaticFriction.cpp | 155 ++++++++++++++++++ .../HydraulicPneumaticFriction.hpp | 57 +++++++ .../test_hydraulic_pneumatic_friction.cpp | 42 +++++ 6 files changed, 268 insertions(+), 1 deletion(-) create mode 100644 buoy_gazebo/src/HydraulicPneumaticFriction/CMakeLists.txt create mode 100644 buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.cpp create mode 100644 buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.hpp create mode 100644 buoy_gazebo/test/test_hydraulic_pneumatic_friction.cpp diff --git a/buoy_description/models/mbari_wec/model.sdf b/buoy_description/models/mbari_wec/model.sdf index 79d966dc..4008f256 100644 --- a/buoy_description/models/mbari_wec/model.sdf +++ b/buoy_description/models/mbari_wec/model.sdf @@ -62,5 +62,9 @@ + + HydraulicRam + + diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt index ebd9f403..27315638 100644 --- a/buoy_gazebo/CMakeLists.txt +++ b/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_hydraulic_pneumatic_friction ROS) endif() diff --git a/buoy_gazebo/src/HydraulicPneumaticFriction/CMakeLists.txt b/buoy_gazebo/src/HydraulicPneumaticFriction/CMakeLists.txt new file mode 100644 index 00000000..c6fa5cb4 --- /dev/null +++ b/buoy_gazebo/src/HydraulicPneumaticFriction/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_plugin(HydraulicPneumaticFriction + SOURCES + HydraulicPneumaticFriction.cpp + INCLUDE_DIRS + .. + EXTRA_ROS_PKGS + splinter_ros +) \ No newline at end of file diff --git a/buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.cpp b/buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.cpp new file mode 100644 index 00000000..f84b0d17 --- /dev/null +++ b/buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.cpp @@ -0,0 +1,155 @@ +// 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 "HydraulicPneumaticFriction.hpp" +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + + +namespace buoy_gazebo +{ +class HydraulicPneumaticFrictionPrivate +{ +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 pistonSpeed; // m/s + + /// \brief Piston mean friction force + const std::vector meanFriction; // N + + /// \brief Construct and approximation of friction model using linear spline + splinter_ros::Splinter1d hydraulic_pneumatic_friction; + + HydraulicPneumaticFrictionPrivate() + : pistonSpeed{-0.4F, -0.3F, -0.2F, -0.1F, 0.0F, 0.1F, 0.2F, 0.3F, 0.4F}, + meanFriction{1200.0F, 1000.0F, 700.0F, 500.0F, 0.0F, -1000.0F, -1400.0F, -2100.0F, -2900.0F}, + hydraulic_pneumatic_friction(pistonSpeed, meanFriction) + { + } +}; + +////////////////////////////////////////////////// +HydraulicPneumaticFriction::HydraulicPneumaticFriction() +: dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void HydraulicPneumaticFriction::Configure( + const ignition::gazebo::Entity & _entity, + const std::shared_ptr & _sdf, + ignition::gazebo::EntityComponentManager & _ecm, + ignition::gazebo::EventManager & /*_eventMgr*/) +{ + this->dataPtr->model = ignition::gazebo::Model(_entity); + if (!this->dataPtr->model.Valid(_ecm)) { + ignerr << "HydraulicPneumaticFriction 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("PrismaticJointName"); + if (PrismaticJointName.empty()) { + ignerr << "HydraulicPneumaticFriction 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 HydraulicPneumaticFriction may not influence this joint.\n"; + return; + } +} + +////////////////////////////////////////////////// +void HydraulicPneumaticFriction::PreUpdate( + const ignition::gazebo::UpdateInfo & _info, + ignition::gazebo::EntityComponentManager & _ecm) +{ + IGN_PROFILE("HydraulicPneumaticFriction::PreUpdate"); + // Nothing left to do if paused. + if (_info.paused) { + return; + } + + auto SimTime = std::chrono::duration(_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( + 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->hydraulic_pneumatic_friction.eval(fabs(prismaticJointVelComp->Data().at(0))); + + // Create new component for applying force if it doesn't already exist + auto forceComp = _ecm.Component( + 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::HydraulicPneumaticFriction, + ignition::gazebo::System, + buoy_gazebo::HydraulicPneumaticFriction::ISystemConfigure, + buoy_gazebo::HydraulicPneumaticFriction::ISystemPreUpdate); diff --git a/buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.hpp b/buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.hpp new file mode 100644 index 00000000..8298fb05 --- /dev/null +++ b/buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.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 HYDRAULICPNEUMATICFRICTION__HYDRAULICPNEUMATICFRICTION_HPP_ +#define HYDRAULICPNEUMATICFRICTION__HYDRAULICPNEUMATICFRICTION_HPP_ + +#include + +#include +#include + + +namespace buoy_gazebo +{ +// Forward declaration +class HydraulicPneumaticFrictionPrivate; +class HydraulicPneumaticFriction : public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPreUpdate +{ +public: + /// \brief Constructor + HydraulicPneumaticFriction(); + + /// \brief Destructor + ~HydraulicPneumaticFriction() override = default; + + // Documentation inherited + void Configure( + const ignition::gazebo::Entity & _entity, + const std::shared_ptr & _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 dataPtr; +}; +} // namespace buoy_gazebo + +#endif // HYDRAULICPNEUMATICFRICTION__HYDRAULICPNEUMATICFRICTION_HPP_ diff --git a/buoy_gazebo/test/test_hydraulic_pneumatic_friction.cpp b/buoy_gazebo/test/test_hydraulic_pneumatic_friction.cpp new file mode 100644 index 00000000..ce00825c --- /dev/null +++ b/buoy_gazebo/test/test_hydraulic_pneumatic_friction.cpp @@ -0,0 +1,42 @@ +// 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 + +#include + +#include + + +// NOLINTNEXTLINE +class HydraulicPneumaticFrictionTest : public ::testing::Test +{ +public: + const std::vector speed; // m/s + const std::vector force; // N + splinter_ros::Splinter1d friction_model; + + HydraulicPneumaticFrictionTest() + : speed{-0.4F, -0.2F, -0.1F, 0.0F, 0.1F, 0.2F, 0.3F, 0.4F}, + force{1200.0F, 700.0F, 500.0F, 0.0F, -1000.0F, -1400.0F, -2100.0F, -2900.0F}, + friction_model(speed, force) + { + } +}; + +TEST_F(HydraulicPneumaticFrictionTest, ModelAccuracy) +{ + EXPECT_NEAR(1000.0, friction_model.eval(-0.3), 100.0); +} From 89ebeaa90ca4a033a0186d133adedc378c7bc002 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Fri, 14 Oct 2022 20:15:35 -0700 Subject: [PATCH 02/10] updates Signed-off-by: Dharini Dutia --- buoy_description/models/mbari_wec/model.sdf | 2 +- buoy_gazebo/CMakeLists.txt | 2 +- .../HydraulicPneumaticFriction/CMakeLists.txt | 8 ---- buoy_gazebo/src/PTOFriction/CMakeLists.txt | 8 ++++ .../PTOFriction.cpp} | 41 ++++++++++--------- .../PTOFriction.hpp} | 16 ++++---- ...tic_friction.cpp => test_pto_friction.cpp} | 13 +++--- 7 files changed, 47 insertions(+), 43 deletions(-) delete mode 100644 buoy_gazebo/src/HydraulicPneumaticFriction/CMakeLists.txt create mode 100644 buoy_gazebo/src/PTOFriction/CMakeLists.txt rename buoy_gazebo/src/{HydraulicPneumaticFriction/HydraulicPneumaticFriction.cpp => PTOFriction/PTOFriction.cpp} (78%) rename buoy_gazebo/src/{HydraulicPneumaticFriction/HydraulicPneumaticFriction.hpp => PTOFriction/PTOFriction.hpp} (75%) rename buoy_gazebo/test/{test_hydraulic_pneumatic_friction.cpp => test_pto_friction.cpp} (71%) diff --git a/buoy_description/models/mbari_wec/model.sdf b/buoy_description/models/mbari_wec/model.sdf index 4008f256..f47706f6 100644 --- a/buoy_description/models/mbari_wec/model.sdf +++ b/buoy_description/models/mbari_wec/model.sdf @@ -62,7 +62,7 @@ - + HydraulicRam diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt index 27315638..3369adaf 100644 --- a/buoy_gazebo/CMakeLists.txt +++ b/buoy_gazebo/CMakeLists.txt @@ -167,7 +167,7 @@ if(BUILD_TESTING) # Add gtests gz_add_gtest(test_stopwatch ROS) - gz_add_gtest(test_hydraulic_pneumatic_friction ROS) + gz_add_gtest(test_pto_friction ROS) endif() diff --git a/buoy_gazebo/src/HydraulicPneumaticFriction/CMakeLists.txt b/buoy_gazebo/src/HydraulicPneumaticFriction/CMakeLists.txt deleted file mode 100644 index c6fa5cb4..00000000 --- a/buoy_gazebo/src/HydraulicPneumaticFriction/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -gz_add_plugin(HydraulicPneumaticFriction - SOURCES - HydraulicPneumaticFriction.cpp - INCLUDE_DIRS - .. - EXTRA_ROS_PKGS - splinter_ros -) \ No newline at end of file diff --git a/buoy_gazebo/src/PTOFriction/CMakeLists.txt b/buoy_gazebo/src/PTOFriction/CMakeLists.txt new file mode 100644 index 00000000..51618cf4 --- /dev/null +++ b/buoy_gazebo/src/PTOFriction/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_plugin(PTOFriction + SOURCES + PTOFriction.cpp + INCLUDE_DIRS + .. + EXTRA_ROS_PKGS + splinter_ros +) \ No newline at end of file diff --git a/buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.cpp b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp similarity index 78% rename from buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.cpp rename to buoy_gazebo/src/PTOFriction/PTOFriction.cpp index f84b0d17..6b2104ee 100644 --- a/buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.cpp +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "HydraulicPneumaticFriction.hpp" +#include "PTOFriction.hpp" #include #include #include @@ -33,7 +33,7 @@ namespace buoy_gazebo { -class HydraulicPneumaticFrictionPrivate +class PTOFrictionPrivate { public: /// \brief Piston joint entity @@ -49,24 +49,26 @@ class HydraulicPneumaticFrictionPrivate const std::vector meanFriction; // N /// \brief Construct and approximation of friction model using linear spline - splinter_ros::Splinter1d hydraulic_pneumatic_friction; + splinter_ros::Splinter1d pto_friction_model; - HydraulicPneumaticFrictionPrivate() - : pistonSpeed{-0.4F, -0.3F, -0.2F, -0.1F, 0.0F, 0.1F, 0.2F, 0.3F, 0.4F}, - meanFriction{1200.0F, 1000.0F, 700.0F, 500.0F, 0.0F, -1000.0F, -1400.0F, -2100.0F, -2900.0F}, - hydraulic_pneumatic_friction(pistonSpeed, meanFriction) + 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) { } }; ////////////////////////////////////////////////// -HydraulicPneumaticFriction::HydraulicPneumaticFriction() -: dataPtr(std::make_unique()) +PTOFriction::PTOFriction() +: dataPtr(std::make_unique()) { } ////////////////////////////////////////////////// -void HydraulicPneumaticFriction::Configure( +void PTOFriction::Configure( const ignition::gazebo::Entity & _entity, const std::shared_ptr & _sdf, ignition::gazebo::EntityComponentManager & _ecm, @@ -74,7 +76,7 @@ void HydraulicPneumaticFriction::Configure( { this->dataPtr->model = ignition::gazebo::Model(_entity); if (!this->dataPtr->model.Valid(_ecm)) { - ignerr << "HydraulicPneumaticFriction plugin should be attached to a model entity. " << + ignerr << "PTOFriction plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; return; } @@ -83,7 +85,7 @@ void HydraulicPneumaticFriction::Configure( // Get params from SDF for Prismatic Joint. auto PrismaticJointName = _sdf->Get("PrismaticJointName"); if (PrismaticJointName.empty()) { - ignerr << "HydraulicPneumaticFriction found an empty PrismaticJointName parameter. " << + ignerr << "PTOFriction found an empty PrismaticJointName parameter. " << "Failed to initialize."; return; } @@ -94,17 +96,17 @@ void HydraulicPneumaticFriction::Configure( PrismaticJointName); if (this->dataPtr->PrismaticJointEntity == ignition::gazebo::kNullEntity) { ignerr << "Joint with name [" << PrismaticJointName << "] not found. " << - "The HydraulicPneumaticFriction may not influence this joint.\n"; + "The PTOFriction may not influence this joint.\n"; return; } } ////////////////////////////////////////////////// -void HydraulicPneumaticFriction::PreUpdate( +void PTOFriction::PreUpdate( const ignition::gazebo::UpdateInfo & _info, ignition::gazebo::EntityComponentManager & _ecm) { - IGN_PROFILE("HydraulicPneumaticFriction::PreUpdate"); + IGN_PROFILE("PTOFriction::PreUpdate"); // Nothing left to do if paused. if (_info.paused) { return; @@ -132,7 +134,8 @@ void HydraulicPneumaticFriction::PreUpdate( // Interpolate the new friction force based on current joint velocity auto friction_force = - this->dataPtr->hydraulic_pneumatic_friction.eval(fabs(prismaticJointVelComp->Data().at(0))); + 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( @@ -149,7 +152,7 @@ void HydraulicPneumaticFriction::PreUpdate( } // namespace buoy_gazebo IGNITION_ADD_PLUGIN( - buoy_gazebo::HydraulicPneumaticFriction, + buoy_gazebo::PTOFriction, ignition::gazebo::System, - buoy_gazebo::HydraulicPneumaticFriction::ISystemConfigure, - buoy_gazebo::HydraulicPneumaticFriction::ISystemPreUpdate); + buoy_gazebo::PTOFriction::ISystemConfigure, + buoy_gazebo::PTOFriction::ISystemPreUpdate); diff --git a/buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.hpp b/buoy_gazebo/src/PTOFriction/PTOFriction.hpp similarity index 75% rename from buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.hpp rename to buoy_gazebo/src/PTOFriction/PTOFriction.hpp index 8298fb05..31fe81ab 100644 --- a/buoy_gazebo/src/HydraulicPneumaticFriction/HydraulicPneumaticFriction.hpp +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HYDRAULICPNEUMATICFRICTION__HYDRAULICPNEUMATICFRICTION_HPP_ -#define HYDRAULICPNEUMATICFRICTION__HYDRAULICPNEUMATICFRICTION_HPP_ +#ifndef PTOFRICTION__PTOFRICTION_HPP_ +#define PTOFRICTION__PTOFRICTION_HPP_ #include @@ -24,17 +24,17 @@ namespace buoy_gazebo { // Forward declaration -class HydraulicPneumaticFrictionPrivate; -class HydraulicPneumaticFriction : public ignition::gazebo::System, +class PTOFrictionPrivate; +class PTOFriction : public ignition::gazebo::System, public ignition::gazebo::ISystemConfigure, public ignition::gazebo::ISystemPreUpdate { public: /// \brief Constructor - HydraulicPneumaticFriction(); + PTOFriction(); /// \brief Destructor - ~HydraulicPneumaticFriction() override = default; + ~PTOFriction() override = default; // Documentation inherited void Configure( @@ -50,8 +50,8 @@ class HydraulicPneumaticFriction : public ignition::gazebo::System, private: /// \brief Private data pointer - std::unique_ptr dataPtr; + std::unique_ptr dataPtr; }; } // namespace buoy_gazebo -#endif // HYDRAULICPNEUMATICFRICTION__HYDRAULICPNEUMATICFRICTION_HPP_ +#endif // PTOFRICTION__PTOFRICTION_HPP_ diff --git a/buoy_gazebo/test/test_hydraulic_pneumatic_friction.cpp b/buoy_gazebo/test/test_pto_friction.cpp similarity index 71% rename from buoy_gazebo/test/test_hydraulic_pneumatic_friction.cpp rename to buoy_gazebo/test/test_pto_friction.cpp index ce00825c..c077b150 100644 --- a/buoy_gazebo/test/test_hydraulic_pneumatic_friction.cpp +++ b/buoy_gazebo/test/test_pto_friction.cpp @@ -21,22 +21,23 @@ // NOLINTNEXTLINE -class HydraulicPneumaticFrictionTest : public ::testing::Test +class PTOFrictionTest : public ::testing::Test { public: const std::vector speed; // m/s const std::vector force; // N splinter_ros::Splinter1d friction_model; - HydraulicPneumaticFrictionTest() - : speed{-0.4F, -0.2F, -0.1F, 0.0F, 0.1F, 0.2F, 0.3F, 0.4F}, - force{1200.0F, 700.0F, 500.0F, 0.0F, -1000.0F, -1400.0F, -2100.0F, -2900.0F}, + 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(HydraulicPneumaticFrictionTest, ModelAccuracy) +TEST_F(PTOFrictionTest, ModelAccuracy) { - EXPECT_NEAR(1000.0, friction_model.eval(-0.3), 100.0); + 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); } From 30762e5712d2eca6aa98f3149d40f80fc5222454 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Mon, 7 Nov 2022 13:38:03 -0800 Subject: [PATCH 03/10] updated per feedback Signed-off-by: Dharini Dutia --- buoy_gazebo/src/PTOFriction/PTOFriction.cpp | 8 +++++--- buoy_gazebo/test/test_pto_friction.cpp | 12 ++++++++---- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp index 6b2104ee..fcea4dd0 100644 --- a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp @@ -54,8 +54,8 @@ class PTOFrictionPrivate 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}, + : pistonSpeed{-5.0, -0.4, -0.1, -0.05, 0.0, 0.05, 0.1, 0.4, 5.0}, + meanFriction{12750.0, 1200.0, 700.0, 400.0, 0.0, -750.0, -1000.0, -2900.0, -32033.0}, pto_friction_model(pistonSpeed, meanFriction) { } @@ -133,9 +133,11 @@ void PTOFriction::PreUpdate( } // Interpolate the new friction force based on current joint velocity + // Velocity sign flipped to account for the direction difference between + // sim and physical buoy auto friction_force = this->dataPtr->pto_friction_model.eval( - fabs(prismaticJointVelComp->Data().at(0)), this->dataPtr->fill_mode); + -prismaticJointVelComp->Data().at(0), this->dataPtr->fill_mode); // Create new component for applying force if it doesn't already exist auto forceComp = _ecm.Component( diff --git a/buoy_gazebo/test/test_pto_friction.cpp b/buoy_gazebo/test/test_pto_friction.cpp index c077b150..e84376e0 100644 --- a/buoy_gazebo/test/test_pto_friction.cpp +++ b/buoy_gazebo/test/test_pto_friction.cpp @@ -29,8 +29,8 @@ class PTOFrictionTest : public ::testing::Test splinter_ros::Splinter1d friction_model; PTOFrictionTest() - : speed{0.4F, 0.1F, -0.1F, -0.4F}, - force{1200.0F, 700.0F, -1000.0F, -2900.0F}, + : speed{-5.0, -0.4, -0.1, -0.05, 0.0, 0.05, 0.1, 5.0}, + force{12750.0, 1200.0, 700.0, 400.0, 0.0, -750.0, -1000.0, -32033.0}, friction_model(speed, force) { } @@ -38,6 +38,10 @@ class PTOFrictionTest : public ::testing::Test 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); + EXPECT_NEAR(0.0, friction_model.eval(0.0, splinter_ros::USE_BOUNDS), 0.05); + EXPECT_NEAR(700.0, friction_model.eval(-0.1, splinter_ros::USE_BOUNDS), 0.05); + EXPECT_NEAR(-1000.0, friction_model.eval(0.1, splinter_ros::USE_BOUNDS), 0.05); + EXPECT_NEAR(-2100.0, friction_model.eval(0.3, splinter_ros::USE_BOUNDS), 150.0); + EXPECT_NEAR(1000.0, friction_model.eval(-0.3, splinter_ros::USE_BOUNDS), 250.0); + EXPECT_NEAR(-2900.0, friction_model.eval(0.4, splinter_ros::USE_BOUNDS), 500.0); } From 2a534057174bc2e986af2eabccff07fc6354399e Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Tue, 8 Nov 2022 22:58:10 -0800 Subject: [PATCH 04/10] replacing splinter Signed-off-by: Dharini Dutia --- buoy_gazebo/CMakeLists.txt | 21 +-------------------- buoy_gazebo/src/PTOFriction/CMakeLists.txt | 2 +- buoy_gazebo/src/PTOFriction/PTOFriction.cpp | 8 +++----- buoy_gazebo/test/test_pto_friction.cpp | 16 ++++++++-------- 4 files changed, 13 insertions(+), 34 deletions(-) diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt index 1ab9f9ab..e1f9a73d 100644 --- a/buoy_gazebo/CMakeLists.txt +++ b/buoy_gazebo/CMakeLists.txt @@ -16,8 +16,6 @@ endif() find_package(ament_cmake REQUIRED) find_package(buoy_description REQUIRED) -# find_package(buoy_interfaces REQUIRED) -# find_package(rclcpp REQUIRED) find_package(ignition-cmake2 REQUIRED) find_package(ignition-plugin1 REQUIRED COMPONENTS register) @@ -114,10 +112,6 @@ install(TARGETS BuoyECMStateData INCLUDES DESTINATION include ) -install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/src/buoy_utils - DESTINATION include/buoy_gazebo - FILES_MATCHING PATTERN "*.hpp" -) install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/src/buoy_utils DESTINATION include FILES_MATCHING PATTERN "*.hpp" @@ -135,15 +129,6 @@ install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/src/PolytropicPneumaticSpring ament_export_targets(export_BuoyECMStateData) -ament_export_targets(export_Interp1d HAS_LIBRARY_TARGET) -install(TARGETS Interp1d - EXPORT export_Interp1d - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib - INCLUDES DESTINATION include -) - # Resources install(DIRECTORY worlds @@ -187,16 +172,13 @@ if(BUILD_TESTING) ) endif() if(gz_add_test_ROS) - set(ROS_PKGS rclcpp splinter_ros ${gz_add_test_EXTRA_ROS_PKGS}) + set(ROS_PKGS rclcpp ${gz_add_test_EXTRA_ROS_PKGS}) foreach(PKG ${ROS_PKGS}) find_package(${PKG} REQUIRED) endforeach() ament_target_dependencies(${TEST_NAME} ${ROS_PKGS}) endif() include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src) - target_link_libraries(${TEST_NAME} - Stopwatch - ) install( TARGETS ${TEST_NAME} DESTINATION lib/${PROJECT_NAME} @@ -211,7 +193,6 @@ if(BUILD_TESTING) endfunction() # Add gtests - gz_add_gtest(test_stopwatch ROS) gz_add_gtest(test_pto_friction ROS) endif() diff --git a/buoy_gazebo/src/PTOFriction/CMakeLists.txt b/buoy_gazebo/src/PTOFriction/CMakeLists.txt index 51618cf4..356dc639 100644 --- a/buoy_gazebo/src/PTOFriction/CMakeLists.txt +++ b/buoy_gazebo/src/PTOFriction/CMakeLists.txt @@ -4,5 +4,5 @@ gz_add_plugin(PTOFriction INCLUDE_DIRS .. EXTRA_ROS_PKGS - splinter_ros + simple_interp ) \ No newline at end of file diff --git a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp index fcea4dd0..bbcc97d3 100644 --- a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp @@ -23,7 +23,7 @@ #include -#include +#include #include #include @@ -49,9 +49,7 @@ class PTOFrictionPrivate const std::vector 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; + simple_interp::Interp1d pto_friction_model; PTOFrictionPrivate() : pistonSpeed{-5.0, -0.4, -0.1, -0.05, 0.0, 0.05, 0.1, 0.4, 5.0}, @@ -137,7 +135,7 @@ void PTOFriction::PreUpdate( // sim and physical buoy auto friction_force = this->dataPtr->pto_friction_model.eval( - -prismaticJointVelComp->Data().at(0), this->dataPtr->fill_mode); + -prismaticJointVelComp->Data().at(0)); // Create new component for applying force if it doesn't already exist auto forceComp = _ecm.Component( diff --git a/buoy_gazebo/test/test_pto_friction.cpp b/buoy_gazebo/test/test_pto_friction.cpp index e84376e0..e3f0d3a5 100644 --- a/buoy_gazebo/test/test_pto_friction.cpp +++ b/buoy_gazebo/test/test_pto_friction.cpp @@ -15,7 +15,7 @@ #include -#include +#include #include @@ -26,7 +26,7 @@ class PTOFrictionTest : public ::testing::Test public: const std::vector speed; // m/s const std::vector force; // N - splinter_ros::Splinter1d friction_model; + simple_interp::Interp1d friction_model; PTOFrictionTest() : speed{-5.0, -0.4, -0.1, -0.05, 0.0, 0.05, 0.1, 5.0}, @@ -38,10 +38,10 @@ class PTOFrictionTest : public ::testing::Test TEST_F(PTOFrictionTest, ModelAccuracy) { - EXPECT_NEAR(0.0, friction_model.eval(0.0, splinter_ros::USE_BOUNDS), 0.05); - EXPECT_NEAR(700.0, friction_model.eval(-0.1, splinter_ros::USE_BOUNDS), 0.05); - EXPECT_NEAR(-1000.0, friction_model.eval(0.1, splinter_ros::USE_BOUNDS), 0.05); - EXPECT_NEAR(-2100.0, friction_model.eval(0.3, splinter_ros::USE_BOUNDS), 150.0); - EXPECT_NEAR(1000.0, friction_model.eval(-0.3, splinter_ros::USE_BOUNDS), 250.0); - EXPECT_NEAR(-2900.0, friction_model.eval(0.4, splinter_ros::USE_BOUNDS), 500.0); + EXPECT_NEAR(0.0, friction_model.eval(0.0), 0.05); + EXPECT_NEAR(700.0, friction_model.eval(-0.1), 0.05); + EXPECT_NEAR(-1000.0, friction_model.eval(0.1), 0.05); + EXPECT_NEAR(-2100.0, friction_model.eval(0.3), 150.0); + EXPECT_NEAR(1000.0, friction_model.eval(-0.3), 250.0); + EXPECT_NEAR(-2900.0, friction_model.eval(0.4), 500.0); } From 79d51e36e67e2c1a64c0daadd41c846632828669 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Tue, 22 Nov 2022 00:28:17 -0800 Subject: [PATCH 05/10] missing dep Signed-off-by: Dharini Dutia --- buoy_gazebo/CMakeLists.txt | 2 +- buoy_gazebo/test/test_pto_friction.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt index e1f9a73d..2623fdfc 100644 --- a/buoy_gazebo/CMakeLists.txt +++ b/buoy_gazebo/CMakeLists.txt @@ -172,7 +172,7 @@ if(BUILD_TESTING) ) endif() if(gz_add_test_ROS) - set(ROS_PKGS rclcpp ${gz_add_test_EXTRA_ROS_PKGS}) + set(ROS_PKGS rclcpp simple_interp ${gz_add_test_EXTRA_ROS_PKGS}) foreach(PKG ${ROS_PKGS}) find_package(${PKG} REQUIRED) endforeach() diff --git a/buoy_gazebo/test/test_pto_friction.cpp b/buoy_gazebo/test/test_pto_friction.cpp index e3f0d3a5..96de676f 100644 --- a/buoy_gazebo/test/test_pto_friction.cpp +++ b/buoy_gazebo/test/test_pto_friction.cpp @@ -41,7 +41,7 @@ TEST_F(PTOFrictionTest, ModelAccuracy) EXPECT_NEAR(0.0, friction_model.eval(0.0), 0.05); EXPECT_NEAR(700.0, friction_model.eval(-0.1), 0.05); EXPECT_NEAR(-1000.0, friction_model.eval(0.1), 0.05); - EXPECT_NEAR(-2100.0, friction_model.eval(0.3), 150.0); - EXPECT_NEAR(1000.0, friction_model.eval(-0.3), 250.0); - EXPECT_NEAR(-2900.0, friction_model.eval(0.4), 500.0); + EXPECT_NEAR(-2100.0, friction_model.eval(0.3), 180.0); + EXPECT_NEAR(1000.0, friction_model.eval(-0.3), 50.0); + EXPECT_NEAR(-2900.0, friction_model.eval(0.4), 0.05); } From 55dcc0c0ce1728034a49cdacb3d711ab9dbdda2a Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Mon, 28 Nov 2022 01:24:44 -0800 Subject: [PATCH 06/10] correcting force signs Signed-off-by: Dharini Dutia --- buoy_gazebo/src/PTOFriction/PTOFriction.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp index bbcc97d3..76c3e941 100644 --- a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp @@ -143,9 +143,9 @@ void PTOFriction::PreUpdate( if (forceComp == nullptr) { _ecm.CreateComponent( this->dataPtr->PrismaticJointEntity, - ignition::gazebo::components::JointForceCmd({friction_force})); // Create this iteration + ignition::gazebo::components::JointForceCmd({-friction_force})); // Create this iteration } else { - forceComp->Data()[0] += friction_force; // Add friction to existing forces + forceComp->Data()[0] -= friction_force; // Add friction to existing forces } } From ff43e8e79d3cdb2479ca57918a23863b5774e305 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Mon, 28 Nov 2022 14:41:25 -0800 Subject: [PATCH 07/10] moving test Signed-off-by: Dharini Dutia --- buoy_gazebo/CMakeLists.txt | 58 ++----------------- buoy_tests/CMakeLists.txt | 4 ++ .../tests}/test_pto_friction.cpp | 0 3 files changed, 9 insertions(+), 53 deletions(-) rename {buoy_gazebo/test => buoy_tests/tests}/test_pto_friction.cpp (100%) diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt index 2623fdfc..44a2d5c8 100644 --- a/buoy_gazebo/CMakeLists.txt +++ b/buoy_gazebo/CMakeLists.txt @@ -127,6 +127,11 @@ install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/src/PolytropicPneumaticSpring FILES_MATCHING PATTERN "*.hpp" ) +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/src/PTOFriction + DESTINATION include/buoy_gazebo + FILES_MATCHING PATTERN "*.hpp" +) + ament_export_targets(export_BuoyECMStateData) # Resources @@ -142,59 +147,6 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - # GTest - find_package(ament_cmake_gtest REQUIRED) - ament_find_gtest() - - # launch_testing - find_package(launch_testing_ament_cmake REQUIRED) - - # Helper function to generate gtest - function(gz_add_gtest TEST_NAME) - set( - options - ROS - LAUNCH_TEST - GAZEBO - ) - set(oneValueArgs) - set(multiValueArgs - EXTRA_ROS_PKGS) - - cmake_parse_arguments(gz_add_test "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) - - ament_add_gtest_executable(${TEST_NAME} - test/${TEST_NAME}.cpp - ) - if(gz_add_test_GAZEBO) - target_link_libraries(${TEST_NAME} - ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER} - ) - endif() - if(gz_add_test_ROS) - set(ROS_PKGS rclcpp simple_interp ${gz_add_test_EXTRA_ROS_PKGS}) - foreach(PKG ${ROS_PKGS}) - find_package(${PKG} REQUIRED) - endforeach() - ament_target_dependencies(${TEST_NAME} ${ROS_PKGS}) - endif() - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src) - install( - TARGETS ${TEST_NAME} - DESTINATION lib/${PROJECT_NAME} - ) - if(gz_add_test_LAUNCH_TEST) - add_launch_test(test/launch/${TEST_NAME}.launch.py - TIMEOUT 300 - ) - else() - ament_add_gtest_test(${TEST_NAME}) - endif() - endfunction() - - # Add gtests - gz_add_gtest(test_pto_friction ROS) - endif() ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") diff --git a/buoy_tests/CMakeLists.txt b/buoy_tests/CMakeLists.txt index 31ec79dc..03f14fe9 100644 --- a/buoy_tests/CMakeLists.txt +++ b/buoy_tests/CMakeLists.txt @@ -158,6 +158,10 @@ if(BUILD_TESTING) buoy_add_gtest(pc_commands_ros_feedback ROS LAUNCH_TEST) buoy_add_gtest(sc_commands_ros_feedback ROS LAUNCH_TEST) + buoy_add_gtest(test_pto_friction ROS + EXTRA_ROS_PKGS + buoy_gazebo + simple_interp) # Add pytests buoy_add_pytest(no_inputs no_inputs) diff --git a/buoy_gazebo/test/test_pto_friction.cpp b/buoy_tests/tests/test_pto_friction.cpp similarity index 100% rename from buoy_gazebo/test/test_pto_friction.cpp rename to buoy_tests/tests/test_pto_friction.cpp From bf19bd1b5aa371c734b861984acf0f3c2289662c Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Tue, 29 Nov 2022 16:51:29 -0800 Subject: [PATCH 08/10] adjusting absement values Signed-off-by: Dharini Dutia --- buoy_description/models/mbari_wec/model.sdf | 8 ++++---- buoy_tests/worlds/TestMachine.sdf | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/buoy_description/models/mbari_wec/model.sdf b/buoy_description/models/mbari_wec/model.sdf index e95b7aa1..7f764365 100644 --- a/buoy_description/models/mbari_wec/model.sdf +++ b/buoy_description/models/mbari_wec/model.sdf @@ -19,8 +19,8 @@ upper_polytropic true - 4.75e-9 - 4.5e-11 + 5.75e-9 + 5.5e-11 1.7e+6 2.03 0.0127 @@ -46,8 +46,8 @@ lower_polytropic false - 4.75e-9 - 4.5e-11 + 5.75e-9 + 5.5e-11 1.7e+6 2.03 0.0115 diff --git a/buoy_tests/worlds/TestMachine.sdf b/buoy_tests/worlds/TestMachine.sdf index b11ca86d..0c5fd2d4 100644 --- a/buoy_tests/worlds/TestMachine.sdf +++ b/buoy_tests/worlds/TestMachine.sdf @@ -202,8 +202,8 @@ upper_polytropic true - 4.75e-9 - 4.5e-11 + 5.75e-9 + 5.5e-11 1.7e+6 2.03 0.0127 @@ -230,8 +230,8 @@ lower_polytropic false - 4.75e-9 - 4.5e-11 + 5.75e-9 + 5.5e-11 1.7e+6 2.03 0.0115 From 1ebd6c6bc48d12a802c8d4ed0963c4d7ee9c7d8b Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 29 Nov 2022 17:11:54 -0800 Subject: [PATCH 09/10] fixed bug in spring valve/pump; modified absements to pass tests; moved negative sign on friction and added comment Signed-off-by: Michael Anderson --- buoy_description/models/mbari_wec/model.sdf | 8 ++--- buoy_gazebo/src/PTOFriction/PTOFriction.cpp | 16 +++++----- .../PolytropicPneumaticSpring.cpp | 30 ++++++++++--------- .../launch/sc_pump_ros_feedback_py.launch.py | 8 +++-- .../launch/sc_valve_ros_feedback_py.launch.py | 8 +++-- buoy_tests/tests/sc_commands_ros_feedback.cpp | 12 ++++---- buoy_tests/worlds/TestMachine.sdf | 8 ++--- 7 files changed, 49 insertions(+), 41 deletions(-) diff --git a/buoy_description/models/mbari_wec/model.sdf b/buoy_description/models/mbari_wec/model.sdf index e95b7aa1..77bf5b2a 100644 --- a/buoy_description/models/mbari_wec/model.sdf +++ b/buoy_description/models/mbari_wec/model.sdf @@ -19,8 +19,8 @@ upper_polytropic true - 4.75e-9 - 4.5e-11 + 7.75e-6 + 5.1e-8 1.7e+6 2.03 0.0127 @@ -46,8 +46,8 @@ lower_polytropic false - 4.75e-9 - 4.5e-11 + 7.75e-6 + 5.1e-8 1.7e+6 2.03 0.0115 diff --git a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp index 76c3e941..2fcd5305 100644 --- a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp @@ -52,8 +52,10 @@ class PTOFrictionPrivate simple_interp::Interp1d pto_friction_model; PTOFrictionPrivate() - : pistonSpeed{-5.0, -0.4, -0.1, -0.05, 0.0, 0.05, 0.1, 0.4, 5.0}, - meanFriction{12750.0, 1200.0, 700.0, 400.0, 0.0, -750.0, -1000.0, -2900.0, -32033.0}, + : pistonSpeed{-5.0, -0.4, -0.1, -0.05, -0.01, 0.0, + 0.01, 0.05, 0.1, 0.4, 5.0}, + meanFriction{12750.0, 1200.0, 700.0, 400.0, 160.0, 0.0, + -550.0, -750.0, -1000.0, -2900.0, -32033.0}, pto_friction_model(pistonSpeed, meanFriction) { } @@ -131,10 +133,10 @@ void PTOFriction::PreUpdate( } // Interpolate the new friction force based on current joint velocity - // Velocity sign flipped to account for the direction difference between + // Velocity and Force sign flipped to account for the direction difference between // sim and physical buoy - auto friction_force = - this->dataPtr->pto_friction_model.eval( + double friction_force = + -this->dataPtr->pto_friction_model.eval( -prismaticJointVelComp->Data().at(0)); // Create new component for applying force if it doesn't already exist @@ -143,9 +145,9 @@ void PTOFriction::PreUpdate( if (forceComp == nullptr) { _ecm.CreateComponent( this->dataPtr->PrismaticJointEntity, - ignition::gazebo::components::JointForceCmd({-friction_force})); // Create this iteration + ignition::gazebo::components::JointForceCmd({friction_force})); // Create this iteration } else { - forceComp->Data()[0] -= friction_force; // Add friction to existing forces + forceComp->Data()[0] += friction_force; // Add friction to existing forces } } diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index 69ce02ed..631c74ee 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -101,9 +101,6 @@ struct PolytropicPneumaticSpringConfig /// \brief specific heat of gas under constant pressure (kJ/(kg K)) double c_p{1.04}; - /// \brief c=mR(specific) (Pa*m^3)/K - double c{0.0}; - /// \brief initial Volume (m^3) double V0{dead_volume}, V1{dead_volume}, V2{dead_volume}; @@ -130,6 +127,9 @@ struct PolytropicPneumaticSpringPrivate /// \brief mass of gas (kg) double mass{0.0}; + /// \brief c=mR(specific) (Pa*m^3)/K + double c{0.0}; + /// \brief current Volume (m^3) double V{0.0266}; @@ -201,7 +201,8 @@ void PolytropicPneumaticSpring::openValve( } // Ideal Gas Law - const double mRT = this->dataPtr->mass * this->dataPtr->config_->R * this->dataPtr->T; + this->dataPtr->c = this->dataPtr->mass * this->dataPtr->config_->R; + const double mRT = this->dataPtr->c * this->dataPtr->T; P1 = mRT / V1; P2 = mRT / V2; } @@ -238,7 +239,8 @@ void PolytropicPneumaticSpring::pumpOn( } // Ideal Gas Law - const double mRT = this->dataPtr->mass * this->dataPtr->config_->R * this->dataPtr->T; + this->dataPtr->c = this->dataPtr->mass * this->dataPtr->config_->R; + const double mRT = this->dataPtr->c * this->dataPtr->T; P1 = mRT / V1; P2 = mRT / V2; } @@ -262,7 +264,7 @@ void PolytropicPneumaticSpring::computeLawOfCoolingForce(const double & x, const this->dataPtr->Q_rate = h * A * dT; // Ideal Gas Law: P = (m*R)*T/V - this->dataPtr->P = this->dataPtr->config_->c * this->dataPtr->T / this->dataPtr->V; + this->dataPtr->P = this->dataPtr->c * this->dataPtr->T / this->dataPtr->V; // F = P*A this->dataPtr->F = this->dataPtr->P * this->dataPtr->config_->piston_area; @@ -278,7 +280,7 @@ void PolytropicPneumaticSpring::computePolytropicForce(const double & x, const d // polytropic relationship: P = P0*(V0/V)^n this->dataPtr->P = P0 * pow(V0 / this->dataPtr->V, this->dataPtr->n); // Ideal Gas Law: T = P*V/(m*R) - this->dataPtr->T = this->dataPtr->P * this->dataPtr->V / this->dataPtr->config_->c; + this->dataPtr->T = this->dataPtr->P * this->dataPtr->V / this->dataPtr->c; // no heat loss if adiabatic static const double cp_R = this->dataPtr->config_->c_p / this->dataPtr->config_->R; @@ -362,9 +364,9 @@ void PolytropicPneumaticSpring::Configure( igndbg << "V2: " << config.V2 << std::endl; this->dataPtr->V0 = config.V1; - config.c = this->dataPtr->P1 * config.V1 / config.T0; - igndbg << "c: " << config.c << std::endl; - this->dataPtr->mass = config.c / config.R; + this->dataPtr->c = this->dataPtr->P1 * config.V1 / config.T0; + igndbg << "c: " << this->dataPtr->c << std::endl; + this->dataPtr->mass = this->dataPtr->c / config.R; igndbg << "mass: " << this->dataPtr->mass << std::endl; } else { // no hysteresis config.n0 = SdfParamDouble(_sdf, "n", PolytropicPneumaticSpringConfig::ADIABATIC_INDEX); @@ -381,16 +383,16 @@ void PolytropicPneumaticSpring::Configure( this->dataPtr->V0 = config.V0; igndbg << "V0: " << config.V0 << std::endl; - config.c = this->dataPtr->P0 * config.V0 / config.T0; - igndbg << "c: " << config.c << std::endl; - this->dataPtr->mass = config.c / config.R; + this->dataPtr->c = this->dataPtr->P0 * config.V0 / config.T0; + igndbg << "c: " << this->dataPtr->c << std::endl; + this->dataPtr->mass = this->dataPtr->c / config.R; igndbg << "mass: " << this->dataPtr->mass << std::endl; } this->dataPtr->V = this->dataPtr->V0; this->dataPtr->P = this->dataPtr->P0; // Ideal Gas Law: T = P*V/(m*R) - this->dataPtr->T = this->dataPtr->P * this->dataPtr->V / config.c; + this->dataPtr->T = this->dataPtr->P * this->dataPtr->V / this->dataPtr->c; config.model = ignition::gazebo::Model(_entity); if (!config.model.Valid(_ecm)) { diff --git a/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py b/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py index 1e8b1a9e..4452cbb6 100644 --- a/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py +++ b/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py @@ -33,7 +33,7 @@ def test_sc_pump_ros(self, gazebo_test_fixture, proc_info): self.assertEqual(t, 0) self.assertEqual(self.test_helper.iterations, 0) - preCmdIterations = 40000 + preCmdIterations = 45000 statusCheckIterations = 1000 postCmdIterations = 60000 @@ -134,11 +134,13 @@ def test_sc_pump_ros(self, gazebo_test_fixture, proc_info): self.assertGreater(post_pump_range_finder, pre_pump_range_finder - 2.2 * 0.0254, - 'Piston should retract 2 inches/min for 1 minute') + 'Piston should retract 2 inches/min for 1 minute' + + ' -- decrease pump absement') self.assertLess(post_pump_range_finder, pre_pump_range_finder - 1.8 * 0.0254, - 'Piston should retract 2 inches/min for 1 minute') + 'Piston should retract 2 inches/min for 1 minute' + + ' -- increase pump absement') # TODO(anyone) remove once TestFixture is fixed upstream self.test_helper.stop() diff --git a/buoy_tests/launch/sc_valve_ros_feedback_py.launch.py b/buoy_tests/launch/sc_valve_ros_feedback_py.launch.py index 5ba40b28..f97b6af6 100644 --- a/buoy_tests/launch/sc_valve_ros_feedback_py.launch.py +++ b/buoy_tests/launch/sc_valve_ros_feedback_py.launch.py @@ -33,7 +33,7 @@ def test_sc_valve_ros(self, gazebo_test_fixture, proc_info): self.assertEqual(t, 0) self.assertEqual(self.test_helper.iterations, 0) - preCmdIterations = 40000 + preCmdIterations = 45000 statusCheckIterations = 1000 postCmdIterations = 5000 @@ -135,11 +135,13 @@ def test_sc_valve_ros(self, gazebo_test_fixture, proc_info): self.assertGreater(post_valve_range_finder, pre_valve_range_finder + 0.8 * 0.0254 * 5.0, - 'Piston should extend 1 inch/sec for 5 seconds') + 'Piston should extend 1 inch/sec for 5 seconds' + + ' -- increase valve absement') self.assertLess(post_valve_range_finder, pre_valve_range_finder + 1.2 * 0.0254 * 5.0, - 'Piston should extend 1 inch/sec for 5 seconds') + 'Piston should extend 1 inch/sec for 5 seconds' + + ' -- decrease valve absement') # TODO(anyone) remove once TestFixture is fixed upstream self.test_helper.stop() diff --git a/buoy_tests/tests/sc_commands_ros_feedback.cpp b/buoy_tests/tests/sc_commands_ros_feedback.cpp index 926298bf..73158c45 100644 --- a/buoy_tests/tests/sc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/sc_commands_ros_feedback.cpp @@ -165,7 +165,7 @@ class BuoySCTests : public ::testing::Test ////////////////////////////////////////////////// TEST_F(BuoySCTests, SCValveROS) { - int preCmdIterations{40000}, statusCheckIterations{1000}, postCmdIterations{5000}; + int preCmdIterations{45000}, statusCheckIterations{1000}, postCmdIterations{5000}; // Run simulation server and wait for piston to settle fixture->Server()->Run(true /*blocking*/, preCmdIterations, false /*paused*/); @@ -297,12 +297,12 @@ TEST_F(BuoySCTests, SCValveROS) EXPECT_GT( post_valve_range_finder, pre_valve_range_finder + 0.8F /*inch per sec*/ * INCHES_TO_METERS * 5.0F /*seconds*/) << - "Piston should extend 1 inch/sec for 5 seconds"; + "Piston should extend 1 inch/sec for 5 seconds -- increase valve absement"; EXPECT_LT( post_valve_range_finder, pre_valve_range_finder + 1.2F /*inch per sec*/ * INCHES_TO_METERS * 5.0F /*seconds*/) << - "Piston should extend 1 inch/sec for 5 seconds"; + "Piston should extend 1 inch/sec for 5 seconds -- decrease valve absement"; // Stop spinning node node->stop(); @@ -314,7 +314,7 @@ TEST_F(BuoySCTests, SCValveROS) ////////////////////////////////////////////////// TEST_F(BuoySCTests, SCPumpROS) { - int preCmdIterations{40000}, statusCheckIterations{1000}, postCmdIterations{60000}; + int preCmdIterations{45000}, statusCheckIterations{1000}, postCmdIterations{60000}; // Run simulation server and allow piston to settle fixture->Server()->Run(true /*blocking*/, preCmdIterations, false /*paused*/); @@ -442,12 +442,12 @@ TEST_F(BuoySCTests, SCPumpROS) EXPECT_GT( post_pump_range_finder, pre_pump_range_finder - 2.2F /*inches per minute*/ * INCHES_TO_METERS * 1.0F /*minute*/) << \ - "Piston should retract 2 inches/min for 1 minute"; + "Piston should retract 2 inches/min for 1 minute -- decrease pump absement"; EXPECT_LT( post_pump_range_finder, pre_pump_range_finder - 1.8F /*inches per minute*/ * INCHES_TO_METERS * 1.0F /*minute*/) << \ - "Piston should retract 2 inches/min for 1 minute"; + "Piston should retract 2 inches/min for 1 minute -- increase pump absement"; // Stop spinning node node->stop(); diff --git a/buoy_tests/worlds/TestMachine.sdf b/buoy_tests/worlds/TestMachine.sdf index b11ca86d..5e47e1eb 100644 --- a/buoy_tests/worlds/TestMachine.sdf +++ b/buoy_tests/worlds/TestMachine.sdf @@ -202,8 +202,8 @@ upper_polytropic true - 4.75e-9 - 4.5e-11 + 7.75e-6 + 5.1e-8 1.7e+6 2.03 0.0127 @@ -230,8 +230,8 @@ lower_polytropic false - 4.75e-9 - 4.5e-11 + 7.75e-6 + 5.1e-8 1.7e+6 2.03 0.0115 From f08c60e8a92a4499ee192762852bb23f0391772f Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 29 Nov 2022 18:20:42 -0800 Subject: [PATCH 10/10] increase test timeout slightly Signed-off-by: Michael Anderson --- buoy_tests/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buoy_tests/CMakeLists.txt b/buoy_tests/CMakeLists.txt index 03f14fe9..22621b57 100644 --- a/buoy_tests/CMakeLists.txt +++ b/buoy_tests/CMakeLists.txt @@ -89,7 +89,7 @@ if(BUILD_TESTING) if(buoy_add_gtest_LAUNCH_TEST) add_launch_test(launch/${TEST_NAME}.launch.py - TIMEOUT 600 + TIMEOUT 650 ) else() ament_add_gtest_test(${TEST_NAME}) @@ -127,7 +127,7 @@ if(BUILD_TESTING) endif() add_launch_test(launch/${PYTEST_NAME}_py.launch.py - TIMEOUT 600 + TIMEOUT 650 ) endfunction()