Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding Hydraulic Pneumatic Friction plugin #100

Merged
merged 15 commits into from
Dec 5, 2022
Merged

Conversation

quarkytale
Copy link
Contributor

A new plugin and test framework is ready. Need to work on model accuracy.

Signed-off-by: Dharini Dutia <dharini@openrobotics.org>
@hamilton8415
Copy link
Collaborator

Nice job Dharini making a clean and simple plugin to address this need. I have a few suggestions for checking/consideration. The friction force versus speed interpolation defined by:

pistonSpeed{-0.4F, -0.3F, -0.2F, -0.1F, 0.0F, 0.1F, 0.2F, 0.3F, 0.4F},
meanFriction{1200.0F, 1000.0F, 700.0F, 500.0F, 0.0F, -1000.0F, -1400.0F, -2100.0F, -2900.0F}

probably needs a larger extend, in the paper tests were done up to these speeds (+/- 0.4 m/s) but in actuality the piston could go quite a bit faster. I'd say to be sure of covering the possibilities, the chart should go up to +/- 2m/s. Because this interpolation consists of just a few straight lines, it may not be necessary to include points in the intermediate parts of the straight sections, which could speed up the interpolation some. I believe splinter may be configured to use the highest values in the interpolation table when independent variables greater than the defined extent are used, this would be good to include if it's possible. @andermi knows what's possible I believe.

Also, in what's sure to be a continual source of confusion, the internal gazebo model uses and opposite sign for the position of the ram extension compared with the paper and actual system at sea. So we need to swap the sign of the velocity relative to the paper. For now I suggest we keep this annoyance as we've addressed it throughout the code in a number of places, if we decide to change it to match, we can do it once and hopefully find all places to fix it.

Finally, I will suggest a simpler name for this plugin. I think PTO_Friction or similar is fine, the friction being simulated here is a combination of a lot of contributions, from the hydraulic ram, from the pneumatic part, from extraneous other seals, etc. So avoiding the specificity of HydraulicPneumatic might be a good idea. Not too important if it's challenging to change.

@quarkytale
Copy link
Contributor Author

That's exactly the feedback I was looking for, thanks Andy!
I saw the behaviour with speed outside the bounds I modelled for was kinda weird. I would want to add all the values inclusive of the range speed will touch. Love some help on getting that data. Otherwise, I'm not sure how to configure the splinter to work that case, will ping Michael!
Changes upcoming:

  • Switching the speed's sign
  • Changing plugin name to PTOFriction

@hamilton8415
Copy link
Collaborator

I think it's OK to just linearly extrapolate the lines we have out to +/-3m/s or thereabouts. No harm in going further and that reduces the issue with things being out of bounds. +/-5m/s would be way faster than we ever expect to see...

@hamilton8415
Copy link
Collaborator

Michael is out on the boat today, so here's an example of splinter use from elsewhere that returns the endpoints when an out of bound request is made:
const double eff_m = this->hyd_eff_m.eval(rpm, splinter_ros::USE_BOUNDS);

I haven't tested this carefully, (in michael we trust...:), but in the friction case we'd still want a wide expanse and only count on this for the unexpected case of a higher speed than expected. Otherwise I'm not sure what spinter returns when the argument is out of bounds, perhaps it extrapolates, or returns dragons...

@quarkytale quarkytale linked an issue Oct 14, 2022 that may be closed by this pull request
Signed-off-by: Dharini Dutia <dharini@openrobotics.org>
@quarkytale quarkytale marked this pull request as ready for review October 15, 2022 03:15
@quarkytale
Copy link
Contributor Author

Update the plugin as per feedback.
The model is still very inaccurate:

/home/osrf/buoy_ws/src/buoy_sim/buoy_gazebo/test/test_pto_friction.cpp:41: Failure
2: The difference between 0.0 and friction_model.eval(0.0, splinter_ros::USE_BOUNDS) is 150.00000000000006, which exceeds 0.05, where
2: 0.0 evaluates to 0,
2: friction_model.eval(0.0, splinter_ros::USE_BOUNDS) evaluates to -150.00000000000006, and
2: 0.05 evaluates to 0.050000000000000003.
2: /home/osrf/buoy_ws/src/buoy_sim/buoy_gazebo/test/test_pto_friction.cpp:42: Failure
2: The difference between -2100.0 and friction_model.eval(-0.3, splinter_ros::USE_BOUNDS) is 193.74997289851353, which exceeds 100.0, where
2: -2100.0 evaluates to -2100,
2: friction_model.eval(-0.3, splinter_ros::USE_BOUNDS) evaluates to -2293.7499728985135, and
2: 100.0 evaluates to 100.

What's our tolerance?

@andermi
Copy link
Collaborator

andermi commented Oct 19, 2022

The model is still very inaccurate:

I'm going to open a PR in a moment to add a different interpolator. We have found issues with splinter not doing the right thing, unfortunately.

Edit: here's the interp PR: #101

@mjcarroll
Copy link
Collaborator

Pending removing splinter.

Signed-off-by: Dharini Dutia <dharini@openrobotics.org>
@mjcarroll
Copy link
Collaborator

osrf/mbari_wec_utils#36 has been merged, so you should be able to update accordingly here.

@andermi andermi self-requested a review November 8, 2022 20:27
Copy link
Collaborator

@andermi andermi left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM once splinter is replaced 👍

Signed-off-by: Dharini Dutia <dharini@openrobotics.org>
Signed-off-by: Dharini Dutia <dharini@openrobotics.org>
@quarkytale
Copy link
Contributor Author

quarkytale commented Nov 9, 2022

Replaced splinter, but the build will totally fail without the motor mode PR. Shall I wait for it to be merged or change base and get this in?

@andermi
Copy link
Collaborator

andermi commented Nov 9, 2022

build will totally fail without the motor mode PR

Sorry, I didn't quite think that one through. The issue is that ElectroHydraulicPTO plugin still needs splinter_ros? ... and we can't remove the splinter_ros dependency without the switch to 1d interpolation on mechanical and volumetric efficiencies from motormode

@hamilton8415
Copy link
Collaborator

Agreed, I am running down a couple of test issues with motormode but let's aim to get that merged this week so we can have a buildable main again.

Signed-off-by: Dharini Dutia <dharini@openrobotics.org>
@quarkytale
Copy link
Contributor Author

buoy sc tests are failing since buoy position is now different with the friction added, working on updating the numbers.

Signed-off-by: Dharini Dutia <dharini@openrobotics.org>
@andermi andermi self-requested a review November 28, 2022 19:00
Copy link
Collaborator

@andermi andermi left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I just caught a couple more things

)
endif()
if(gz_add_test_ROS)
set(ROS_PKGS rclcpp simple_interp ${gz_add_test_EXTRA_ROS_PKGS})
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

simple_interp should be added as an argument to the gz_add_gtest function after EXTRA_ROS_PKGS instead of explicitly adding it here.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In order to use your include files for the friction plugin in the test in buoy_tests, copy the pattern here to install your include files

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry, I was wrong about copying that pattern to use the includes. (I wasn't thinking quite clearly!)

You need to export the friction plugin library

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

but that's only if you want to use the class directly to recreate the test you made...
Or, you could just use the gz_sim test fixture and load the plugin that way for other testing

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Either way works for me. I'l follow the pattern of other tests by using the gazebo test fixture

Comment on lines 24 to 37
class PTOFrictionTest : public ::testing::Test
{
public:
const std::vector<double> speed; // m/s
const std::vector<double> force; // N
simple_interp::Interp1d friction_model;

PTOFrictionTest()
: speed{-5.0, -0.4, -0.1, -0.05, 0.0, 0.05, 0.1, 5.0},
force{12750.0, 1200.0, 700.0, 400.0, 0.0, -750.0, -1000.0, -32033.0},
friction_model(speed, force)
{
}
};
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Try including your plugin class rather than recreate here so we test the values in used in the plugin.

}
};

TEST_F(PTOFrictionTest, ModelAccuracy)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is a good test for the interpolator.
I wonder if it would be good to add a test like:

  1. run the sim without the friction plugin disabled and measure settling time
  2. run the sim with the friction plugin enabled and the settling time should be faster

Signed-off-by: Dharini Dutia <dharini@openrobotics.org>
Signed-off-by: Dharini Dutia <dharini@openrobotics.org>
@quarkytale
Copy link
Contributor Author

Changing absement values is not affecting the piston motion

…ed negative sign on friction and added comment

Signed-off-by: Michael Anderson <anderson@mbari.org>
…into quarkytale/friction

Signed-off-by: Michael Anderson <anderson@mbari.org>
@andermi
Copy link
Collaborator

andermi commented Nov 30, 2022

Changing absement values is not affecting the piston motion

fixed with 1ebd6c6

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

@andermi andermi left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM

@quarkytale
Copy link
Contributor Author

Getting this in, working on adding a launch test Michael suggested, in another PR

@quarkytale quarkytale merged commit 3c6f9c4 into main Dec 5, 2022
@quarkytale quarkytale deleted the quarkytale/friction branch December 5, 2022 19:30
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

Hydraulic-pneumatic Friction Model
4 participants