121 lines
5.6 KiB
C++
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";
|
|
}
|