Update mooring force calculation

[srmainwaring]

This PR updates the mooring force calculation to replace one of the solver loops with a closed form expression and an alternative estimate for the initial solver position.

See https://github.com/osrf/buoy_sim/pull/107#issuecomment-1454844684 for details.

In addition to these changes it is recommended to add checks on edge cases (where the solver is not expected to converge), and possible a bracket check, however the Eigen newton solver does not specifically require this.

[srmainwaring]

Hi @mabelzhang, thanks for the review feedback. I've not forgotten about this and will come back with updates. In the meanwhile I've borrowed the code for another project - and have made some more wholesales changes removing the specific dependencies on the buoy and including the chain weight contribution. I think this plugin would be very useful upstreamed to gz-sim7. Happy to help with this if you want to follow up?

Update: 29 Mar 2023

• Applied review feedback and updated code style to gz norms.
• Remove some unused variables and renamed remaining link to just 'link'.
• Default name is "Buoy" but this should be set in the plugin params.
• Ditto the anchor position should always be set.
• Chain w parameter is mass per metre * gravity_accel, should set gravity as constant and pass mass per metre in the plugin params.
• I have not added the force updates that include vertical component and allow you to remove the effective radius param, but can do if you are happy with the proposal.

[srmainwaring]

We're not actually using the weight w right now - Tz is unused, by design. Does that still align with your intention for the tolerance?

I haven't change the tolerance calculation in the PR, and don't think it is affected by the inclusion / exclusion of the vertical component.

To implement the tolerance changes for my own version I've replaced

  // Update V and H based on latest buoy position
  this->dataPtr->UpdateVH(_ecm);
  // If buoy is not far enough from anchor for there to be a need to pull it
  // back, no need to apply mooring force
  if (this->dataPtr->H < this->dataPtr->effectiveRadius)
  {
    if (!this->dataPtr->notifiedInsideEffectiveRadius)
    {
      this->dataPtr->notifiedInsideEffectiveRadius = true;
      gzmsg << "Buoy horizontal radius H [" << this->dataPtr->H << "]"
            << " is inside effective radius R ["
            << this->dataPtr->effectiveRadius << "]"
            << " skipping force update." << std::endl;
    }
    return;
  }
  this->dataPtr->notifiedInsideEffectiveRadius = false;

with

  // Update V and H based on latest buoy position
  this->dataPtr->UpdateVH(_ecm);

  // Skip solver if the chain can drop vertically (within tolerance).
  double toleranceL = 0.1;
  if (this->dataPtr->V + this->dataPtr->H <= this->dataPtr->L + toleranceL)
  {
    // Assume all force is vertical.
    double Tz = - this->dataPtr->w * this->dataPtr->V;

    math::Vector3d force(0.0, 0.0, Tz);
    math::Vector3d torque = math::Vector3d::Zero;
    if (force.IsFinite() && torque.IsFinite())
    {
      // this->dataPtr->link.SetVisualizationLabel("Mooring");
      this->dataPtr->link.AddWorldWrench(_ecm, force, torque);
    }
    else
    {
      gzerr << "[Mooring] force is not finite.\n";
    }
    return;
  }

When the slack has not been taken up the chain hangs vertically and the buoy is subject to the weight of the chain. As soon as the buoy starts to move further away from the anchor the chain must lift off the seabed a little, and so the horizontal force is also added. The net force will initially be mostly directed down, with the horizontal component increasing as the buoy drifts further from the anchor point. The behaviour is (nearly) continuous in direction and magnitude. The small tolerance is to prevent the solver from failing to converge by asking it to find a catenary solution when the chain is almost vertical. The full implementation is in the link to my asv_sim repo if you want to see all the changes.

If you set an arbitrary cutoff the force will change from zero to a possibly large value in one time step. If the buoy is moving back and forth across that cutoff it may introduce some odd signals into the physics, which may or may not be material.

If you'd like to follow up on why I made the changes and would recommend them I'd be happy to follow up with a call (maybe the Monday meeting next week). I've use them in simulations for 3-4 light buoys (which move a lot) in moderate sea states in simulations that ran for 20 - 30 minutes RT with no convergence issues on the solver.

[mabelzhang]

Gonna merge into target PR and try luck with CI there. CI is timing out probably due to usual CI machine slowness. This PR is not changing any tests, and the mooring test is passing for me locally.