Files
rc_yacht/simulation/chrono_rudder_dry.cpp

121 lines
5.6 KiB
C++

#include <chrono/core/ChTypes.h>
#include <chrono/physics/ChBody.h>
#include <chrono/physics/ChLinkLock.h>
#include <chrono/physics/ChLinkMotorRotationAngle.h>
#include <chrono/physics/ChLinkMotorRotationTorque.h>
#include <chrono/physics/ChLinkDistance.h>
#include <chrono/physics/ChSystemNSC.h>
#include <chrono/collision/ChCollisionModel.h>
#include <chrono/physics/ChContactMaterialNSC.h>
#include <chrono/utils/ChUtilsCreators.h>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
using namespace chrono;
struct RunResult {
double command;
double horn;
double rudder;
double pushrod_error;
int contacts;
double servo_torque;
};
static std::shared_ptr<ChBody> body(ChSystemNSC& sys, const ChVector3d& pos, double mass, const ChVector3d& inertia) {
auto b = chrono_types::make_shared<ChBody>();
b->SetPos(pos);
b->SetMass(mass);
b->SetInertiaXX(inertia);
b->SetFixed(false);
b->EnableCollision(false);
sys.AddBody(b);
return b;
}
static ChVector3d endpoint(const std::shared_ptr<ChBody>& b, double x) {
return b->TransformPointLocalToParent(ChVector3d(x, 0, 0));
}
static RunResult run_case(double target) {
constexpr double servo_x = 0.000;
constexpr double rudder_x = 0.0847;
constexpr double horn_length = 0.0040;
constexpr double tiller_length = 0.0035;
constexpr double rod_length = 0.0844;
ChSystemNSC sys;
sys.SetGravitationalAcceleration(ChVector3d(0, -9.81, 0));
auto ground = chrono_types::make_shared<ChBody>();
ground->SetFixed(true);
ground->EnableCollision(true);
auto ground_mat = chrono_types::make_shared<ChContactMaterialNSC>();
ground_mat->SetFriction(0.65f);
ground_mat->SetRestitution(0.05f);
utils::AddBoxGeometry(ground.get(), ground_mat, ChVector3d(0.20, 0.002, 0.02), ChVector3d(0.04, -0.001, 0));
sys.AddBody(ground);
auto horn = body(sys, ChVector3d(servo_x + horn_length * 0.5, 0, 0), 0.012, ChVector3d(1e-7, 1e-7, 2e-8));
constexpr double actual_rod_length = rudder_x + tiller_length - horn_length;
auto rod = body(sys, ChVector3d((horn_length + rudder_x + tiller_length) * 0.5, 0, 0), 0.018, ChVector3d(1e-7, 1e-7, 1.4e-5));
rod->SetPos(ChVector3d((horn_length + rudder_x + tiller_length) * 0.5, 0, 0));
auto rudder = body(sys, ChVector3d(rudder_x + tiller_length * 0.5, 0, 0), 0.045, ChVector3d(1.5e-6, 1.5e-6, 4e-7));
rudder->EnableCollision(true);
utils::AddBoxGeometry(rudder.get(), ground_mat, ChVector3d(tiller_length, 0.001, 0.005));
auto servo_joint = chrono_types::make_shared<ChLinkLockRevolute>();
servo_joint->Initialize(ground, horn, ChFrame<>(ChVector3d(servo_x, 0, 0)));
sys.AddLink(servo_joint);
auto rudder_joint = chrono_types::make_shared<ChLinkLockRevolute>();
rudder_joint->Initialize(ground, rudder, ChFrame<>(ChVector3d(rudder_x, 0, 0)));
rudder_joint->LimitRz().SetActive(true);
rudder_joint->LimitRz().SetMin(-0.65);
rudder_joint->LimitRz().SetMax(0.65);
sys.AddLink(rudder_joint);
auto horn_rod = chrono_types::make_shared<ChLinkLockRevolute>();
horn_rod->Initialize(horn, rod, ChFrame<>(ChVector3d(horn_length, 0, 0)));
sys.AddLink(horn_rod);
auto rod_tiller = chrono_types::make_shared<ChLinkLockRevolute>();
rod_tiller->Initialize(rod, rudder, ChFrame<>(ChVector3d(rudder_x + tiller_length, 0, 0)));
sys.AddLink(rod_tiller);
auto motor = chrono_types::make_shared<ChLinkMotorRotationAngle>();
motor->Initialize(horn, ground, ChFrame<>(ChVector3d(servo_x, 0, 0)));
motor->SetAngleFunction(chrono_types::make_shared<ChFunctionRamp>(0, target / 0.25));
sys.AddLink(motor);
for (int i = 0; i < 2500; ++i)
sys.DoStepDynamics(0.0001);
const auto horn_end = endpoint(horn, horn_length);
const auto rudder_end = endpoint(rudder, tiller_length);
const auto rod_end_a = endpoint(rod, -actual_rod_length * 0.5);
const auto rod_end_b = endpoint(rod, actual_rod_length * 0.5);
const double rod_error = std::max((horn_rod->GetFrame1Abs().GetPos() - horn_rod->GetFrame2Abs().GetPos()).Length(),
(rod_tiller->GetFrame1Abs().GetPos() - rod_tiller->GetFrame2Abs().GetPos()).Length());
const double horn_angle = std::atan2(horn_end.y() - servo_x, horn_end.x() - servo_x);
const double rudder_angle = std::atan2(rudder_end.y(), rudder_end.x() - rudder_x);
return {target, horn_angle, rudder_angle, rod_error, static_cast<int>(sys.GetNumContacts()), motor->GetMotorTorque()};
}
int main() {
std::vector<RunResult> results;
for (double angle : {-0.55, 0.0, 0.55})
results.push_back(run_case(angle));
std::ofstream out("reports/chrono_rudder_dry_report.json");
out << std::setprecision(10) << "{\n \"engine\": \"Project Chrono Multibody NSC\",\n \"contact_model\": \"NSC\",\n \"friction\": 0.65,\n \"bodies_have_mass_and_inertia\": true,\n \"servo_actuation\": \"ChLinkMotorRotationAngle\",\n \"servo_torque_is_measured\": true,\n \"cases\": [\n";
for (size_t i = 0; i < results.size(); ++i) {
const auto& r = results[i];
out << " {\"command_rad\": " << r.command << ", \"servo_angle_rad\": " << r.horn << ", \"rudder_angle_rad\": " << r.rudder << ", \"pushrod_constraint_error_m\": " << r.pushrod_error << ", \"contacts\": " << r.contacts << ", \"servo_torque_nm\": " << r.servo_torque << "}" << (i + 1 == results.size() ? "\n" : ",\n");
}
out << " ]\n}\n";
for (const auto& r : results)
std::cout << r.command << " " << r.horn << " " << r.rudder << " " << r.pushrod_error << " " << r.contacts << " " << r.servo_torque << "\n";
}