Skip to content

Commit

Permalink
add mappingType=2, linear interp
Browse files Browse the repository at this point in the history 
this mappingType allows users to specify a mapping of thrust commands and thrust forces as seen typically in thrust profile datasheets; linear interpolation using the two nearest points in the mapping is done to compute the actual thrust force from a thrust command
  • Loading branch information
@acxz
acxz committed Aug 3, 2022
1 parent 13fed23 commit d3cf6b7
Show file tree
Hide file tree
Showing 2 changed files with 173 additions and 2 deletions.
77 changes: 75 additions & 2 deletions usv_gazebo_plugins/include/usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,9 +69,13 @@ namespace gazebo
/// \brief Link where thrust force is applied.
public: physics::LinkPtr link;

/// \brief Thruster mapping (0=linear; 1=GLF, nonlinear).
/// \brief Thruster mapping (0=linear; 1=GLF, nonlinear; 2=linear interp).
public: int mappingType;

/// \brief Lookup map of cmd values -> force values
/// for use in mappingType=2,linear interp
public: std::map<double, double> cmdForceMap;

/// \brief Topic name for incoming ROS thruster commands.
public: std::string cmdTopic;

Expand Down Expand Up @@ -135,7 +139,8 @@ namespace gazebo
/// <enableAngle>: If true, thruster will have adjustable angle.
/// If false, thruster will have constant angle.
/// Optional elements:
/// <mappingType>: Thruster mapping (0=linear; 1=GLF, nonlinear),
/// <mappingType>: Thruster mapping (0=linear; 1=GLF, nonlinear;
/// 2=linear interp),
/// default is 0
/// <maxCmd>:Maximum of thrust commands,
/// defualt is 1.0
Expand All @@ -145,6 +150,10 @@ namespace gazebo
/// default is 250.0 N
/// <maxForceRev>: Maximum reverse force [N].
/// default is -100.0 N
/// <cmdValues>: values of thrust commands corresponding to thrust forces
/// default is minCmd maxCmd
/// <forceValues>: values of thrust forces corresponding to thrust commands
/// default is maxForceRev maxForceFwd
/// <maxAngle>: Absolute value of maximum thruster angle [radians].
/// default is pi/2
///
Expand Down Expand Up @@ -184,6 +193,46 @@ namespace gazebo
/// <maxAngle>1.57</maxAngle>
/// </thruster>
/// </plugin>
///
/// Here is an equivalent example but using a mapping type of 2,linear interp:
///
/// <plugin name="example" filename="libusv_gazebo_thrust_plugin.so">
/// <!-- General plugin parameters -->
/// <cmdTimeout>1.0</cmdTimeout>
///
/// <thruster>
/// <linkName>left_propeller_link</linkName>
/// <propJointName>left_engine_propeller_joint</propJointName>
/// <engineJointName>left_chasis_engine_joint</engineJointName>
/// <cmdTopic>left_thrust_cmd</cmdTopic>
/// <angleTopic>left_thrust_angle</angleTopic>
/// <enableAngle>false</enableAngle>
/// <mappingType>2</mappingType>
/// <maxCmd>1.0</maxCmd>
/// <minCmd>-1.0</minCmd>
/// <maxForceFwd>250.0</maxForceFwd>
/// <maxForceRev>-100.0</maxForceRev>
/// <cmdValues>-1.0 0 1.0</cmdValues>
/// <ForceValues>-100.0 0 250.0</cmdValues>
/// <maxAngle>1.57</maxAngle>
/// </thruster>
/// <thruster>
/// <linkName>right_propeller_link</linkName>
/// <propJointName>right_engine_propeller_joint</propJointName>
/// <engineJointName>right_chasis_engine_joint</engineJointName>
/// <cmdTopic>right_thrust_cmd</cmdTopic>
/// <angleTopic>right_thrust_angle</angleTopic>
/// <enableAngle>false</enableAngle>
/// <mappingType>2</mappingType>
/// <maxCmd>1.0</maxCmd>
/// <minCmd>-1.0</minCmd>
/// <maxForceFwd>250.0</maxForceFwd>
/// <maxForceRev>-100.0</maxForceRev>
/// <cmdValues>-1.0 0 1.0</cmdValues>
/// <ForceValues>-100.0 0 250.0</cmdValues>
/// <maxAngle>1.57</maxAngle>
/// </thruster>
/// </plugin>

class UsvThrust : public ModelPlugin
{
Expand All @@ -210,6 +259,22 @@ namespace gazebo
const std::string &_paramName,
const double _defaultVal) const;

/// \brief Convenience function for getting SDF parameters.
/// \param[in] _sdfPtr Pointer to an SDF element to parse.
/// \param[in] _paramName The name of the vector element to parse.
/// \param[in] _defaultVal The default vector value returned if the element
/// does not exist.
/// \return The vector value parsed.
private: std::vector<double> SdfParamVector(
sdf::ElementPtr _sdfPtr,
const std::string &_paramName,
const std::string _defaultValString) const;

/// \brief Conversion of a string to a double vector
/// \param[in] _input The string to convert to a vector of doubles.
/// \return The vector converted from the input string.
private: std::vector<double> StrToVector(std::string _input) const;

/// \brief Takes ROS Drive commands and scales them by max thrust.
/// \param[in] _cmd ROS drive command.
/// \return Value scaled and saturated.
Expand Down Expand Up @@ -245,6 +310,14 @@ namespace gazebo
const double _maxPos,
const double _maxNeg) const;

/// \brief Uses linear interpolatoin between given thrust command
/// to thruster force mapping.
/// \param[in] _cmd Thrust command.
/// \param[in] _cmdForceMap Mapping b/t thrust command and thrust force.
/// \return Thrust force [N].
private: double LinearInterpThrustCmd(const double _cmd,
const std::map<double, double> _cmdForceMap) const;

/// \brief Rotate engine using engine joint PID
/// \param[in] _i Index of thruster whose engine will be rotated
/// \param[in] _stepTime common::Time since last rotation
Expand Down
98 changes: 98 additions & 0 deletions usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@

#include <cmath>
#include <functional>
#include <vector>

#include "usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh"

Expand Down Expand Up @@ -82,6 +83,36 @@ double UsvThrust::SdfParamDouble(sdf::ElementPtr _sdfPtr,
return val;
}

//////////////////////////////////////////////////
std::vector<double> UsvThrust::SdfParamVector(sdf::ElementPtr _sdfPtr,
const std::string &_paramName, const std::string _defaultValString) const
{
if (!_sdfPtr->HasElement(_paramName))
{
ROS_INFO_STREAM("Parameter <" << _paramName << "> not found: "
"Using default value of <" << _defaultValString << ">.");
std::vector<double> _defaultVal = StrToVector(_defaultValString);
return _defaultVal;
}

std::string valString = _sdfPtr->Get<std::string>(_paramName);
std::vector<double> val = StrToVector(valString);
ROS_DEBUG_STREAM("Parameter found - setting <" << _paramName <<
"> to <" << valString << ">.");
return val;
}

//////////////////////////////////////////////////
std::vector<double> UsvThrust::StrToVector(std::string _input) const
{
std::vector<double> output;
std::string buf;
std::stringstream ss(_input);
while (ss >> buf)
output.push_back(std::stod(buf));
return output;
}

//////////////////////////////////////////////////
void UsvThrust::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
Expand Down Expand Up @@ -240,6 +271,33 @@ void UsvThrust::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
thruster.maxAngle = this->SdfParamDouble(thrusterSDF, "maxAngle",
M_PI / 2);

if (thruster.mappingType == 2)
{
std::string defaultCmdValuesString = std::to_string(thruster.minCmd)
+ " "
+ std::to_string(thruster.maxCmd);
std::vector<double> cmdValues = this->SdfParamVector(thrusterSDF,
"cmdValues",
defaultCmdValuesString);
std::string defaultForceValuesString =
std::to_string(thruster.maxForceRev)
+ " "
+ std::to_string(thruster.maxForceFwd);
std::vector<double> forceValues = this->SdfParamVector(thrusterSDF,
"forceValues",
defaultForceValuesString);
if (cmdValues.size() != forceValues.size())
ROS_FATAL_STREAM("cmdValues and forceValues size must match!");

if (cmdValues.size() < 1)
ROS_FATAL_STREAM("need at least one cmdValues/forceValues pair!");

for (size_t i = 0; i < cmdValues.size(); ++i)
{
thruster.cmdForceMap[cmdValues[i]] = forceValues[i];
}
}

// Push to vector and increment
this->thrusters.push_back(thruster);
thrusterSDF = thrusterSDF->GetNextElement("thruster");
Expand Down Expand Up @@ -342,6 +400,41 @@ double UsvThrust::GlfThrustCmd(const double _cmd,
return val;
}

//////////////////////////////////////////////////
double UsvThrust::LinearInterpThrustCmd(const double _cmd,
const std::map<double, double> _cmdForceMap) const
{
double val = 0.0;

// first element whose key is NOT considered to go before _cmd
auto iter = _cmdForceMap.lower_bound(_cmd);

if (iter == _cmdForceMap.end())
{
// all keys are considered to go before
// last element is closest
return _cmdForceMap.rbegin()->second;
}

double i1 = iter->first;
double o1 = iter->second;

if (iter == _cmdForceMap.begin())
return o1;

iter--;

double i0 = iter->first;
double o0 = iter->second;

double w1 = _cmd - i0;
double w0 = i1 - _cmd;

val = (o0*w0 + o1*w1)/(w0 + w1);

return val;
}

//////////////////////////////////////////////////
void UsvThrust::Update()
{
Expand Down Expand Up @@ -390,6 +483,11 @@ void UsvThrust::Update()
this->thrusters[i].maxForceFwd,
this->thrusters[i].maxForceRev);
break;
case 2:
tforcev.X() = this->LinearInterpThrustCmd(
this->thrusters[i].currCmd,
this->thrusters[i].cmdForceMap);
break;
default:
ROS_FATAL_STREAM("Cannot use mappingType=" <<
this->thrusters[i].mappingType);
Expand Down

0 comments on commit d3cf6b7

Please sign in to comment.