search

patrykcieslak / stonefish

Stonefish - an advanced C++ simulation library designed for (but not limited to) marine robotics.
https://stonefish.readthedocs.io/
GNU General Public License v3.0
115 stars 31 forks source link

Actuators behaving weirdly when near the surface #10

Closed lhcubillos closed 4 years ago

lhcubillos commented 4 years ago

Describe the bug Hi! First of all thank you for your amazing library! I work for an underwater robotics laboratory in Chile, and we have benefited a lot from the possibility of accurately modeling our robots! We've seen one problem, though: whenever the robot goes to the surface, it starts spinning endlessly. Checking the actuators in the Debug menu, I see that no force is being done by the actuators until it reaches the surface, and then it starts in full, even though no new commands are being sent. If I try with a simpler model (just a box with 6 thrusters), the problem doesn't appear.

To Reproduce The model of the ROV is quite complex, but this is how we are creating it:

//Create ROVerto body
//Main Frame
sf::Polyhedron* profileO = new sf::Polyhedron("ProfileOrigin", path_data + "/bperfil_59,5_ORIGIN_.stl", 1, sf::I4(), "ProfileMat", sf::BodyPhysicsType::SUBMERGED, "metal_gray");
profileO->ScalePhysicalPropertiesToArbitraryMass(1.364);
sf::Polyhedron* profile53 = new sf::Polyhedron("ProfileHorizontalLateral", path_data + "/bperfil_53_.stl", 1, sf::I4(), "ProfileMat", sf::BodyPhysicsType::SUBMERGED, "metal_gray");
profile53->ScalePhysicalPropertiesToArbitraryMass(1.763);
sf::Polyhedron* profile11 = new sf::Polyhedron("ProfileVerticalFeet", path_data + "/bperfil_11,0_.stl", 1, sf::I4(), "ProfileMat", sf::BodyPhysicsType::SUBMERGED, "metal_gray");
profile11->ScalePhysicalPropertiesToArbitraryMass(.366);
sf::Polyhedron* profile41 = new sf::Polyhedron("ProfileHorizontalInner", path_data + "/bperfil_59,5_ORIGIN_.stl", 1, sf::I4(), "ProfileMat", sf::BodyPhysicsType::SUBMERGED, "metal_gray");
profile41->ScalePhysicalPropertiesToArbitraryMass(1.364);
sf::Polyhedron* sheet_acr = new sf::Polyhedron("Sheet", path_data + "/bsoporte_lateral_.stl", 1, sf::I4(), "AcrMat", sf::BodyPhysicsType::SUBMERGED, "polim_white");
sheet_acr->ScalePhysicalPropertiesToArbitraryMass(1.11);
sf::Polyhedron* top_acr = new sf::Polyhedron("TopAcrylic", path_data + "/bsoporte_acr_.stl", 1, sf::I4(), "AcrMat", sf::BodyPhysicsType::SUBMERGED, "polim_white");
top_acr->ScalePhysicalPropertiesToArbitraryMass(.142);
sf::Polyhedron* foam_feet = new sf::Polyhedron("FoamFeet", path_data + "/bespuma_pata_.stl", 1, sf::I4(), "FoamMat", sf::BodyPhysicsType::SUBMERGED, "yellow");
foam_feet->ScalePhysicalPropertiesToArbitraryMass(.01);
// sf::Cylinder* cylinder = new sf::Cylinder("cylinder", .3, .1, sf::I4(), "BoxMat", sf::BodyPhysicsType::SUBMERGED, "yellow");

//Inner Components
sf::Polyhedron* bottle = new sf::Polyhedron("AluminiumBottle", path_data + "/bbotella_alu_.stl", 1, sf::I4(), "FoamMat", sf::BodyPhysicsType::SUBMERGED, "black");
bottle->ScalePhysicalPropertiesToArbitraryMass(3.7); // Peso promedio entre ambas botellas
sf::Polyhedron* camera = new sf::Polyhedron("Camera", path_data + "/bcamara_.stl", 1, sf::I4(), "CamMat", sf::BodyPhysicsType::SUBMERGED, "black");
camera->ScalePhysicalPropertiesToArbitraryMass(1.718);
sf::Polyhedron* supp_cams_l = new sf::Polyhedron("SupportCameraLow", path_data + "/bsoporte_cams_abj_.stl", 1, sf::I4(), "SuppMat", sf::BodyPhysicsType::SUBMERGED, "black");
supp_cams_l->ScalePhysicalPropertiesToArbitraryMass(.277);
sf::Polyhedron* supp_cams_h = new sf::Polyhedron("SupportCameraHigh", path_data + "/bsoporte_cams_arr_.stl", 1, sf::I4(), "SuppMat", sf::BodyPhysicsType::SUBMERGED, "black");
supp_cams_h->ScalePhysicalPropertiesToArbitraryMass(.655);
sf::Polyhedron* supp_sens = new sf::Polyhedron("SupportSensor", path_data + "/bsoporte_sens_.stl", 1, sf::I4(), "SuppMat", sf::BodyPhysicsType::SUBMERGED, "black");
supp_sens->ScalePhysicalPropertiesToArbitraryMass(1.189);
sf::Polyhedron* light = new sf::Polyhedron("LightBottle", path_data + "/bluz_.stl", 1, sf::I4(), "LightMat", sf::BodyPhysicsType::SUBMERGED, "black");
light->ScalePhysicalPropertiesToArbitraryMass(.668);
sf::Polyhedron* ROV_sensor = new sf::Polyhedron("ROVSensor", path_data + "/bsensor_.stl", 1, sf::I4(), "SensorMat", sf::BodyPhysicsType::SUBMERGED, "black");
ROV_sensor->ScalePhysicalPropertiesToArbitraryMass(7.2);
sf::Polyhedron* foam_block_1 = new sf::Polyhedron("FoamBlock", path_data + "/bespuma_.stl", 1, sf::I4(), "FoamMat", sf::BodyPhysicsType::SUBMERGED, "yellow");
// foam_block_1->ScalePhysicalPropertiesToArbitraryMass(.123);
sf::Box* foam_block_2 = new sf::Box("FoamBlock2", sf::Vector3(.1,.12,.035), sf::I4(), "FoamMat", sf::BodyPhysicsType::SUBMERGED, "yellow");
sf::Box* foam_block_3 = new sf::Box("FoamBlock3", sf::Vector3(.08,.12,.3), sf::I4(), "FoamMat", sf::BodyPhysicsType::SUBMERGED, "yellow");
sf::Polyhedron* foam_corner = new sf::Polyhedron("FoamCorner", path_data + "/bespuma_esq_.stl", 1, sf::I4(), "FoamMat", sf::BodyPhysicsType::SUBMERGED, "yellow");
foam_corner->ScalePhysicalPropertiesToArbitraryMass(.016);
sf::Polyhedron* propeller = new sf::Polyhedron("Propeller", path_data + "/bT200_.stl", 1, sf::Transform(sf::Quaternion(M_PI_2, 0, 0)), "ThrustMat", sf::BodyPhysicsType::SUBMERGED, "black");
propeller->ScalePhysicalPropertiesToArbitraryMass(.34382302);

//NOTE: investigate variable "isBuoyant". In the example UnderwaterTest they set it false.

//Build body
vehicle = new sf::Compound("Vehicle", profileO, sf::I4(), sf::BodyPhysicsType::SUBMERGED);
vehicle->AddExternalPart(profile11, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.250, .220500, 0.07)));
vehicle->AddExternalPart(profile11, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.250, .220500, 0.07)));
vehicle->AddExternalPart(profile11, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.250, -.220500, 0.07)));
vehicle->AddExternalPart(profile11, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.250, -.220500, 0.07)));
vehicle->AddExternalPart(profile41, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.090, 0, -0.154)));
vehicle->AddExternalPart(profile41, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0, 0, -0.315)));
vehicle->AddExternalPart(profile53, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0, .220500, 0)));
vehicle->AddExternalPart(profile53, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0, -.220500, 0)));
vehicle->AddExternalPart(profile53, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0, .220500, -0.154)));
vehicle->AddExternalPart(profile53, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0, -.220500, -0.154)));
vehicle->AddExternalPart(profile53, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0, .220500, -0.315)));
vehicle->AddExternalPart(profile53, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0, -.220500, -0.315)));
vehicle->AddExternalPart(foam_feet, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.250, 0.220500, 0.137732)));
vehicle->AddExternalPart(foam_feet, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.250, 0.220500, 0.137732)));
vehicle->AddExternalPart(foam_feet, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.250, -0.220500, 0.137732)));
vehicle->AddExternalPart(foam_feet, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.250, -0.220500, 0.137732)));
vehicle->AddExternalPart(sheet_acr, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.268075, 0, -0.1575)));
vehicle->AddExternalPart(sheet_acr, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.268075, 0, -0.1575)));
vehicle->AddExternalPart(top_acr, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.129032, 0.158930, -0.3315)));
vehicle->AddExternalPart(top_acr, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.129032, 0.158930, -0.3315)));
vehicle->AddExternalPart(top_acr, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.129032, -0.158930, -0.3315)));
vehicle->AddExternalPart(top_acr, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.129032, -0.158930, -0.3315)));
// vehicle->AddExternalPart(cylinder, sf::Transform(sf::Quaternion::getIdentity(), sf::Vector3(1.2, 1.2, 0)));

//Add inner components
vehicle->AddExternalPart(bottle, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.093553, 0.0055, -0.073530)));
vehicle->AddExternalPart(bottle, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.093460, 0.0055, -0.235823)));
// vehicle->AddExternalPart(bottle, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.004749, 0.0055, -0.155542)));
vehicle->AddExternalPart(camera, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.210, 0.079198, -0.099868)));
vehicle->AddExternalPart(camera, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.210, -0.003752, -0.099868)));
vehicle->AddExternalPart(camera, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.210, -0.086702, -0.099868)));
vehicle->AddExternalPart(supp_cams_l, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.210694, -0.004496, -0.179)));
vehicle->AddExternalPart(supp_cams_h, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.209106, -0.002968, -0.025)));
vehicle->AddExternalPart(supp_sens, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.165369, -0.0055, -0.129)));
vehicle->AddExternalPart(light, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.165, 0.1414, -0.129844)));
vehicle->AddExternalPart(light, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.165, -0.1524, -0.129844)));
vehicle->AddExternalPart(ROV_sensor, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.164742, -0.005441, -0.186226)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, M_PI_2), sf::Vector3(0.213250, 0.1735, -0.2684)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.0225, 0, -0.268))); //ADDED
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.0225, 0, -0.204))); //ADDED
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.0225, 0.12, -0.048))); //ADDED
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.0225, -0.12, -0.048))); //ADDED
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.0225, 0, -0.048))); //ADDED
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.0805, 0.1439, -0.298)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.184, 0.1439, -0.298)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.184, -0.1439, -0.298)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.0805, -0.1439, -0.298)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, M_PI_2), sf::Vector3(0.213250, -0.1735, -0.2684)));
vehicle->AddExternalPart(foam_block_2, sf::Transform(sf::Quaternion(0, 0, M_PI_2), sf::Vector3(0.213250, -0.1735, -0.0984))); //ADDED
vehicle->AddExternalPart(foam_block_2, sf::Transform(sf::Quaternion(0, 0, M_PI_2), sf::Vector3(0.213250, 0.1735, -0.0984))); //ADDED
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.182, 0.16061, -0.365)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.0785, 0.16061, -0.365)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.078844, 0.16061, -0.365)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.182344, 0.16061, -0.365)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.182344, -0.155684, -0.365)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.078844, -0.155684, -0.365)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.0785, -0.155684, -0.365)));
vehicle->AddExternalPart(foam_block_1, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.182, -0.155684, -0.365)));
vehicle->AddExternalPart(foam_corner, sf::Transform(sf::Quaternion(0, 0, M_PI), sf::Vector3(0.261650, 0.248232, -0.300)));
vehicle->AddExternalPart(foam_corner, sf::Transform(sf::Quaternion(0, 0, M_PI), sf::Vector3(-0.261650, 0.248232, -0.300)));
vehicle->AddExternalPart(foam_corner, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.261650, -0.248232, -0.300)));
vehicle->AddExternalPart(foam_corner, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.261650, -0.248232, -0.300)));
vehicle->AddExternalPart(foam_block_3, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.308075, 0.15, -0.1575))); //ADDED
vehicle->AddExternalPart(foam_block_3, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.308075, -0.15, -0.1575))); //ADDED
vehicle->AddExternalPart(foam_block_3, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.308075, 0.15, -0.1575))); //ADDED
vehicle->AddExternalPart(foam_block_3, sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(-0.308075, -0.15, -0.1575))); //ADDED

//Create Thrusters
xl = new sf::Thruster("XL", propeller, 0.076, std::make_pair(0.43,0.43), 0.09, 3000, true);
xr = new sf::Thruster("XR", propeller, 0.076, std::make_pair(0.43,0.43), 0.09, 3000, true);
zb = new sf::Thruster("ZB", propeller, 0.076, std::make_pair(0.43,0.43), 0.09, 3000, true);
zf = new sf::Thruster("ZF", propeller, 0.076, std::make_pair(0.43,0.43), 0.09, 3000, true);
yd = new sf::Thruster("YD", propeller, 0.076, std::make_pair(0.43,0.43), 0.09, 3000, true);
yu = new sf::Thruster("YU", propeller, 0.076, std::make_pair(0.43,0.43), 0.09, 3000, true);

//Thrusters
roverto->DefineLinks(vehicle);
roverto->AddLinkActuator(xl, "Vehicle", sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0, -0.289940, -0.144792)));
roverto->AddLinkActuator(xr, "Vehicle", sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0, 0.289940, -0.144792)));
roverto->AddLinkActuator(yd, "Vehicle", sf::Transform(sf::Quaternion(M_PI_2, 0, 0), sf::Vector3(-0.009208, 0, 0.069440)));
roverto->AddLinkActuator(yu, "Vehicle", sf::Transform(sf::Quaternion(M_PI_2, 0, 0), sf::Vector3(-0.009208, 0, -0.38440)));
roverto->AddLinkActuator(zb, "Vehicle", sf::Transform(sf::Quaternion(0, -M_PI_2, 0), sf::Vector3(-0.319590, -0.003956, -0.153)));
roverto->AddLinkActuator(zf, "Vehicle", sf::Transform(sf::Quaternion(0, -M_PI_2, 0), sf::Vector3(0.319590, -0.003956, -0.153)));

Expected behavior I expected the robot in the surface to keep a similar heading behavior as that in deep water. The spinning makes it impossible to control.

Screenshots I uploaded a short video of the problem, available here.

Desktop (please complete the following information):

Thank you very much!

patrykcieslak commented 4 years ago

Hello! You're welcome :) This is a weird issue.... Can you please send me the meshes that u used (email)? I will not share them with anyone. I need to run it on my computer and see if I can reproduce the problem and debug it. Cheers

lhcubillos commented 4 years ago

Sent! Thank you very much.

patrykcieslak commented 4 years ago

Hello!

Sorry for the delay but I had a lot of work before holidays and then I went home and I just came back.

I looked into the STL files and recreated the robot so I can see the problem. As I was thinking from the beginning the robot starts spinning when it is close to the surface because the algorithm computing buoyancy is switching from the precomputed volume to the actual integration of pressure around the geometry. This is not an issue with actuators. You can remove them completely and the behaviour is exactly the same.

The problems happen because you are using models which are too detailed and at the same time use big triangles. The important thing is that you should be using the more detailed models only for the graphics and for the physics use simplified models, but be careful to subdivide large triangles. Moreover, when possible use the standard shapes like the cube or cylinder instead of loading models from files. I will be working on some automatic algorithm to improve the geometry loaded from files to make it easier to setup a simulation.

The solution for now would be to replace the rectangular objects with boxes instead of models or to provide two different versions: one for the graphics and one for the physics. The one for the graphics doesn't have any meaning for the simulation so it can be arbitrarily complex but the one for the physics have to be simplified. For example, when you have an object which is a plate with holes, remove the holes for the physics version. When you do it for all of the components of the model you should get rid of the fast rotation.

BTW. I set isBuoyant to false because it is for the skin (shell) which is not watertight. This way I avoid computing buoyancy for something that doesn't add almost any. Moreover, all of the parts that are inside something should be created as internal because then they are not used in drag computation. I am in the proces of creating good documentation and I will release it with version 1.0 soon.

Cheers!

lhcubillos commented 4 years ago

Thank you so much Patryk! We'll try your suggestions! And thank you for taking the time to dig into this issue.