Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add clamping #480

Open
wants to merge 4 commits into
base: gazebo_classic
Choose a base branch
from
Open

Add clamping #480

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e604bc4
require wave_gazebo_plugins to use c++17
acxz Jul 13, 2022
1cbd8c5
require usv_gazebo_plugins to use c++17
acxz Jul 13, 2022
7b763bd
separate functionality of maxCmd into max/minCmd
acxz Jul 18, 2022
d026061
add input command and output thrust clamping to thruster plugin
acxz Jul 18, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions usv_gazebo_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@ catkin_package(
CATKIN_DEPENDS message_runtime gazebo_dev roscpp wave_gazebo_plugins
)

# Plugins require c++11
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
# Plugins require c++17
set(CMAKE_CXX_FLAGS "-std=c++17 ${CMAKE_CXX_FLAGS}")

include_directories( include
${catkin_INCLUDE_DIRS}
Expand Down
12 changes: 10 additions & 2 deletions usv_gazebo_plugins/include/usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,12 @@ namespace gazebo
/// \param[in] _msg The thrust angle message to process.
public: void OnThrustAngle(const std_msgs::Float32::ConstPtr &_msg);

/// \brief Maximum abs val of incoming command.
/// \brief Maximum val of incoming command.
public: double maxCmd;

/// \brief Minimum val of incoming command.
public: double minCmd;

/// \brief Max forward force in Newtons.
public: double maxForceFwd;

Expand Down Expand Up @@ -134,8 +137,10 @@ namespace gazebo
/// Optional elements:
/// <mappingType>: Thruster mapping (0=linear; 1=GLF, nonlinear),
/// default is 0
/// <maxCmd>:Maximum (abs val) of thrust commands,
/// <maxCmd>:Maximum of thrust commands,
/// defualt is 1.0
/// <minCmd>:Minimum of thrust commands,
/// defualt is -1.0
/// <maxForceFwd>: Maximum forward force [N].
/// default is 250.0 N
/// <maxForceRev>: Maximum reverse force [N].
Expand All @@ -159,6 +164,7 @@ namespace gazebo
/// <enableAngle>false</enableAngle>
/// <mappingType>1</mappingType>
/// <maxCmd>1.0</maxCmd>
/// <minCmd>-1.0</minCmd>
/// <maxForceFwd>250.0</maxForceFwd>
/// <maxForceRev>-100.0</maxForceRev>
/// <maxAngle>1.57</maxAngle>
Expand All @@ -172,6 +178,7 @@ namespace gazebo
/// <enableAngle>false</enableAngle>
/// <mappingType>1</mappingType>
/// <maxCmd>1.0</maxCmd>
/// <minCmd>-1.0</minCmd>
/// <maxForceFwd>250.0</maxForceFwd>
/// <maxForceRev>-100.0</maxForceRev>
/// <maxAngle>1.57</maxAngle>
Expand Down Expand Up @@ -208,6 +215,7 @@ namespace gazebo
/// \return Value scaled and saturated.
private: double ScaleThrustCmd(const double _cmd,
const double _max_cmd,
const double _min_cmd,
const double _max_pos,
const double _max_neg) const;

Expand Down
17 changes: 15 additions & 2 deletions usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -232,6 +232,7 @@ void UsvThrust::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
}

thruster.maxCmd = this->SdfParamDouble(thrusterSDF, "maxCmd", 1.0);
thruster.minCmd = this->SdfParamDouble(thrusterSDF, "minCmd", -1.0);
thruster.maxForceFwd =
this->SdfParamDouble(thrusterSDF, "maxForceFwd", 250.0);
thruster.maxForceRev =
Expand Down Expand Up @@ -297,7 +298,7 @@ void UsvThrust::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)

//////////////////////////////////////////////////
double UsvThrust::ScaleThrustCmd(const double _cmd, const double _maxCmd,
const double _maxPos, const double _maxNeg) const
const double _minCmd, const double _maxPos, const double _maxNeg) const
{
double val = 0.0;
if (_cmd >= 0.0)
Expand All @@ -308,7 +309,8 @@ double UsvThrust::ScaleThrustCmd(const double _cmd, const double _maxCmd,
else
{
double absMaxNeg = std::abs(_maxNeg);
val = _cmd / _maxCmd * absMaxNeg;
double absMinCmd = std::abs(_minCmd);
val = _cmd / absMinCmd * absMaxNeg;
val = std::max(val, -1.0 * absMaxNeg);
}
return val;
Expand Down Expand Up @@ -364,6 +366,12 @@ void UsvThrust::Update()
// Adjust thruster engine joint angle with PID
this->RotateEngine(i, now - this->thrusters[i].lastAngleUpdateTime);

// Apply input command clamping
this->thrusters[i].currCmd = std::min(this->thrusters[i].currCmd,
this->thrusters[i].maxCmd);
this->thrusters[i].currCmd = std::max(this->thrusters[i].currCmd,
this->thrusters[i].minCmd);

// Apply the thrust mapping
ignition::math::Vector3d tforcev(0, 0, 0);
switch (this->thrusters[i].mappingType)
Expand All @@ -372,6 +380,7 @@ void UsvThrust::Update()
tforcev.X() = this->ScaleThrustCmd(this->thrusters[i].currCmd/
this->thrusters[i].maxCmd,
this->thrusters[i].maxCmd,
this->thrusters[i].minCmd,
this->thrusters[i].maxForceFwd,
this->thrusters[i].maxForceRev);
break;
Expand All @@ -387,6 +396,10 @@ void UsvThrust::Update()
break;
}

// Apply thrust clamping
tforcev.X() = std::max(tforcev.X(), this->thrusters[i].maxForceFwd);
tforcev.X() = std::min(tforcev.X(), this->thrusters[i].maxForceRev);

// Apply force for each thruster
this->thrusters[i].link->AddLinkForce(tforcev);

Expand Down
1 change: 1 addition & 0 deletions wamv_gazebo/urdf/thruster_layouts/wamv_gazebo_thruster_config.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
<!-- Optional Parameters -->
<mappingType>1</mappingType>
<maxCmd>1.0</maxCmd>
<minCmd>-1.0</minCmd>
<maxForceFwd>250.0</maxForceFwd>
<maxForceRev>-100.0</maxForceRev>
<maxAngle>${pi/2}</maxAngle>
Expand Down
16 changes: 4 additions & 12 deletions wave_gazebo_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,21 +2,13 @@ cmake_minimum_required(VERSION 2.8.3)
project(wave_gazebo_plugins)

###############################################################################
# Compile as C++11, supported in ROS Kinetic and newer

#set(CMAKE_CXX_STANDARD 11)
#set(CMAKE_CXX_STANDARD_REQUIRED ON)

# For this package set as C++14
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX14)
set(CMAKE_CXX_FLAGS "-std=c++14")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "-std=c++0x")
CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
if(COMPILER_SUPPORTS_CXX17)
add_compile_options(-std=c++17)
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler.")
endif()

# Set policy for CMake 3.1+. Use OLD policy to let FindBoost.cmake, dependency
Expand Down