From 47ce9cf07d09b72a054632312d60b1bc81e88719 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Mon, 18 Jul 2022 16:42:42 -0400 Subject: [PATCH] separate functionality of maxCmd into max/minCmd use a separate minCmd variable to scale negative input commands --- .../usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh | 12 ++++++++++-- usv_gazebo_plugins/src/usv_gazebo_dynamics_plugin.cc | 5 +++++ usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc | 7 +++++-- .../wamv_gazebo_thruster_config.xacro | 1 + 4 files changed, 21 insertions(+), 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 index d924c22db..6beb8307c 100644 --- 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: /// : Thruster mapping (0=linear; 1=GLF, nonlinear), /// default is 0 - /// :Maximum (abs val) of thrust commands, + /// :Maximum of thrust commands, /// defualt is 1.0 + /// :Minimum of thrust commands, + /// defualt is -1.0 /// : Maximum forward force [N]. /// default is 250.0 N /// : Maximum reverse force [N]. @@ -159,6 +164,7 @@ namespace gazebo /// false /// 1 /// 1.0 + /// -1.0 /// 250.0 /// -100.0 /// 1.57 @@ -172,6 +178,7 @@ namespace gazebo /// false /// 1 /// 1.0 + /// -1.0 /// 250.0 /// -100.0 /// 1.57 @@ -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 index 05aae52bc..e7323b6f2 100644 --- 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 index 2527bcb75..98b09de09 100644 --- 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 index 712404953..65e771fe5 100644 --- 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 @@ 1 1.0 + -1.0 250.0 -100.0 ${pi/2}