Add mooring #107

mabelzhang · 2022-11-22T05:50:44Z

Addresses #15

Opening for visibility. Test isn't complete yet.

Adds mooring line, chain, and seafloor.

2022-11-12-01-30_chainOnSeafloor_scale_4x.mp4

Test WIP

The test isn't able to push the buoy all the way to the bounds to test the bounds yet. Because the heave cone is so heavy, a very large force has to be applied to move the buoy just a few meters. After some iterations, an out of bounds error happens, seeming to do with collision, and the pose values go haywire.

3000 iterations of 10,000 N (is that the unit?) in x:

[Dbg] [mooring_drift.cpp:106] iter 2275, buoy link pose: 11.9083 0.003313 -2.44625
[Dbg] [mooring_drift.cpp:106] iter 2276, buoy link pose: -2.64042e+39 -9.35589e+38 6.45126e+38
/home/developer/mbari/buoy_sim_ws/src/buoy_sim/buoy_tests/tests/mooring_drift.cpp:108: Failure
Expected: ((buoyXY - anchorXY).Length()) < (maxRadius), actual: 2.80127e+39 vs 60

ODE INTERNAL ERROR 1: assertion "aabbBound >= dMinIntExact && aabbBound < dMaxIntExact" failed in collide() [collision_space.cpp:460]
Aborted (core dumped)

Note how the pose jumps from (12, 0, -2.4) to something huge. That's not the actual test failing (looks for < 60 m from anchor), that's values going out of bounds.

There are a few things I can try:

Apply ocean currents (Adds support for ocean currents gazebosim/gz-sim#800) manually on the command line and see how far that pushes the buoy
Enable GUI in the test - is there a way to do that? I didn't find a GUI parameter in ServerConfig that we can pass to the server.
Write a standalone plugin (basically identical to the one in the test) to run in the simulation and watch the GUI to check things are moving correctly.

My main uncertainty is that, since I'm not seeing the GUI, even before it goes out of bounds, I don't know if the buoy is moving to the force correctly.

Signed-off-by: Mabel Zhang <[email protected]>

mjcarroll · 2022-11-28T22:26:50Z

Check performance against main branch
Check performance with collisions available
Test instability with ApplyLinkWrench system
Implement static computation to shortcut dynamics (and as something to check against).

andermi · 2022-12-05T18:40:53Z

Implement static computation to shortcut dynamics (and as something to check against).

@mabelzhang Here's a wheel file (in a zip since github doesn't like .whl extension) for a mooring catenary calculator in python I whipped up. We can chat about it at the 2pm, or sooner if you'd like.

unzip and then pip3 install
mooring_catenary.zip

Then, you can run the gui calculator on the command line:
$ mooring_catenary_calc.py

or, import the function:
from mooring_catenary import solve_catenary
and then you can solve the catenary and compute the other values like in the calculator:

input:

V = 82.  # meters, vertical distance from buoy to anchor
H = 120.  # meters, horizontal distance from buoy to anchor
L = 160.  # meters, total length of chain
w = 20.  # N/m, weight of chain per unit length

solve:

# y: catenary curve from B to H
# B: meters, length of chain laying on the bottom, start of catenary
# c: meters, scaling factor, ratio of horizontal component of chain tension and weight of cable per unit length
y, B, c = solve_catenary(V, H, L)
T_x = c*w  # N, Horizontal component of chain tension
T_y = -w*(L - B)  # N, vertical component of chain tension @ buoy
S = L - B  # meters, length of chain suspended

output:

y = 28.14*np.cosh((x-52.38)/28.14) - 28.14
B = 52.38 (m)
c = 28.14 (m)
F_x (buoy) = -562.79 (N)
F_y (bouy) = -2152.43 (N)
F_x (anchor) = 562.79 (N)
S = 107.62 (m)

Signed-off-by: Mabel Zhang <[email protected]>

mabelzhang · 2022-12-24T05:40:25Z

I added the first pass of the catenary solver based on Slack discussions. Not sure if I did it correctly.
It's not working yet (hence converted PR to draft).
I'm getting solverInfo == 5 and nans, which I need to look into.
After that, the tension and force still need to be calculated from the solver's results.

Signed-off-by: Mabel Zhang <[email protected]>

mabelzhang · 2023-01-06T04:20:58Z

I went back to reread the Python diagram and the code. That made me fix a few things in the C++ solution class that I had misunderstood before.

It doesn't give me NaNs anymore, so I probably fixed something.

VSolver (in CatenarySoln.hpp. I think the fvec[0] here is supposed to be the x=H that is solved):

[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 5 fvec[0]: 120
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 5 fvec[0]: 120
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 159.431
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 161.174
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 5 fvec[0]: 120
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 159.431
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 161.174
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 157.793
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 165.028
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 152.096
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 189.322
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 137.247
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 4 fvec[0]: 138.85
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 5 fvec[0]: 120
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 5 fvec[0]: 120
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 159.431
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 161.174
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 5 fvec[0]: 120
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 159.431
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 161.174
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 157.793
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 165.028
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 152.096
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 189.322
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 1 fvec[0]: 137.247
[ign gazebo-1] [Dbg] [CatenarySoln.hpp:199] VSolver solverInfo: 4 fvec[0]: 138.85

This solverInfo looks okay.

HSolver (in MooringForce.cpp) just keeps printing the same thing:

[ign gazebo-1] [Dbg] [MooringForce.cpp:183] HSolver solverInfo: 5 c: 0 B: 78

This solverInfo is 5, which I think means it didn't find a solution? So maybe I have something else wrong.
Without doing anything, B is exactly L - V (160-82), which means it's finding a solution at the initial b = 0, hmm maybe that's okay? But c = 0, (since B = L - V, (L-B)^2 - V^2 is exactly 0), which I think is what we don't want? c needs to be > 0 right?

Performance is very very slow. Other than I might be doing something wrong, maybe b can be incremented faster.
I can also try moving the buoy to see if I get NaNs, or if B is still reasonable.

andermi · 2023-01-06T04:34:24Z

There's an example of solver parameters to modify in ElectroHydraulicPTO.cpp

andermi · 2023-01-06T04:37:03Z


// See MINPACK documentation for detail son this solver
// Parameters and defaults are (Scalar = double):
//           : factor(Scalar(100.))
//           , maxfev(1000)
//           , xtol(std::sqrt(NumTraits<Scalar>::epsilon()))
//           , nb_of_subdiagonals(-1)
//           , nb_of_superdiagonals(-1)
//           , epsfcn(Scalar(0.)) {}
  Eigen::HybridNonLinearSolver<ElectroHydraulicSoln> solver(this->dataPtr->functor);
  solver.parameters.xtol = 0.0001;
  solver.parameters.maxfev = 1000;
  solver.diag.setConstant(3, 1.);
  solver.useExternalScaling = true;       // Improves solution stability dramatically.

Try useExternalScaling=true

mabelzhang · 2023-01-07T06:58:31Z

With these lines locally

    vSolver.diag.setConstant(1, 1.);
    vSolver.useExternalScaling = true;

It didn't seem to make a difference to the results or the speed.

I think I must be doing something wrong in the HSolver, for it to always get solverInfo = 5... maybe I need to check my operator() again.

Tell me if this sounds wrong. The inner optimization wants y(H)-V, where y is the cosh function, so it sets fvec[0]=y(H)-V at the end. The outer optimization wants y_B(B)=H, so it first calls the inner class with initial guess x0=H, and once the inner optimization is done, it uses whatever x that is found, and just returns that? Or should it be returning y_B(B)-H (I thought that was what I was doing in f6fb01a in this line, but that was giving NaNs. Maybe I wasn't returning what I thought I was returning?

buoy_gazebo/src/MooringForce/MooringForce.cpp

buoy_gazebo/src/MooringForce/CatenarySoln.hpp

andermi · 2023-01-07T09:21:03Z

but that was giving NaNs

possibly because the initial guess of B = L - V - 0 is invalid since B is strictly less than L - V

Or should it be returning y_B(B)-H

this ^^. it should be finding the B that makes x-H == 0. Just returning x tries to make x == 0 given a B which will obviously not give x == H (unless the buoy is directly over the anchor, in which case the assumptions of this catenary calc are invalid)

the assumptions are that we can use this calculation when the mooring line is pulled sufficiently taut such that the section of the line on the floor, B, is basically pulled straight with no loops/bights. Also, the catenary equation is only valid when H > B, otherwise the line just makes a straight line starting from B up to the buoy. This all means there's some radius around the anchor where we can start to use this solution.

andermi · 2023-05-13T00:04:05Z

does it still work with the old effectiveRadius code?

mabelzhang · 2023-05-13T00:19:32Z

Yeah 7e79a40 (right before removing effectiveRadius) works. Even 959fa9e that replaces effectiveRadius with the new toleranceL works.

The test breaks starting from the merge from main, which... there's so much new stuff. From the printouts, I can tell at least one new thing is WaveBodyInteractions.cpp, which at least wasn't printing stuff prior to the merge from main.

Maybe the test just needs to be loosened, or we can set the forces to be smaller, so that the test stops before it explodes.
Actually it fails quite quickly... it doesn't last long. It fails before any mooring printout (now it is throttled to 1 second).

If I just run the simulation, it doesn't crash. Only the test crashes.

Maybe decreasing the force in the test would help. I'd have to play with it more next week.

Signed-off-by: Mabel Zhang <[email protected]>

mabelzhang · 2023-05-15T20:57:25Z

1 million is too big in the force in the test.
Decreasing to 100,000 force + 2000 iterations pass the test, but the buoy is far from the max 160 m when it's stretched out.

Playing with the numbers to find a small enough number of iterations so that the buoy is pushed to near 160 without wasting too much test time:
12000 iterations at 155.006:

[Dbg] [mooring_force.cpp:116] Final buoy link pose: 235.006 99.7026 -2.21778 Final horizontal distance between buoy and anchor: 155.006

10,000 iterations at 155.119:

[Dbg] [mooring_force.cpp:116] Final buoy link pose: 235.118 99.5661 -2.74595 Final horizontal distance between buoy and anchor: 155.119

8000 iterations at 155.103:

[Dbg] [mooring_force.cpp:116] Final buoy link pose: 234.398 85.2305 -2.77727 Final horizontal distance between buoy and anchor: 155.103

5000 iterations at 153.577:

[Dbg] [mooring_force.cpp:116] Final buoy link pose: 209.852 17.9978 -1.399 Final horizontal distance between buoy and anchor: 153.577

So I think 100,000 force and 8000 iterations is a stable place to leave the test.
f68d994 does that. Will see how CI goes.

Signed-off-by: Mabel Zhang <[email protected]>

mabelzhang · 2023-05-16T01:23:17Z

I'm not sure why one test was missing the log, and the other test was timing out. Hard to see what's happening with a lot of printouts from WaveBodyInteractions.cpp. Maybe we should throttle that like Rhys throttled the mooring plugin printouts.

Added SDF parameter to disable plugin by default. Might as well see if the tests pass with the plugin disabled.

Signed-off-by: Mabel Zhang <[email protected]>

mabelzhang · 2023-05-18T01:08:42Z

This is weird. I commented out the mooring test, and the CI is still missing logs. I don't see this happening to other PRs. Have we seen this elsewhere?

andermi · 2023-05-18T03:16:08Z

No, I haven't seen that, although github has been acting strangely the last couple days...

andermi · 2023-05-19T22:28:48Z

I reran the test and I noticed it was running for >5hrs before it was cancelled

mabelzhang · 2023-05-19T22:39:33Z

Yeah I saw that. Locally, there's a lot of printouts from WaveBodyInteractions.cpp that obscures the printouts from MooringForce.cpp. I'll probably disable the printouts from the former to look into the latter more closely.

andermi · 2023-05-20T00:18:46Z

those printouts are already masked as Debug... did you enable debug printing?

mabelzhang · 2023-05-20T01:15:10Z

I tried levels 1-4 in the first line of the mooring test that sets them, but running the test still prints all the debug printouts. I mean, the mooring printouts are debug too, and I want to see those.
The mooring printouts are now throttled to once per second, which is reasonable and reduces traffic, so they are getting buried by the other printouts.

andermi · 2023-06-01T20:34:43Z

osrf/mbari_wec_utils#51 should fix CI test issue

Signed-off-by: Mabel Zhang <[email protected]>

mabelzhang · 2023-06-16T03:37:42Z

aff4053 should fix the test now, and a few bugs I found...

The plugin now has an <enable> flag that defaults to false, so the mooring is off by default.
The test calls a service to enable the mooring plugin, so that it can be tested.

That way this PR can be merged without affecting other things.
If we have time, a launch parameter to turn it on can be added in a separate PR. If not, it can be turned on by a service call to /mooring_force/enable.

Signed-off-by: Mabel Zhang <[email protected]>

mabelzhang · 2023-06-17T04:19:13Z

Wow, CI is finally green.

mabelzhang · 2023-06-17T04:20:33Z

Recap:

Plugin is disabled by default via the <enable>false</enable> in the model.sdf file.
Plugin can be enabled live by calling /mooring_force/enable service. The test does that.
To do in another PR: Add launch parameter to enable or disable mooring.

andermi

LGTM! :)

mabelzhang added 4 commits November 2, 2022 17:24

mooring adaptation from tether

cfdd211

Signed-off-by: Mabel Zhang <[email protected]>

add seafloor and chain lying on it

482d3ef

Signed-off-by: Mabel Zhang <[email protected]>

Merge branch 'main' into mabelzhang/mooring

9e008d2

add mooring test

1ce26ba

Signed-off-by: Mabel Zhang <[email protected]>

mabelzhang requested a review from mjcarroll November 22, 2022 05:50

Merge branch 'main' into mabelzhang/mooring

01f64cc

Merge branch 'main' into mabelzhang/mooring

c45673e

mabelzhang added 3 commits December 5, 2022 16:00

Merge branch 'main' into mabelzhang/mooring

79212bd

remove SDF changes for mooring

19a8d7e

Signed-off-by: Mabel Zhang <[email protected]>

add catenary solver (not working yet)

f6fb01a

Signed-off-by: Mabel Zhang <[email protected]>

mabelzhang marked this pull request as draft December 24, 2022 05:38

Merge branch 'main' into mabelzhang/mooring

ed8b032

Fix math for y_B, return value of CatenaryHSoln. Fix comments

431ddf1

Signed-off-by: Mabel Zhang <[email protected]>

andermi reviewed Jan 7, 2023

View reviewed changes

buoy_gazebo/src/MooringForce/MooringForce.cpp Outdated Show resolved Hide resolved

andermi reviewed Jan 7, 2023

View reviewed changes

buoy_gazebo/src/MooringForce/CatenarySoln.hpp Outdated Show resolved Hide resolved

andermi reviewed Jan 7, 2023

View reviewed changes

buoy_gazebo/src/MooringForce/CatenarySoln.hpp Outdated Show resolved Hide resolved

mabelzhang self-assigned this Jan 9, 2023

mabelzhang mentioned this pull request Jan 10, 2023

Mooring #15

Closed

2 tasks

mabelzhang linked an issue Jan 12, 2023 that may be closed by this pull request

Mooring #15

Closed

2 tasks

decrease force and iterations so mooring test doesnt go out of bounds

f68d994

Signed-off-by: Mabel Zhang <[email protected]>

add SDF parameter to enable plugin

8b40c59

Signed-off-by: Mabel Zhang <[email protected]>

skip test

b97299c

Signed-off-by: Mabel Zhang <[email protected]>

Merge branch 'main' into mabelzhang/mooring

5a475c6

mabelzhang added 2 commits June 15, 2023 23:33

Add enable service. Fix enable SDF parsing. Call service from test.

aff4053

Signed-off-by: Mabel Zhang <[email protected]>

change printout to warn

f002d34

Signed-off-by: Mabel Zhang <[email protected]>

mabelzhang added 7 commits June 15, 2023 23:57

remove seafloor (no longer needed) because anchor is virtual

494c9ab

Signed-off-by: Mabel Zhang <[email protected]>

linter

78851d5

Signed-off-by: Mabel Zhang <[email protected]>

increase mooring test timeout

a312043

Signed-off-by: Mabel Zhang <[email protected]>

Merge branch 'main' into mabelzhang/mooring

84e5ae0

copyright year

a781eb4

Signed-off-by: Mabel Zhang <[email protected]>

cpplint

76a9611

Signed-off-by: Mabel Zhang <[email protected]>

copyright one line

bcf3482

Signed-off-by: Mabel Zhang <[email protected]>

andermi approved these changes Jun 20, 2023

View reviewed changes

mabelzhang merged commit a4938fb into main Jun 20, 2023

mabelzhang deleted the mabelzhang/mooring branch June 20, 2023 23:59

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add mooring #107

Add mooring #107

mabelzhang commented Nov 22, 2022 •

edited

Loading

mjcarroll commented Nov 28, 2022

andermi commented Dec 5, 2022 •

edited

Loading

mabelzhang commented Dec 24, 2022 •

edited

Loading

mabelzhang commented Jan 6, 2023 •

edited

Loading

andermi commented Jan 6, 2023

andermi commented Jan 6, 2023

mabelzhang commented Jan 7, 2023

andermi commented Jan 7, 2023

andermi commented May 13, 2023

mabelzhang commented May 13, 2023 •

edited

Loading

mabelzhang commented May 15, 2023 •

edited

Loading

mabelzhang commented May 16, 2023

mabelzhang commented May 18, 2023

andermi commented May 18, 2023

andermi commented May 19, 2023

mabelzhang commented May 19, 2023

andermi commented May 20, 2023

mabelzhang commented May 20, 2023 •

edited

Loading

andermi commented Jun 1, 2023

mabelzhang commented Jun 16, 2023

mabelzhang commented Jun 17, 2023

mabelzhang commented Jun 17, 2023

andermi left a comment

Add mooring #107

Add mooring #107

Conversation

mabelzhang commented Nov 22, 2022 • edited Loading

Test WIP

mjcarroll commented Nov 28, 2022

andermi commented Dec 5, 2022 • edited Loading

mabelzhang commented Dec 24, 2022 • edited Loading

mabelzhang commented Jan 6, 2023 • edited Loading

andermi commented Jan 6, 2023

andermi commented Jan 6, 2023

mabelzhang commented Jan 7, 2023

andermi commented Jan 7, 2023

andermi commented May 13, 2023

mabelzhang commented May 13, 2023 • edited Loading

mabelzhang commented May 15, 2023 • edited Loading

mabelzhang commented May 16, 2023

mabelzhang commented May 18, 2023

andermi commented May 18, 2023

andermi commented May 19, 2023

mabelzhang commented May 19, 2023

andermi commented May 20, 2023

mabelzhang commented May 20, 2023 • edited Loading

andermi commented Jun 1, 2023

mabelzhang commented Jun 16, 2023

mabelzhang commented Jun 17, 2023

mabelzhang commented Jun 17, 2023

andermi left a comment

Choose a reason for hiding this comment

mabelzhang commented Nov 22, 2022 •

edited

Loading

andermi commented Dec 5, 2022 •

edited

Loading

mabelzhang commented Dec 24, 2022 •

edited

Loading

mabelzhang commented Jan 6, 2023 •

edited

Loading

mabelzhang commented May 13, 2023 •

edited

Loading

mabelzhang commented May 15, 2023 •

edited

Loading

mabelzhang commented May 20, 2023 •

edited

Loading