diff --git a/Tests/ConfigurableThrusterModels/plot_rotor_models.cpp b/Tests/ConfigurableThrusterModels/plot_rotor_models.cpp deleted file mode 100644 index 5b0c296d..00000000 --- a/Tests/ConfigurableThrusterModels/plot_rotor_models.cpp +++ /dev/null @@ -1,83 +0,0 @@ -#include "actuators/ConfigurableThrusterModels.h" -#include -#include "data_tamer/data_tamer.hpp" - -#include "data_tamer/sinks/mcap_sink.hpp" - -int main() -{ - sf::td::ZeroOrder zeroOrder; - sf::td::FirstOrder firstOrder(1.0); - sf::td::Yoerger yoerger(0.037, 16.5); - sf::td::Bessa bessa(1,1,1,1,1); - - double time = 0.0; - double sp = 0.0; - - double outputZeroOrder = zeroOrder.f(time, sp); - double outputFirstOrder = firstOrder.f(time, sp); - double outputYoerger = yoerger.f(time, sp); - double outputBessa = bessa.f(time, sp); - - // Multiple channels can use this sink. Data will be saved in mylog.mcap - auto mcap_sink = std::make_shared("mylog.mcap"); - - // Create a channel and attach a sink. A channel can have multiple sinks - auto channel = DataTamer::LogChannel::create("dynamics"); - channel->addDataSink(mcap_sink); - - // You can register any arithmetic value. You are responsible for their lifetime! - auto id1 = channel->registerValue("outputZeroOrder", &outputZeroOrder); - auto id2 = channel->registerValue("outputFirstOrder", &outputFirstOrder); - auto id3 = channel->registerValue("outputYoerger", &outputYoerger); - auto id4 = channel->registerValue("outputBessa", &outputBessa); - auto id0 = channel->registerValue("setpoint", &sp); - - // loop. First 10 seconds, setpoint is 0. Next 10 seconds setpoint is 20. Next 10 seconds, setpoint is again 0 - double dt = 0.001; - -while(time < 90.0) -{ - if(time < 10.0) - { - sp = 0.0; - } - else if(time < 20.0) - { - sp = 50.0; - } - - else if (time < 40) - { - sp = 0.0; - } - else if (time < 60) - { - sp = 450.0; - } - else - { - sp = 0.0; - } - - outputZeroOrder = zeroOrder.f(time, sp); - outputFirstOrder = firstOrder.f(time, sp); - outputYoerger = yoerger.f(time, sp); - outputBessa = bessa.f(time, sp); - - std::chrono::duration duration(time); - std::chrono::nanoseconds ns = std::chrono::duration_cast(duration); - channel->takeSnapshot(ns); - - std::cout << "ns: " << ns.count() << std::endl; - - - time += dt; - - // sleep for dt seconds - //std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(dt * 1000))); -} - - - return 0; -} \ No newline at end of file diff --git a/Tests/ConfigurableThrusterModels/plot_thruster_models.cpp b/Tests/ConfigurableThrusterModels/plot_thruster_models.cpp deleted file mode 100644 index d5b4ec40..00000000 --- a/Tests/ConfigurableThrusterModels/plot_thruster_models.cpp +++ /dev/null @@ -1,85 +0,0 @@ -#include "actuators/ConfigurableThrusterModels.h" -#include -#include "data_tamer/data_tamer.hpp" - -#include "data_tamer/sinks/mcap_sink.hpp" - -int main() -{ - sf::td::BasicThrustConversion basic0001(0.001); - sf::td::BasicThrustConversion basic0005(0.005); - sf::td::BasicThrustConversion basic001(0.01); - sf::td::BasicThrustConversion basic005(0.05); - sf::td::BasicThrustConversion basic01(0.1); - sf::td::BasicThrustConversion basic05(0.5); - - sf::td::DeadBandConversion db0001(0.001, 0.001 , - 30 * 30, 30 * 30); - sf::td::DeadBandConversion db0005(0.005, 0.005 , - 30 * 30, 30 * 30); - sf::td::DeadBandConversion db001(0.01, 0.01 , - 30 * 30, 30 * 30); - sf::td::DeadBandConversion db005(0.05, 0.05 , - 30 * 30, 30 * 30); - sf::td::DeadBandConversion db01(0.1, 0.1 , - 30 * 30, 30 * 30); - sf::td::DeadBandConversion db05(0.5, 0.5 , - 30 * 30, 30 * 30); - - double input; - double output0001, output0005, output001, output005, output01, output05; - double outputdb0001, outputdb0005, outputdb001, outputdb005, outputdb01, outputdb05; - - // Multiple channels can use this sink. Data will be saved in mylog.mcap - auto mcap_sink = std::make_shared("mylog2.mcap"); - - // Create a channel and attach a sink. A channel can have multiple sinks - auto channel = DataTamer::LogChannel::create("conversions"); - channel->addDataSink(mcap_sink); - - // You can register any arithmetic value. You are responsible for their lifetime! - auto id1 = channel->registerValue("input", &input); - auto id2 = channel->registerValue("out0.001", &output0001); - auto id3 = channel->registerValue("out0.005", &output0005); - auto id4 = channel->registerValue("out0.01", &output001); - auto id5 = channel->registerValue("out0.05", &output005); - auto id6 = channel->registerValue("out0.1", &output01); - auto id7 = channel->registerValue("out0.5", &output05); - - auto id8 = channel->registerValue("outdb0.001", &outputdb0001); - auto id9 = channel->registerValue("outdb0.005", &outputdb0005); - auto id10 = channel->registerValue("outdb0.01", &outputdb001); - auto id11 = channel->registerValue("outdb0.05", &outputdb005); - auto id12 = channel->registerValue("outdb0.1", &outputdb01); - auto id13 = channel->registerValue("outdb0.5", &outputdb05); - - - // loop. First 10 seconds, setpoint is 0. Next 10 seconds setpoint is 20. Next 10 seconds, setpoint is again 0 - double dt = 0.001; - - double start = -100; - double end = 100; - - - for (double i = start; i < end; i+=dt) - { - input = i; - output0001 = basic0001.f(input); - output0005 = basic0005.f(input); - output001 = basic001.f(input); - output005 = basic005.f(input); - output01 = basic01.f(input); - output05 = basic05.f(input); - - outputdb0001 = db0001.f(input); - outputdb0005 = db0005.f(input); - outputdb001 = db001.f(input); - outputdb005 = db005.f(input); - outputdb01 = db01.f(input); - outputdb05 = db05.f(input); - - channel->takeSnapshot(); - std::cout << "input: " << input << std::endl; - } - - return 0; - - - // sleep for dt seconds - //std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(dt * 1000))); -} - diff --git a/Tests/ConfigurableThrusterTest/ConfigurableThrusterTestManager.cpp b/Tests/ConfigurableThrusterTest/ConfigurableThrusterTestManager.cpp deleted file mode 100644 index 85177ec8..00000000 --- a/Tests/ConfigurableThrusterTest/ConfigurableThrusterTestManager.cpp +++ /dev/null @@ -1,163 +0,0 @@ -/* - This file is a part of Stonefish. - - Stonefish is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Stonefish is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -// -// ConfigurableThrusterTestManager.cpp -// Stonefish -// -// Created by Roger Pi on 06/10/2024. -// Copyright (c) 2024 Roger Pi. All rights reserved. -// - -#include "ConfigurableThrusterTestManager.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -ConfigurableThrusterTestManager::ConfigurableThrusterTestManager(sf::Scalar stepsPerSecond) - : SimulationManager(stepsPerSecond, sf::SolverType::SOLVER_SI, sf::CollisionFilteringType::COLLISION_EXCLUSIVE) -{ - rotor_model_ = // - std::make_shared(0.99999999); - - thrust_model_ = // - std::make_shared(0.005); - - mcap_sink_ = std::make_shared("thruster_test.mcap"); - - // Create a channel and attach a sink. A channel can have multiple sinks - channel_ = DataTamer::LogChannel::create("output"); - channel_->addDataSink(mcap_sink_); - - channel_->registerValue("setpoint", &setpoint_); - channel_->registerValue("omega", &omega_); - channel_->registerValue("thrust", &thrust_); - - double out = 0; - double in = 0; - - auto channel2 = DataTamer::LogChannel::create("conv"); - channel2->addDataSink(mcap_sink_); - - channel2->registerValue("out", &out); - channel2->registerValue("in", &in); - - for (in = -200; in < 200; in += 0.1) - { - out = thrust_model_->f(in); - channel2->takeSnapshot(); - // sleep 0.01 - std::this_thread::sleep_for(std::chrono::nanoseconds(10)); - } -} - -ConfigurableThrusterTestManager::~ConfigurableThrusterTestManager() -{ - // if (thruster_) - // { - // delete thruster_; - // } -} - -void ConfigurableThrusterTestManager::BuildScenario() -{ - ///////MATERIALS//////// - CreateMaterial("Dummy", sf::UnitSystem::Density(sf::CGS, sf::MKS, 0.9), 0.3); - CreateMaterial("Fiberglass", sf::UnitSystem::Density(sf::CGS, sf::MKS, 1.5), 0.9); - CreateMaterial("Rock", sf::UnitSystem::Density(sf::CGS, sf::MKS, 3.0), 0.6); - - SetMaterialsInteraction("Dummy", "Dummy", 0.5, 0.2); - SetMaterialsInteraction("Fiberglass", "Fiberglass", 0.5, 0.2); - SetMaterialsInteraction("Rock", "Rock", 0.9, 0.7); - SetMaterialsInteraction("Fiberglass", "Dummy", 0.5, 0.2); - SetMaterialsInteraction("Rock", "Dummy", 0.6, 0.4); - SetMaterialsInteraction("Rock", "Fiberglass", 0.6, 0.4); - - ///////LOOKS/////////// - CreateLook("yellow", sf::Color::RGB(1.f, 0.9f, 0.f), 0.3f, 0.f); - CreateLook("grey", sf::Color::RGB(0.3f, 0.3f, 0.3f), 0.4f, 0.5f); - CreateLook("black", sf::Color::RGB(0.1f, 0.1f, 0.1f), 0.4f, 0.5f); - CreateLook("seabed", sf::Color::RGB(0.7f, 0.7f, 0.5f), 0.9f, 0.f, 0.f, "", sf::GetDataPath() + "sand_normal.png"); - CreateLook("propeller", sf::Color::RGB(1.f, 1.f, 1.f), 0.3f, 0.f, 0.f, sf::GetDataPath() + "propeller_tex.png"); - - ////////OBJECTS - EnableOcean(0.0); - getOcean()->setWaterType(0.2); - getAtmosphere()->SetupSunPosition(0.0, 60.0); - - sf::Plane* floor = new sf::Plane("Floor", sf::Scalar(10000), "Rock", "seabed"); - AddStaticEntity(floor, sf::Transform(sf::IQ(), sf::Vector3(0, 0, 5))); - - sf::BodyPhysicsSettings phy; - phy.mode = sf::BodyPhysicsMode::SUBMERGED; - phy.collisions = true; - phy.buoyancy = false; - - sf::Polyhedron* prop1 = // - new sf::Polyhedron("Propeller", phy, // - sf::GetDataPath() + "propeller.obj", // - sf::Scalar(1), // - sf::I4(), // - "Dummy", "propeller"); - - thruster_ = // - new sf::ConfigurableThruster("ThrusterTest", // - prop1, // - rotor_model_, // - thrust_model_, // - true, false); - - sf::FeatherstoneRobot* dummy_robot = new sf::FeatherstoneRobot("DummyRobot", true); - - // Box - sf::Box* box = new sf::Box("Box", phy, sf::Vector3(0.01, 0.01, 0.01), sf::I4(), "Dummy", "grey"); - - dummy_robot->DefineLinks(box); - dummy_robot->BuildKinematicStructure(); - - dummy_robot->AddLinkActuator(thruster_, "Box", sf::Transform(sf::Quaternion(0, 0, 0), sf::Vector3(0.2, 0, 0))); - - AddRobot(dummy_robot, sf::Transform(sf::IQ(), sf::Vector3(0, 0, 2))); - - thruster_->setWatchdog(-1); - thruster_->setSetpoint(100); -} - -void ConfigurableThrusterTestManager::SimulationStepCompleted(sf::Scalar timeStep) -{ - timestamp_ += timeStep; - - if (timestamp_ > 10) - { - thruster_->setSetpoint(-100); - } - - setpoint_ = thruster_->getSetpoint(); - omega_ = thruster_->getOmega(); - thrust_ = thruster_->getThrust(); - - channel_->takeSnapshot(); -} \ No newline at end of file diff --git a/Tests/ConfigurableThrusterTest/ConfigurableThrusterTestManager.h b/Tests/ConfigurableThrusterTest/ConfigurableThrusterTestManager.h deleted file mode 100644 index e1891367..00000000 --- a/Tests/ConfigurableThrusterTest/ConfigurableThrusterTestManager.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - This file is a part of Stonefish. - - Stonefish is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Stonefish is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -// -// ConfigurableThrusterTestManager.h -// Stonefish -// -// Created by Roger Pi on 06/10/2024. -// Copyright (c) 2024 Roger Pi. All rights reserved. -// - -#pragma once - -#include -#include - -#include "data_tamer/data_tamer.hpp" -#include "data_tamer/sinks/mcap_sink.hpp" -#include - - -class ConfigurableThrusterTestManager : public sf::SimulationManager -{ -public: - ConfigurableThrusterTestManager(sf::Scalar stepsPerSecond); - - virtual ~ConfigurableThrusterTestManager(); - - void BuildScenario(); - - void SimulationStepCompleted(sf::Scalar timeStep) override; - -protected: - std::shared_ptr mcap_sink_; - std::shared_ptr channel_; - - sf::Scalar setpoint_; - sf::Scalar omega_; - sf::Scalar thrust_; - - sf::Scalar timestamp_; - - std::shared_ptr rotor_model_; - std::shared_ptr thrust_model_; - - sf::ConfigurableThruster* thruster_; -}; diff --git a/Tests/ConfigurableThrusterTest/main.cpp b/Tests/ConfigurableThrusterTest/main.cpp deleted file mode 100644 index 1cfeab3f..00000000 --- a/Tests/ConfigurableThrusterTest/main.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/* - This file is a part of Stonefish. - - Stonefish is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Stonefish is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -// -// main.cpp -// Stonefish -// -// Created by Roger Pi on 06/10/2024. -// Copyright (c) 2024 Roger Pi. All rights reserved. -// - - -#include -#include "ConfigurableThrusterTestManager.h" - -int main(int argc, const char * argv[]) -{ - sf::RenderSettings s; - s.windowW = 1200; - s.windowH = 900; - s.aa = sf::RenderQuality::HIGH; - s.shadows = sf::RenderQuality::HIGH; - s.ao = sf::RenderQuality::HIGH; - s.atmosphere = sf::RenderQuality::HIGH; - s.ocean = sf::RenderQuality::HIGH; - - sf::HelperSettings h; - h.showFluidDynamics = false; - h.showCoordSys = false; - h.showBulletDebugInfo = false; - h.showSensors = false; - h.showActuators = false; - h.showForces = false; - h.showJoints = false; - - ConfigurableThrusterTestManager simulationManager(500.0); - sf::GraphicalSimulationApp app("ThrusterTest", std::string(DATA_DIR_PATH), s, h, &simulationManager); - app.Run(); - - return 0; -} - diff --git a/Tests/FloatingTest/FloatingTestManager.cpp b/Tests/FloatingTest/FloatingTestManager.cpp index 12cd01a0..6e1466bf 100644 --- a/Tests/FloatingTest/FloatingTestManager.cpp +++ b/Tests/FloatingTest/FloatingTestManager.cpp @@ -67,8 +67,13 @@ void FloatingTestManager::BuildScenario() //Propeller phy.mode = sf::BodyPhysicsMode::SUBMERGED; phy.buoyancy = false; - sf::Polyhedron* prop = new sf::Polyhedron("Propeller", phy, sf::GetDataPath() + "propeller.obj", sf::Scalar(1), sf::I4(), "Fiberglass", "propeller"); - sf::Thruster* thrust = new sf::Thruster("Thruster", prop, 0.3, std::make_pair(0.48, 0.48), 0.05, 500.0, true); + + sf::Polyhedron* prop = new sf::Polyhedron("Propeller", phy, sf::GetDataPath() + "propeller.obj", sf::Scalar(1), sf::I4(), "Fiberglass", "propeller"); + std::shared_ptr rotorDynamics; + rotorDynamics = std::make_shared(1.0, 10.0, 5.0, 5.0); + std::shared_ptr thrustModel; + thrustModel = std::make_shared(0.18, 0.48, 0.48, 0.05, true, getOcean()->getLiquid().density); + sf::Thruster* thrust = new sf::Thruster("Thruster", prop, rotorDynamics, thrustModel, 0.18, true, 105.0, false, true); //Sensors sf::Odometry* odom = new sf::Odometry("Odom"); diff --git a/Tests/UnderwaterTest/UnderwaterTestManager.cpp b/Tests/UnderwaterTest/UnderwaterTestManager.cpp index d0dfa9df..a9584744 100644 --- a/Tests/UnderwaterTest/UnderwaterTestManager.cpp +++ b/Tests/UnderwaterTest/UnderwaterTestManager.cpp @@ -118,10 +118,6 @@ void UnderwaterTestManager::BuildScenario() getAtmosphere()->SetupSunPosition(0.0, 60.0); getNED()->Init(41.77737, 3.03376, 0.0); - //sf::Obstacle* tank = new sf::Obstacle("CIRS Tank", sf::GetDataPath() + "cirs_tank.obj", 1.0, sf::I4(), "Rock", "seabed"); - //AddStaticEntity(tank, sf::I4()); - //sf::Plane* seabed = new sf::Plane("Seabed", 1000.0, "Rock", "seabed", 5.f); - //AddStaticEntity(seabed, sf::Transform(sf::IQ(), sf::Vector3(0,0,5.0))); sf::Terrain* seabed = new sf::Terrain("Seabed", sf::GetDataPath() + "terrain.png", 1.0, 1.0, 5.0, "Rock", "seabed", 5.f); AddStaticEntity(seabed, sf::Transform(sf::IQ(), sf::Vector3(0,0,15.0))); sf::Obstacle* cyl = new sf::Obstacle("Cyl", 0.5, 5.0, sf::I4(), "Fiberglass", "seabed"); @@ -189,7 +185,6 @@ void UnderwaterTestManager::BuildScenario() sf::Polyhedron* finger1 = new sf::Polyhedron("Finger1", phy, sf::GetDataPath() + "fingerA_hydro.obj", sf::Scalar(1), sf::I4(), "Dummy", "manipulator"); sf::Polyhedron* finger2 = new sf::Polyhedron("Finger2", phy, sf::GetDataPath() + "fingerA_hydro.obj", sf::Scalar(1), sf::I4(), "Dummy", "manipulator"); - std::vector arm; arm.push_back(baseLink); arm.push_back(link1); @@ -226,11 +221,7 @@ void UnderwaterTestManager::BuildScenario() //vmeshes.push_back(sf::GetDataPath() + "vbs_max.obj"); //vmeshes.push_back(sf::GetDataPath() + "vbs_min.obj"); //sf::VariableBuoyancy* vbs = new sf::VariableBuoyancy("VBS", vmeshes, 0.002); - - //Create ligths - //sf::Light* spot1 = new sf::Light("Spot1", sf::Color::BlackBody(4000.0), 100000.0); //OMNI - //sf::Light* spot1 = new sf::Light("Spot1", 30.0, sf::Color::BlackBody(4000.0), 100000.0); - + //Create sensors sf::Odometry* odom = new sf::Odometry("Odom"); sf::Pressure* press = new sf::Pressure("Pressure"); @@ -287,7 +278,6 @@ void UnderwaterTestManager::BuildScenario() auv->AddLinkActuator(thrusters[2], "Vehicle", sf::Transform(sf::IQ(), sf::Vector3(-0.2807,0.2587,-0.38))); auv->AddLinkActuator(thrusters[3], "Vehicle", sf::Transform(sf::Quaternion(0,-M_PI_2,0), sf::Vector3(-0.5337,0.0,-0.6747))); auv->AddLinkActuator(thrusters[4], "Vehicle", sf::Transform(sf::Quaternion(0,-M_PI_2,0), sf::Vector3(0.5837,0.0,-0.6747))); - //auv->AddLinkActuator(spot1, "Vehicle", sf::Transform(sf::IQ(), sf::Vector3(0,0,1.0))); //auv->AddLinkActuator(vbs, "Vehicle", sf::Transform(sf::IQ(), sf::Vector3(-0.5,0.0,0.0))); //Sensors