separate functionality of maxCmd into max/minCmd

use a separate minCmd variable to scale negative input commands
osrf · Jul 18, 2022 · 47ce9cf · 47ce9cf
1 parent cd7185b
commit 47ce9cf
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 4 deletions.
diff --git a/usv_gazebo_plugins/include/usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh b/usv_gazebo_plugins/include/usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh
@@ -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;
 
@@ -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].
@@ -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>
@@ -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>
@@ -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;
 

diff --git a/usv_gazebo_plugins/src/usv_gazebo_dynamics_plugin.cc b/usv_gazebo_plugins/src/usv_gazebo_dynamics_plugin.cc
@@ -283,6 +283,7 @@ void UsvDynamicsPlugin::Update()
   ROS_DEBUG_STREAM_THROTTLE(1.0, "forceSum :\n" << kForceSum);
 
   // Add dynamic forces/torques to link at CG
+  // (acxz): forces added here
   this->link->AddRelativeForce(
     ignition::math::Vector3d(kForceSum(0), kForceSum(1), kForceSum(2)));
   this->link->AddRelativeTorque(
@@ -294,6 +295,7 @@ void UsvDynamicsPlugin::Update()
   // Grid point location in world frame
   tf2::Vector3 bpntW(0, 0, 0);
   // For each hull
+  ROS_INFO_STREAM_THROTTLE(1.0, "tryna float");
   for (int ii = 0; ii < 2; ii++)
   {
   // Grid point in boat frame
@@ -351,6 +353,9 @@ void UsvDynamicsPlugin::Update()
       // Apply force at grid point
       // From web, Appears that position is in the link frame
       // and force is in world frame
+      // (acxz): forces added here
+      // Does this function add the contribution to the center of gravity as a
+      // torque?
       this->link->AddForceAtRelativePosition(
         ignition::math::Vector3d(0, 0, kBuoyForce),
         ignition::math::Vector3d(bpnt.x(), bpnt.y(), bpnt.z()));

diff --git a/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc b/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc
@@ -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 =
@@ -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)
@@ -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;
@@ -372,6 +374,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;

diff --git a/wamv_gazebo/urdf/thruster_layouts/wamv_gazebo_thruster_config.xacro b/wamv_gazebo/urdf/thruster_layouts/wamv_gazebo_thruster_config.xacro
@@ -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>