ElectroHydraulicPTO Motor Mode #99

andermi · 2022-10-06T00:54:29Z

WIP

make solver more robust
possibly combine rpm, pressure with electronics in solver
try computing Jacobian explicitly to use in solver
try smoothing interpolation with higher order splines
incorporate motor mode to drive piston with user commanded winding current and bias current
possibly add better battery model (or make a new branch)

@hamilton8415 I resurrected motor mode and made this draft PR so we could discuss things as we work out of this same branch

Signed-off-by: Michael Anderson <anderson@mbari.org>

andermi · 2022-10-06T00:56:15Z

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp

+
+  // solver.solve will use functor `df` function to obtain Jacobian instead of
+  // computing numerically
+  const int solver_info = solver.solve(this->dataPtr->x);


I set this to solve rather than solveNumericalDiff so it would use ElectroHydraulicSoln::df from our functor

andermi · 2022-10-06T00:56:39Z

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp

+    fjac(0, 0) = 1 - J_eff_v[0U] * SecondsPerMinute * this->Q / this->HydMotorDisp;  // df0/dx0
+    fjac(0, 1) = -J_eff_v[1U] * SecondsPerMinute * this->Q / this->HydMotorDisp;  // df0/dx1
+    fjac(1, 0) =
+      (-J_eff_m[0U] * T - eff_m * dTdx0) / (this->HydMotorDisp / (2.0 * M_PI));  // df1/dx0
+    fjac(1, 1) = 1 - J_eff_m[1U] * T / (this->HydMotorDisp / (2.0 * M_PI));  // df1/dx1


check my math?

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp

buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp

andermi · 2022-10-06T01:02:33Z

https://github.com/osrf/buoy_sim/blob/decd67c5417d648c7eff11a0004a017ac2df5895/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp#L212

I didn't put this back in yet just to keep the solver simple for now

andermi · 2022-10-06T01:18:03Z

check the comparison to the test machine:
ros2 launch buoy_tests experiment_comparison_select.launch.py manual:=true

hamilton8415 · 2022-10-06T01:41:34Z

Very good, thanks for resurrecting this branch, I was looking into how to bring it up to date and you beat me to it (big surprise). One item perhaps to add to the todo list is a battery model improvement, which may or may not get itself tangled up in the solver. Currently the model used uses a constant EMF value, and an improvement would be a coloumb counter that would drive this Vemf up and down as the battery charges and discharges. The internal resistance of the battery may also be dependent upon the state of charge. I think this is orthogonal to the other work so maybe should be done in a second branch after this is done if it's not related to the solver, we will see. In any event, not a required simulation feature, but would be a nice touch. :)

Signed-off-by: Michael Anderson <anderson@mbari.org>

…o hamilton8415/motormode

…they are applied, depending now on which motor/pump quadrant the hydraulic motor is operating.

Signed-off-by: Michael Anderson <anderson@mbari.org>

…ckage Signed-off-by: Michael Anderson <anderson@mbari.org>

package. still need to modify code to reference these files.

non-linear equatoin solver.

occasionally not-converging, need to had error handling for this still.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

Signed-off-by: Michael Anderson <anderson@mbari.org>

…ode' into hamilton8415/motormode

andermi · 2022-11-14T21:52:35Z

@hamilton8415, I just pushed updates to new tests and new launch files for manual vs ci

hamilton8415 · 2022-11-15T03:31:08Z

@andermi @mjcarroll: I completed the linter work and checked the functionality of motormode in terms of the entire model and simulation, and of course also the tests related to the electro-hydraulic PTO. There are some CI errors, some output below. I'm hoping one of you could look into what's going in there as it's a bit out of my depth. I believe andermi did these originally. Thanks! Looking forward to getting this off the plate and onward with some smaller pull-requests. :)

2022-11-15T03:22:39.4222419Z The following tests FAILED:
2022-11-15T03:22:39.4224100Z 30 - launch_sc_commands_ros_feedback.launch.py (Failed)
2022-11-15T03:22:39.4225675Z 34 - launch_sc_pump_ros_feedback_py.launch.py (Failed)
2022-11-15T03:22:39.5858009Z Errors while running CTest
2022-11-15T03:22:39.5889542Z --- stderr: buoy_tests
2022-11-15T03:22:39.5891090Z Errors while running CTest

andermi · 2022-11-15T18:28:26Z

Somehow I guess I did miss your comment here! Looking into it now.

Signed-off-by: Michael Anderson <anderson@mbari.org>

andermi · 2022-11-15T20:22:21Z

I think we are good to go after a review

andermi · 2022-11-15T20:23:16Z

.gitignore

+splinter/include
+splinter/lib
+splinter/splinter
+splinter/assets
+splinter/CREDITS.md
+splinter/docs
+splinter/README_SPLINTER.md


could delete splinter reference here

andermi · 2022-11-15T20:25:54Z

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp

-  double x = prismaticJointVelComp->Data().at(0);
-  this->dataPtr->functor.I_Wind.RamPosition = 40.0;  // TODO(hamilton8415) x * 39.4;
+  double PistonPos = prismaticJointVelComp->Data().at(0);
+  this->dataPtr->functor.I_Wind.RamPosition = 40;       // 2.03 - PistonPos * 39.4;


@hamilton8415, with this like it is, I don't think it's using your ascii art in WindingCurrentTarget like you planned.

Yeah, that's right. At the moment I've been leaving that feature disabled as it's a bit second order designed to protect the physical hardware, but it's harder to break metal things in simulation. It complicates the interpretation of how the PTO model is performing so while I do test those features in "WindingCurrentTarget.hpp", I've been thinking to let things settle in a simpler case before they're put into use...

andermi · 2022-11-15T20:30:08Z

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp

+    std::cout << "=================================" << std::endl;
+    std::cout << "Warning: Numericals solver in ElectroHydraulicPTO did not converge" << std::endl;
+    std::cout << "solver info: [" << solver_info << "]" << std::endl;
+    std::cout << "=================================" << std::endl;


perhaps change std::cout to igndbg?

I would use std::stringstream to append it all together and send it to igndbg all at once in that case.

std::stringstream ss; ss << "foo\n"; ss << "bar\n"; igndbg << ss.str();

andermi · 2022-11-15T20:32:12Z

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp

+    double tau_c = 0.1;             // N-m
+    double k_v = .06 / 1000;             // N-m/RPM
+    double k_th = 20;
+    return fabs((tau_c * tanh(2 * M_PI * N / 60 / k_th) + k_v * N / 1000) * N);


I get nervous about integer literals in division

andermi · 2022-11-15T20:33:40Z

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp

+#if 0
+  int df(const VectorXd & x, MatrixXd & fjac)
+  {
+    const int n = x.size();
+    assert(fjac.rows() == n);
+    assert(fjac.cols() == n);
+    for (int k = 0; k < n; k++) {
+      for (int j = 0; j < n; j++) {
+        fjac(k, j) = 0.;
+      }
+      fjac(k, k) = 3. - 4. * x[k];
+      if (k) {fjac(k, k - 1) = -1.;}
+      if (k != n - 1) {fjac(k, k + 1) = -2.;}
+    }
+    return 0;
+  }
+#endif


ok to remove this section?

andermi · 2022-11-15T20:34:10Z

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp

+  0.9400, 0.9390, 0.9380, 0.9370, 0.9360, 0.9370, 0.9360, 0.9350,
+  0.9320, 0.9300, 0.9200, 0.9100, 0.9000, 0.8400};
+#endif
+#if 0


ok to remove section?

andermi · 2022-11-15T20:34:24Z

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp

 };
+#if 1


ok to make permanent?

andermi · 2022-11-15T20:35:35Z

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp

+struct EffV
+{
+  EffV() = default;
+
+  std::vector<double> df(const double & rpm, const double & pressure) const
+  {
+    double sech_ = 1.0 / cosh(2e-6 * (rpm + 55.0) * (pressure - 5000.0));
+
+    // rpm
+    double dfdx0 = 2.667e-11 * (pressure - 74268.6) * (pressure - 5000.0) * sech_ * sech_;
+
+    // pressure
+    double dfdx1 = 1.333e-5 * tanh(2e-6 * (rpm + 55.0) * (pressure - 5000.0)) +
+      2.667e-11 * (rpm + 55.0) * (pressure - 74268.6) * sech_ * sech_;
+
+    return std::vector<double>{dfdx0, dfdx1};
+  }
+
+  double operator()(const double & rpm, const double & pressure) const
+  {
+    return std::max(
+      0.5,
+      (-1.333e-5 * pressure + 0.99) * tanh((-2e-6 * pressure + 0.01) * (rpm + 55.0)));
+  }
+};
+
+
+struct EffM
+{
+  EffM() = default;
+
+  std::vector<double> df(const double & rpm, const double & pressure) const
+  {
+    double sech_ = 1.0 / cosh(8e-8 * (rpm - 37500.0) * (pressure + 34.0));
+
+    // rpm
+    double dfdx0 = (4.2664e-13 * rpm - 7.84e-8) * (pressure + 34.0) * sech_ * sech_ -
+      5.333e-6 * tanh((-8e-8 * rpm + 0.003) * (pressure + 34.0));
+
+    // pressure
+    double dfdx1 = 4.2664e-13 * (rpm - 183761.0) * (rpm - 37500.0) * sech_ * sech_;
+
+    return std::vector<double>{dfdx0, dfdx1};
+  }
+
+  double operator()(const double & rpm, const double & pressure) const
+  {
+    if (pressure <= 10.0) {
+      return (2.5871e-14 * rpm + 7.9528e-9) * exp((-3.9e-6 * rpm + 1.25742) * pressure) + 0.1;
+    }
+    return std::max(
+      0.1,
+      (-5.333e-6 * rpm + 0.98) * tanh((-8e-8 * rpm + 0.003) * (pressure + 34.0)));
+  }
 };


I think it's ok to remove these as we aren't doing 2D interpolation anymore

andermi · 2022-11-15T20:37:43Z

buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp

+  /*
+  double df(const double & N) const
+  {
+    if (current_override_) {
+      J_I = 0.0;
+    } else {
+      J_I = this->DefaultDamping.evalJacobian(fabs(N));
+      J_I *= this->ScaleFactor;
+
+      if (N > 0.0) {
+        J_I *= -this->RetractFactor;
+      }
+    }
+
+    return J_I;
  }
+  */


andermi · 2022-11-15T20:39:32Z

buoy_tests/launch/eh_solver.launch.py

+def find_proc(ld):
+    entities = ld.\
+        _IncludeLaunchDescription__launch_description_source.\
+        _LaunchDescriptionSource__launch_description.\
+        _LaunchDescription__entities
+    for entity in entities:
+        if type(entity) is Node:
+            if type(entity._Action__condition) is UnlessCondition:
+                return entity


@mjcarroll, I wish there was a better way to do this... (used on line 78)

Whoa, what is this trying to do exactly?

mjcarroll · 2022-11-15T21:31:40Z

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp



 namespace buoy_gazebo
 {
 class ElectroHydraulicPTOPrivate
 {
 public:
-  /// \brief Piston joint entity
+  static constexpr double RPM_TO_RAD_PER_SEC{2.0 * M_PI / 60.0};


Can we move all of the constants to a common header?

mjcarroll · 2022-11-15T21:33:13Z

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp

  // ~50GPM/600psi ~= .33472 in^3/psi -> Relief valve flow
  // above setpoint, From SUN RPECLAN data sheet
-  const double ReliefValveFlowPerPSI = 50.0 / 600.0;
+  const double ReliefValveFlowPerPSI = 30.0 / 600.0;
  const double CubicInchesPerGallon = 231.0;


More constants that we can put somewhere common?

Thanks Michael, I can make those adjustments. Not entirely sure how high up to put the constants header file, presumably package wide, i'll look to see what makes sense. Also, I'll leave the "entities" question to andermi

I think Constants.hpp in the buoy_utils folder would be reasonable and package wide.

buoy_utils/Constanst.hpp

hamilton8415 · 2022-11-19T21:46:22Z

Hi All, I've implemented most of the above suggestions and it's passing CI. For the constants, as discussed I made a header file in buoy_utils but only put physical constants in there that are useful in a variety of places, unit conversions and similar. For the constants specific to the ElectroHydraulicPTO, I left those in the class definitions as static constexpr. There may be some sense in time to pull the constants out that define the exact behavior of this PTO into a header file in ElectroHydraulicPTO, but didn't go for that level of surgery at this time, instead figuring we need to get this merged in. Thanks for all the help and suggestions, have a great weekend...

andermi · 2022-11-20T18:34:26Z

LGTM. If @mjcarroll approves, I'll merge.

andermi added 11 commits August 23, 2022 17:49

trying another interp; using eff maps from matlab

0487aac

Signed-off-by: Michael Anderson <anderson@mbari.org>

replaced interpolator; fixed tests after new efficiency maps

af87b95

Signed-off-by: Michael Anderson <anderson@mbari.org>

remove JustInterp; put limits on interpolation

b94db06

Signed-off-by: Michael Anderson <anderson@mbari.org>

fix pump in minutes

9f775b4

Signed-off-by: Michael Anderson <anderson@mbari.org>

move splinter to own package

4e8371f

Signed-off-by: Michael Anderson <anderson@mbari.org>

remove splinter from ehpto dir

27e11b8

Signed-off-by: Michael Anderson <anderson@mbari.org>

fix linters

973d81c

Signed-off-by: Michael Anderson <anderson@mbari.org>

add license and copy splinter meta to package after clone

850bcb1

Signed-off-by: Michael Anderson <anderson@mbari.org>

Merge branch 'main' into hamilton8415/motormode

b698bf4

Signed-off-by: Michael Anderson <anderson@mbari.org>

fix from merge

8cf3156

Signed-off-by: Michael Anderson <anderson@mbari.org>

comment out relief valve; try using jacobian

decd67c

Signed-off-by: Michael Anderson <anderson@mbari.org>

andermi requested a review from hamilton8415 October 6, 2022 00:54

andermi commented Oct 6, 2022

View reviewed changes

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp Outdated Show resolved Hide resolved

andermi commented Oct 6, 2022

View reviewed changes

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp Outdated Show resolved Hide resolved

andermi commented Oct 6, 2022

View reviewed changes

buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp Outdated Show resolved Hide resolved

andermi commented Oct 6, 2022

View reviewed changes

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp Outdated Show resolved Hide resolved

andermi added 2 commits October 6, 2022 10:05

remove old splinter

e22e3fc

Signed-off-by: Michael Anderson <anderson@mbari.org>

remove old splinter from cmake

cb22cb9

Signed-off-by: Michael Anderson <anderson@mbari.org>

hamilton8415 and others added 5 commits October 11, 2022 21:38

Merge branch 'hamilton8415/motormode' of github.com:osrf/buoy_sim int…

c6ad155

…o hamilton8415/motormode

Merge branch 'hamilton8415/motormode' of github.com:osrf/buoy_sim int…

26e4fd6

…o hamilton8415/motormode

Changed hydraulic motor efficiency curves to be 1D, and modified how …

9dff230

…they are applied, depending now on which motor/pump quadrant the hydraulic motor is operating.

debugging; merge from origin

401bb1d

Signed-off-by: Michael Anderson <anderson@mbari.org>

typo

901a72c

Signed-off-by: Michael Anderson <anderson@mbari.org>

andermi and others added 5 commits November 1, 2022 11:35

move buoy_utils/Interp1d to buoy_msgs repo under the simple_interp pa…

fd3efca

…ckage Signed-off-by: Michael Anderson <anderson@mbari.org>

Added BuoyA5 hydrodynamics coefficients to directory in buoy_description

b9a18d2

package. still need to modify code to reference these files.

Added tests for the winding current target and the electrohydraulic soln

7208115

non-linear equatoin solver.

Added tests and made improvements to non-linear solver. Still

c156774

occasionally not-converging, need to had error handling for this still.

modified eh_solver

6ac0295

mjcarroll marked this pull request as ready for review November 14, 2022 19:50

mjcarroll mentioned this pull request Nov 14, 2022

Add hydrodynamic coefficients to mbari_wec_base #104

Merged

mjcarroll changed the base branch from main to mjcarroll/hydrodynamic_coeffs November 14, 2022 21:03

Remove coeffs from here

5020fec

Signed-off-by: Michael Carroll <michael@openrobotics.org>

Base automatically changed from mjcarroll/hydrodynamic_coeffs to main November 14, 2022 21:44

andermi and others added 3 commits November 14, 2022 13:46

set up manual vs ci modes for new tests

e48ebd4

Signed-off-by: Michael Anderson <anderson@mbari.org>

Merge branch 'main' into hamilton8415/motormode

73a770d

Merge remote-tracking branch 'refs/remotes/origin/hamilton8415/motorm…

38baa88

…ode' into hamilton8415/motormode

Linters linting and not complaining.

3ad6e2e

fixing pump test

706c89c

Signed-off-by: Michael Anderson <anderson@mbari.org>

andermi commented Nov 15, 2022

View reviewed changes

mjcarroll reviewed Nov 15, 2022

View reviewed changes

Improved definition of constant values by adding

01ddb94

buoy_utils/Constanst.hpp

Removed leftover splinter stuff from .gitignore

07a54c3

mjcarroll approved these changes Nov 21, 2022

View reviewed changes

mjcarroll merged commit d6c0e2e into main Nov 21, 2022

mjcarroll deleted the hamilton8415/motormode branch November 21, 2022 22:32

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

ElectroHydraulicPTO Motor Mode #99

ElectroHydraulicPTO Motor Mode #99

andermi commented Oct 6, 2022 •

edited

Loading

andermi Oct 6, 2022

andermi Oct 6, 2022

andermi commented Oct 6, 2022

andermi commented Oct 6, 2022

hamilton8415 commented Oct 6, 2022

andermi commented Nov 14, 2022 •

edited

Loading

hamilton8415 commented Nov 15, 2022

andermi commented Nov 15, 2022

andermi commented Nov 15, 2022

andermi Nov 15, 2022

andermi Nov 15, 2022

hamilton8415 Nov 15, 2022

andermi Nov 15, 2022

mjcarroll Nov 15, 2022

andermi Nov 15, 2022

andermi Nov 15, 2022

andermi Nov 15, 2022

andermi Nov 15, 2022

andermi Nov 15, 2022

andermi Nov 15, 2022

andermi Nov 15, 2022

mjcarroll Nov 15, 2022

mjcarroll Nov 15, 2022

mjcarroll Nov 15, 2022

hamilton8415 Nov 15, 2022

mjcarroll Nov 15, 2022

hamilton8415 commented Nov 19, 2022

andermi commented Nov 20, 2022

		};
		#if 1

ElectroHydraulicPTO Motor Mode #99

ElectroHydraulicPTO Motor Mode #99

Conversation

andermi commented Oct 6, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

andermi commented Oct 6, 2022

andermi commented Oct 6, 2022

hamilton8415 commented Oct 6, 2022

andermi commented Nov 14, 2022 • edited Loading

hamilton8415 commented Nov 15, 2022

andermi commented Nov 15, 2022

andermi commented Nov 15, 2022

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hamilton8415 commented Nov 19, 2022

andermi commented Nov 20, 2022

andermi commented Oct 6, 2022 •

edited

Loading

andermi commented Nov 14, 2022 •

edited

Loading