From ccca8f0519a9898557b6beb0e9305570206d0d06 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 8 Feb 2026 18:10:28 +0000 Subject: [PATCH] First commit for diff-drive robot (#174) --- CMakeLists.txt | 1 + examples/example_13/example_13.cpp | 106 +++---- examples/example_13/example_13.md | 3 +- .../chrono_robots/chrono_diff_drive_robot.cpp | 289 ------------------ .../chrono_robots/chrono_diff_drive_robot.h | 179 ----------- .../impl/diff_drive_robot_active_wheel.cpp | 81 +++++ .../impl/diff_drive_robot_active_wheel.h | 45 +++ .../impl/diff_drive_robot_chassis.cpp | 74 +++++ .../impl/diff_drive_robot_chassis.h | 51 ++++ .../impl/diff_drive_robot_part.cpp | 58 ++++ .../impl/diff_drive_robot_part.h | 98 ++++++ 11 files changed, 447 insertions(+), 538 deletions(-) delete mode 100644 src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp delete mode 100644 src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h create mode 100644 src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.cpp create mode 100644 src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.h create mode 100644 src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.cpp create mode 100644 src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.h create mode 100644 src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.cpp create mode 100644 src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 12f79c0..36f6def 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -163,6 +163,7 @@ FILE(GLOB SRCS src/bitrl/*.cpp src/bitrl/planning/*.cpp src/bitrl/rigid_bodies/*.cpp src/bitrl/rigid_bodies/chrono_robots/*.cpp + src/bitrl/rigid_bodies/chrono_robots/impl/*.cpp src/bitrl/rigid_bodies/webots_robots/*.cpp src/bitrl/dynamics/*.cpp src/bitrl/utils/*.cpp diff --git a/examples/example_13/example_13.cpp b/examples/example_13/example_13.cpp index 6b218da..49157a0 100644 --- a/examples/example_13/example_13.cpp +++ b/examples/example_13/example_13.cpp @@ -12,6 +12,7 @@ #include "chrono/core/ChRealtimeStep.h" #include "chrono/physics/ChSystemNSC.h" +#include #include "chrono/physics/ChLinkMotorRotationSpeed.h" #include @@ -32,7 +33,7 @@ const real_t DT = 0.01; const real_t SIM_TIME = 5.0; const std::string WINDOW_TITLE( "Example 13"); -void prepare_visualization(chrono::irrlicht::ChVisualSystemIrrlicht& visual) +/*void prepare_visualization(chrono::irrlicht::ChVisualSystemIrrlicht& visual) { visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); //WINDOW_HEIGHT); visual.SetWindowTitle(WINDOW_TITLE); @@ -43,61 +44,46 @@ void prepare_visualization(chrono::irrlicht::ChVisualSystemIrrlicht& visual) visual.AddCamera({0, -2, 1}, {0, 0, 0}); visual.AddTypicalLights(); visual.BindAll(); -} +}*/ + + +/*class DiffDriveRobot +{ +public: + + DiffDriveRobot(); + + void set_motor_speed(real_t rad_speed, uint_t id); + + /// Get active drive wheel speed + chrono::ChVector3d get_wheel_speed(uint_t id); + + /// Get active driver wheel angular velocity + chrono::ChVector3d get_wheel_angular_velocity(uint_t id); + + + +};*/ } // namespace example_13 int main() { - using namespace example_13; + /* using namespace example_13; chrono::ChSystemNSC sys; - sys.SetGravityY(); - - // 2- Create the rigid bodies of the slider-crank mechanical system - // (a crank, a rod, a truss), maybe setting position/mass/inertias of - // their center of mass (COG) etc. - - // ..the truss - auto my_body_A = chrono_types::make_shared(); - sys.AddBody(my_body_A); - my_body_A->SetFixed(true); // truss does not move! - my_body_A->SetName("Ground-Truss"); - - // ..the crank - auto my_body_B = chrono_types::make_shared(); - sys.AddBody(my_body_B); - my_body_B->SetPos(chrono::ChVector3d(1, 0, 0)); // position of COG of crank - my_body_B->SetMass(2); - my_body_B->SetName("Crank"); - - // ..the rod - auto my_body_C = chrono_types::make_shared(); - sys.AddBody(my_body_C); - my_body_C->SetPos(chrono::ChVector3d(4, 0, 0)); // position of COG of rod - my_body_C->SetMass(3); - my_body_C->SetName("Rod"); - - // 3- Create constraints: the mechanical joints between the rigid bodies. - - // .. a revolute joint between crank and rod - auto my_link_BC = chrono_types::make_shared(); - my_link_BC->SetName("RevJointCrankRod"); - my_link_BC->Initialize(my_body_B, my_body_C, chrono::ChFrame<>(chrono::ChVector3d(2, 0, 0))); - sys.AddLink(my_link_BC); - - // .. a slider joint between rod and truss - auto my_link_CA = chrono_types::make_shared(); - my_link_CA->SetName("TransJointRodGround"); - my_link_CA->Initialize(my_body_C, my_body_A, chrono::ChFrame<>(chrono::ChVector3d(6, 0, 0))); - sys.AddLink(my_link_CA); - - // .. a motor between crank and truss - auto my_link_AB = chrono_types::make_shared(); - my_link_AB->Initialize(my_body_A, my_body_B, chrono::ChFrame<>(chrono::ChVector3d(0, 0, 0))); - my_link_AB->SetName("RotationalMotor"); - sys.AddLink(my_link_AB); - auto my_speed_function = chrono_types::make_shared(chrono::CH_PI); // speed w=3.145 rad/sec - my_link_AB->SetSpeedFunction(my_speed_function); + sys.SetGravitationalAcceleration(chrono::ChVector3d(0, 0, -9.81)); + + sys.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET); + chrono::ChCollisionModel::SetDefaultSuggestedEnvelope(0.0025); + chrono::ChCollisionModel::SetDefaultSuggestedMargin(0.0025); + + auto floor_mat = chrono_types::make_shared(); + auto mfloor = chrono_types::make_shared(20, 20, 1, 1000, true, true, floor_mat); + mfloor->SetPos(chrono::ChVector3d(0, 0, -1)); + mfloor->SetFixed(true); + mfloor->GetVisualShape(0)->SetTexture(chrono::GetChronoDataFile("textures/concrete.jpg")); + sys.Add(mfloor); + chrono::irrlicht::ChVisualSystemIrrlicht visual; prepare_visualization(visual); @@ -123,25 +109,6 @@ int main() // .. draw GUI items belonging to Irrlicht screen, if any visual.GetGUIEnvironment()->drawAll(); - // .. draw the rod (from joint BC to joint CA) - tools::drawSegment(&visual, my_link_BC->GetMarker1()->GetAbsCoordsys().pos, - my_link_CA->GetMarker1()->GetAbsCoordsys().pos, - chrono::ChColor(0, 1, 0)); - // .. draw the crank (from joint AB to joint BC) - tools::drawSegment(&visual, my_link_AB->GetFrame2Abs().GetCoordsys().pos, - my_link_BC->GetMarker1()->GetAbsCoordsys().pos, - chrono::ChColor(1, 0, 0)); - - // .. draw a small circle at crank origin - tools::drawCircle(&visual, 0.1, chrono::ChCoordsys<>(chrono::ChVector3d(0, 0, 0), chrono::QUNIT)); - - /* test: delete a link after 10 seconds - if (sys.GetChTime() >10 && (!removed)) - { - sys.RemoveLink(my_link_AB); - removed = true; - }*/ - // ADVANCE SYSTEM STATE BY ONE STEP sys.DoStepDynamics(DT); // Enforce soft real-time @@ -151,6 +118,7 @@ int main() visual.EndScene(); } + */ return 0; } diff --git a/examples/example_13/example_13.md b/examples/example_13/example_13.md index eb3308c..7670577 100644 --- a/examples/example_13/example_13.md +++ b/examples/example_13/example_13.md @@ -10,7 +10,8 @@ A _ChSystem_ is an abstract class. The Chrono library provides the following sub - _ChSystemSMC_ for SMooth Contacts (SMC): contacts are handled using penalty methods, i.e. contacts are deformable Note that if there are no contacts or collisions in your system, it is indifferent to use _ChSystemNSC_ or _ChSystemSMC_. -In this example we will create and simulate a differential drive system using Chrono +In this example we will create and simulate a differential drive system using Chrono. The robot we will develop follows the +Turtlebot robot model defined in Chrono. @code{.cpp} @endcode diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp deleted file mode 100644 index e85c28d..0000000 --- a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp +++ /dev/null @@ -1,289 +0,0 @@ -#include "bitrl/bitrl_config.h" - -#ifdef BITRL_CHRONO - -#include "bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h" -#include "bitrl/bitrl_consts.h" -#include "bitrl/utils/io/json_file_reader.h" -#include "bitrl/extern/nlohmann/json/json.hpp" - -#include -#include -#include -#include -#include - - -#ifdef BITRL_LOG -#include -#endif - -#include -#include -#include -#include - - -namespace bitrl -{ -namespace rb::bitrl_chrono -{ -namespace -{ - -using json = nlohmann::json; - -// helper class to read chassis -struct Chassis -{ - std::array position; - std::array visual_position; - std::string mass_units; - std::string visual_shape; - real_t mass; - bool fixed; - - Chassis(const json &j); -}; - -Chassis::Chassis(const json &j) - : -mass(j["mass"].get()), -mass_units(j["mass_units"].get()), -fixed(j["fixed"].get()), -visual_shape(j["visual_shape"].get()), -position(), -visual_position() -{ - auto pos = j.at("position"); - for (size_t i = 0; i < 3; ++i) - position[i] = pos.at(i).get(); - - auto vis_position = j.at("visual_position"); - for (size_t i = 0; i < 3; ++i) - visual_position[i] = vis_position.at(i).get(); -} - -// helper struct to read a wheel -struct Wheel -{ - std::array position; - std::string mass_units; - std::string visual_shape; - real_t mass; - real_t width; - real_t radius; - - Wheel(const json &j); -}; - -Wheel::Wheel(const json &j) - : -position(), -mass_units(j["mass_units"].get()), -visual_shape(j["visual_shape"].get()), -mass(j["mass"].get()), -width(j["width"].get()), -radius(j["radius"].get()) - -{ - auto pos = j.at("position"); - for (size_t i = 0; i < 3; ++i) - position[i] = pos.at(i).get(); -} - - -auto build_chassis(const utils::io::JSONFileReader& json_reader) -{ - auto chassis_data = json_reader.template at("chassis"); - auto chassis = chrono_types::make_shared(); - chassis->SetMass(chassis_data.mass); - chassis->SetInertiaXX(chrono::ChVector3d(0.1, 0.1, 0.1)); - chassis->SetPos(chrono::ChVector3d(chassis_data.position[0], chassis_data.position[1], chassis_data.position[2])); - chassis->SetFixed(false); - - // add visual shape for visualization - auto vis_shape = chrono_types::make_shared( - chrono::ChVector3d(chassis_data.visual_position[0], chassis_data.visual_position[1], chassis_data.visual_position[2])); - chassis -> AddVisualShape(vis_shape); - - return chassis; -} - -auto build_wheel(const utils::io::JSONFileReader& json_reader, const std::string &wheel_label) -{ - - // read the wheel daat - auto wheel_data = json_reader.template at(wheel_label); - - auto material = chrono_types::make_shared(); - - material->SetYoungModulus(2e7); // stiffness (important) - material->SetPoissonRatio(0.3); - material->SetFriction(0.9f); // traction - material->SetRestitution(0.0f); // no bouncing - - // Optional but recommended - material->SetAdhesion(0.0); - material->SetKn(2e5); // normal stiffness override - material->SetGn(40.0); // normal damping - material->SetKt(2e5); // tangential stiffness - material->SetGt(20.0); - - // rotation axis for the wheel - chrono::ChQuaternion<> q; - q.SetFromAngleAxis(chrono::CH_PI_2, chrono::ChVector3d(1, 0, 0)); - - auto wheel = chrono_types::make_shared(); - wheel->SetMass(wheel_data.mass); - wheel->SetPos(chrono::ChVector3d(wheel_data.position[0], wheel_data.position[1], wheel_data.position[2])); - wheel->SetName(wheel_label); - wheel->SetRot(q); - wheel->EnableCollision(true); - - auto cyl_shape = chrono_types::make_shared( - material, - wheel_data.radius, - wheel_data.width * 0.5 // half-length - ); - wheel->AddCollisionShape(cyl_shape); - - wheel->AddVisualShape( - chrono_types::make_shared(wheel_data.radius, wheel_data.width)); - return wheel; -} - -CHRONO_DiffDriveRobotBase::MotorHandle build_motor(std::shared_ptr wheel, - std::shared_ptr chassis, - const std::string &motor_label) -{ - - - //chrono::ChVector3d axis = chrono::VECT_Y; - - // Build joint frame in ABSOLUTE coordinates - chrono::ChQuaterniond q; - q.SetFromAngleAxis(consts::maths::PI * 0.5, chrono::VECT_X); - chrono::ChFrame<> frame(wheel->GetPos(), q); - - auto motor = chrono_types::make_shared(); - motor->Initialize( - wheel, - chassis, - frame); - motor->SetName(motor_label); - - // set the speed function (rad/s) - auto speed_func = chrono_types::make_shared(0.0); - motor -> SetSpeedFunction(speed_func); - return {motor, speed_func}; -} - - - -} - -void -CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) -{ -#ifdef BITRL_LOG - BOOST_LOG_TRIVIAL(info)<<"Loading robot from file: " << filename; -#endif - utils::io::JSONFileReader json_reader(filename); - json_reader.open(); - - auto material = chrono_types::make_shared(); - - material->SetYoungModulus(2e7); // stiffness (important) - material->SetPoissonRatio(0.3); - material->SetFriction(0.9f); // traction - material->SetRestitution(0.0f); // no bouncing - - // Optional but recommended - material->SetAdhesion(0.0); - material->SetKn(2e5); // normal stiffness override - material->SetGn(40.0); // normal damping - material->SetKt(2e5); // tangential stiffness - material->SetGt(20.0); - auto ground = chrono_types::make_shared( - 5.0, 5.0, 0.1, - 1000, - true, // visual - true, // collision - material - ); - ground->SetFixed(true); - - - // set the gravity acceleration - sys_.SetGravitationalAcceleration(chrono::ChVector3d(0, 0.0, -9.81)); - - chassis_ = build_chassis(json_reader); - sys_.Add(chassis_); - //sys_.Add(ground); - - name_ = json_reader.template get_value("name"); - - BOOST_LOG_TRIVIAL(info)<<"Building wheels...: "; - - auto left_wheel = build_wheel(json_reader, "left-wheel"); - auto right_wheel = build_wheel(json_reader, "right-wheel"); - - sys_.Add(left_wheel); - sys_.Add(right_wheel); - - auto left_motor = build_motor(left_wheel, chassis_, "left-motor"); - auto right_motor = build_motor(right_wheel, chassis_, "right-motor"); - - sys_.AddLink(left_motor.motor); - sys_.AddLink(right_motor.motor); -#ifdef BITRL_LOG - auto zleft = left_motor.motor->GetFrame2Abs().GetRot().GetAxisZ(); - BOOST_LOG_TRIVIAL(info)<<"Left motor rotation: " << zleft; - - auto zright = left_motor.motor->GetFrame2Abs().GetRot().GetAxisZ(); - BOOST_LOG_TRIVIAL(info)<<"Right motor rotation: " << zright; -#endif - - motors_ = std::make_pair(std::move(left_motor), std::move(right_motor)); - - auto caster = build_wheel(json_reader, "caster-wheel"); - sys_.Add(caster); - auto caster_joint = chrono_types::make_shared(); - caster_joint->Initialize( - caster, - chassis_, - chrono::ChFrame<>(chrono::ChCoordsys<>(caster->GetPos())) - ); - sys_.Add(caster_joint); - - // set upd the state - pose_ = std::make_shared(chassis_); - -#ifdef BITRL_LOG - BOOST_LOG_TRIVIAL(info)<<"Loaded robot: " << name_; - BOOST_LOG_TRIVIAL(info)<<"Bodies in loaded robot " << sys_.GetBodies().size(); -#endif -} - -void CHRONO_DiffDriveRobotBase::set_motor_speed(const std::string& motor_name, real_t speed) -{ - if (motor_name == "left-motor") - { - motors_.first.speed -> SetConstant(speed); - } - - if (motor_name == "right-motor") - { - motors_.second.speed -> SetConstant(speed); - } -} - -void CHRONO_DiffDriveRobotBase::step(real_t time_step) -{ - sys_.DoStepDynamics(time_step); -} -} -} -#endif - diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h deleted file mode 100644 index 390352f..0000000 --- a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h +++ /dev/null @@ -1,179 +0,0 @@ -#ifndef CHRONO_DIFF_DRIVE_ROBOT_H -#define CHRONO_DIFF_DRIVE_ROBOT_H - -#include "bitrl/bitrl_config.h" - -#ifdef BITRL_CHRONO - -#include "bitrl/bitrl_types.h" -#include "bitrl/sensors/sensor_manager.h" -#include "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" - - -#include -#include - - -#include -#include -#include - -namespace bitrl -{ -namespace rb::bitrl_chrono -{ -/** - * @class CHRONO_DiffDriveRobotBase - * @ingroup rb_chrono - * @brief Base class for a differential-drive robot using Project Chrono. - * - * This class provides a common foundation for modeling and simulating - * differential-drive robots using Chrono physics objects. It encapsulates - * a Chrono simulation system and supports initialization from an external - * JSON configuration file. - * - * The class can be instantiated directly for simple simulations or - * extended to implement more specialized robot behaviors. - * - * @note This class assumes the use of Chrono's SMC contact model. - */ -class CHRONO_DiffDriveRobotBase -{ -public: - - typedef CHRONO_RobotPose pose_type; - - /** - * Handle the motors - */ - struct MotorHandle { - std::shared_ptr motor; - std::shared_ptr speed; - }; - - - /** - * @brief Load robot and simulation parameters from a JSON file. - * - * This function initializes the robot and its associated Chrono - * simulation objects based on the contents of the provided JSON - * configuration file. - * - * Typical parameters may include: - * - Physical dimensions - * - Mass and inertia properties - * - Wheel configuration - * - Simulation settings - * - * @param filename Path to the JSON configuration file. - * - * @throws std::runtime_error If the file cannot be read or parsed. - */ - void load_from_json(const std::string& filename); - - /** - * @brief Step the underlying chrono::ChSystemSMC one time step - * @param time_step - */ - void step(real_t time_step); - - /** - * @brief Set the motor speed - * @param motor_name The name of the motor - * @param speed The speed - */ - void set_motor_speed(const std::string& motor_name, real_t speed); - - /** - * @brief Set the speed for both motors - * @param speed - */ - void set_motor_speed(real_t speed); - - /** - * @brief The name of the robot - * @return - */ - const std::string& get_name() const noexcept{return name_;} - - /** - * @brief Retruns the number of wheels this robot has - * @return - */ - uint_t n_wheels()const noexcept{return 3;} - - /** - * @brief Returns the number of motors this robot has - * @return - */ - uint_t n_motors()const noexcept{return 2;} - - /** - * Set the pointer to the sensor manager - * @param sensors_manager - */ - void set_sensors(sensors::SensorManager& sensor_manager){sensor_manager_ptr_ = &sensor_manager;} - - /** - * @return - */ - chrono::ChSystemSMC& CHRONO_sys() noexcept{return sys_;} - - - std::shared_ptr CHRONO_chassis()noexcept{return chassis_;} - - /** - * @return Pointer to the state of the robot - */ - std::shared_ptr pose()noexcept{return pose_;} - -protected: - - - /** - * @brief Chrono physics system used for simulation. - * - * This system owns and manages all physical bodies, constraints, - * and contact interactions associated with the robot and the - * environment. - */ - chrono::ChSystemSMC sys_; - - /** - * The chassis of the robot - */ - std::shared_ptr chassis_; - - /** - * The state of the robot - */ - std::shared_ptr pose_; - - /** - * @brief Manages the sensors on the robot - */ - sensors::SensorManager* sensor_manager_ptr_; - - /** - * @brief Motors for the robot - * motors_.first left motor - * motors.second right motor - */ - std::pair motors_; - - /** - * @brief The name of the robot - */ - std::string name_; - -}; - -inline void CHRONO_DiffDriveRobotBase::set_motor_speed(real_t speed) -{ - set_motor_speed("left_motor", speed); - set_motor_speed("right_motor", speed); -} -} -} -#endif -#endif //DIFF_DRIVE_ROBOT_H diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.cpp new file mode 100644 index 0000000..6176e40 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.cpp @@ -0,0 +1,81 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "chrono/assets/ChColor.h" +#include "bitrl/bitrl_types.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.h" + + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ + +CHRONO_DiffDriveRobot_ActiveWheel::CHRONO_DiffDriveRobot_ActiveWheel(const std::string& name, + bool fixed, + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis, + bool collide) + : + CHRONO_DiffDriveRobot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) +{ + m_mesh_name = "active_wheel"; + m_offset = chrono::ChVector3d(0, 0, 0); + m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); + m_density = 200; +} + +void CHRONO_DiffDriveRobot_ActiveWheel::init() { + auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + auto trimesh = chrono::ChTriangleMeshConnected::CreateFromWavefrontFile(vis_mesh_file, false, false); + trimesh->Transform(chrono::ChVector3d(0, 0, 0), chrono::ChMatrix33<>(1)); // scale to a different size + trimesh->RepairDuplicateVertexes(1e-9); // if meshes are not watertight + + double mmass; + chrono::ChVector3d mcog; + chrono::ChMatrix33<> minertia; + trimesh->ComputeMassProperties(true, mmass, mcog, minertia); + chrono::ChMatrix33<> principal_inertia_rot; + chrono::ChVector3d principal_I; + chrono::ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot); + + m_body->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + + // Set inertia + m_body->SetMass(mmass * m_density); + m_body->SetInertiaXX(m_density * principal_I); + + // set relative position to chassis + const chrono::ChFrame<>& X_GP = m_chassis->GetFrameRefToAbs(); // global -> parent + chrono::ChFrame<> X_PC(m_pos, m_rot); // parent -> child + chrono::ChFrame<> X_GC = X_GP * X_PC; // global -> child + m_body->SetFrameRefToAbs(X_GC); + m_body->SetFixed(m_fixed); + + this -> add_collision_shapes(); + + m_body->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::ACTIVE_WHEEL)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + + this -> add_visualization_assets(); + + m_system->Add(m_body); +} + +void CHRONO_DiffDriveRobot_ActiveWheel::enable_collision(bool state) { + m_collide = state; + m_body->EnableCollision(state); +} + +void CHRONO_DiffDriveRobot_ActiveWheel::translate(const chrono::ChVector3d& shift) { + m_body->SetPos(m_body->GetPos() + shift); +} + + +} +} +#endif \ No newline at end of file diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.h b/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.h new file mode 100644 index 0000000..4afb72e --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.h @@ -0,0 +1,45 @@ +#ifndef DIFF_DRIVE_ROBOT_ACTIVE_WHEEL_H +#define DIFF_DRIVE_ROBOT_ACTIVE_WHEEL_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include +#include + +#include "bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.h" +namespace bitrl{ +namespace rb::bitrl_chrono +{ + +class CHRONO_DiffDriveRobot_ActiveWheel : public CHRONO_DiffDriveRobot_Part { +public: + CHRONO_DiffDriveRobot_ActiveWheel(const std::string& name, + bool fixed, + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis, + bool collide); + + // destructor + virtual ~CHRONO_DiffDriveRobot_ActiveWheel()=default; + + /// Initialize the wheel at the specified (absolute) position. + void init(); + + /// Enable/disable collision for the wheel. + void enable_collision(bool state); + +private: + /// Translate the chassis by the specified value. + void translate(const chrono::ChVector3d& shift); +}; + +} +} + +#endif //DIFF_DRIVE_ROBOT_ACTIVE_WHEEL_H +#endif diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.cpp new file mode 100644 index 0000000..bbac13e --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.cpp @@ -0,0 +1,74 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "chrono/assets/ChColor.h" +#include "chrono/physics/ChLinkMotorRotationSpeed.h" + +#include "bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.h" + +namespace bitrl{ +namespace rb::bitrl_chrono +{ + +CHRONO_DiffDriveRobot_Chassis::CHRONO_DiffDriveRobot_Chassis(const std::string& name, + bool fixed, + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + bool collide) + : + CHRONO_DiffDriveRobot_Part(name, fixed, mat, system, body_pos, body_rot, NULL, collide) +{ + this -> m_mesh_name = "chassis"; + this -> m_offset = chrono::ChVector3d(0, 0, 0); + this -> m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); + this -> m_density = 100; +} + +void CHRONO_DiffDriveRobot_Chassis::init() { + auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + auto trimesh = chrono::ChTriangleMeshConnected::CreateFromWavefrontFile(vis_mesh_file, false, false); + trimesh->Transform(chrono::ChVector3d(0, 0, 0), chrono::ChMatrix33<>(1)); // scale to a different size + trimesh->RepairDuplicateVertexes(1e-9); // if meshes are not watertight + + double mmass; + chrono::ChVector3d mcog; + chrono::ChMatrix33<> minertia; + trimesh->ComputeMassProperties(true, mmass, mcog, minertia); + chrono::ChMatrix33<> principal_inertia_rot; + chrono::ChVector3d principal_I; + chrono::ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot); + + m_body->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + + // Set inertia + m_body->SetMass(mmass * m_density); + m_body->SetInertiaXX(m_density * principal_I); + m_body->SetFrameRefToAbs(chrono::ChFrame<>(m_pos, m_rot)); + m_body->SetFixed(m_fixed); + + this -> add_collision_shapes(); + + m_body->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::CHASSIS)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ACTIVE_WHEEL)); + + this -> add_visualization_assets(); + m_system->Add(m_body); +} + +void CHRONO_DiffDriveRobot_Chassis::enable_collision(bool state) { + m_collide = state; + m_body->EnableCollision(state); +} + +void CHRONO_DiffDriveRobot_Chassis::translate(const chrono::ChVector3d& shift) { + m_body->SetPos(m_body->GetPos() + shift); +} + + +} +} + +#endif diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.h b/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.h new file mode 100644 index 0000000..0b05fab --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.h @@ -0,0 +1,51 @@ +// +// Created by alex on 2/8/26. +// + +#ifndef DIFF_DRIVE_ROBOT_CHASSIS_H +#define DIFF_DRIVE_ROBOT_CHASSIS_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include +#include + +#include "chrono/physics/ChSystem.h" +//#include "chrono/physics/ChLinkMotorRotationSpeed.h" + +#include "bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.h" +#include "bitrl/bitrl_types.h" + +namespace bitrl{ +namespace rb::bitrl_chrono{ + +class CHRONO_DiffDriveRobot_Chassis : public CHRONO_DiffDriveRobot_Part { +public: + CHRONO_DiffDriveRobot_Chassis(const std::string& name, + bool fixed, + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + bool collide); + ~CHRONO_DiffDriveRobot_Chassis()=default; + + /// Initialize the chassis at the specified (absolute) position. + void init(); + + /// Enable/disable collision for the robot chassis. + void enable_collision(bool state); + +private: + /// Translate the chassis by the specified value. + void translate(const chrono::ChVector3d& shift); + +}; + +} +} +#endif //DIFF_DRIVE_ROBOT_CHASSIS_H +#endif + diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.cpp new file mode 100644 index 0000000..ce6987b --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.cpp @@ -0,0 +1,58 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.h" + +namespace bitrl{ +namespace rb::bitrl_chrono{ + +CHRONO_DiffDriveRobot_Part::CHRONO_DiffDriveRobot_Part(const std::string& name, + bool fixed, + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis_body, + bool collide){ + m_body = chrono_types::make_shared(); + m_body->SetName(name + "_body"); + m_chassis = chassis_body; + m_mat = mat; + m_pos = body_pos; + m_rot = body_rot; + m_system = system; + m_collide = collide; + m_fixed = fixed; +} + +// Create Visulization assets +void CHRONO_DiffDriveRobot_Part::add_visualization_assets() { + auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + auto trimesh = chrono::ChTriangleMeshConnected::CreateFromWavefrontFile(vis_mesh_file, true, true); + trimesh->Transform(m_offset, chrono::ChMatrix33<>(1)); + auto trimesh_shape = chrono_types::make_shared(); + trimesh_shape->SetMesh(trimesh); + trimesh_shape->SetName(m_mesh_name); + m_body->AddVisualShape(trimesh_shape); + return; +} + +void CHRONO_DiffDriveRobot_Part::enable_collision(bool state) { + m_collide = state; +} + +// Add collision assets +void CHRONO_DiffDriveRobot_Part::add_collision_shapes() { + auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + auto trimesh = chrono::ChTriangleMeshConnected::CreateFromWavefrontFile(vis_mesh_file, false, false); + trimesh->Transform(m_offset, chrono::ChMatrix33<>(1)); + + auto shape = chrono_types::make_shared(m_mat, trimesh, false, false, 0.005); + m_body->AddCollisionShape(shape); + m_body->EnableCollision(m_collide); +} + +} +} +#endif \ No newline at end of file diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.h b/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.h new file mode 100644 index 0000000..fd2b5f1 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.h @@ -0,0 +1,98 @@ + +#ifndef DIFF_DRIVE_ROBOT_PART_H +#define DIFF_DRIVE_ROBOT_PART_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include +#include + +#include "bitrl/bitrl_types.h" +#include "chrono/assets/ChColor.h" +#include "chrono/physics/ChSystem.h" +#include "chrono/physics/ChLinkMotorRotationSpeed.h" + +namespace bitrl{ +namespace rb::bitrl_chrono{ + +enum class CollisionFamily : int_t +{ + CHASSIS = 1, ///< chassis + ACTIVE_WHEEL = 2, ///< active cylinderical drive wheel + PASSIVE_WHEEL = 3, ///< passive cylinderical wheel + ROD = 4, ///< short and long supporting rods + BOTTOM_PLATE = 5, ///< bottom plate + MIDDLE_PLATE = 6, ///< middle plate + TOP_PLATE = 7 ///< top plate +}; + +class CHRONO_DiffDriveRobot_Part { +public: + CHRONO_DiffDriveRobot_Part(const std::string& name, + bool fixed, + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis_body, + bool collide); + + /// destructor + virtual ~CHRONO_DiffDriveRobot_Part()=default; + + /// Return the name of the part. + const std::string& get_name() const { return m_name; } + + /// Set the name of the part. + void set_name(const std::string& name) { m_name = name; } + + /// Return the ChBody of the corresponding Turtlebot part. + std::shared_ptr get_body_ptr() const { return m_body; } + + /// Return the ChBody of the chassis wrt the Turtlebot part. + std::shared_ptr get_chassis_ptr() const { return m_chassis; } + + /// Return the Position of the Turtlebot part. + const chrono::ChVector3d& get_pos() const { return m_body->GetFrameRefToAbs().GetPos(); } + + /// Return the Rotation of the Turtlebot part. + const chrono::ChQuaternion<>& get_rotation() const { return m_body->GetFrameRefToAbs().GetRot(); } + +protected: + /// Initialize the visulization mesh of the Turtlebot part. + void add_visualization_assets(); + + /// Initialize the collision mesh of the Turtlebot part. + void add_collision_shapes(); + + /// Enable/disable collision. + void enable_collision(bool state); + + std::shared_ptr m_body; ///< rigid body + std::shared_ptr m_mat; ///< contact material (shared among all shapes) + std::shared_ptr m_chassis; ///< the chassis body for the robot + + chrono::ChVector3d m_pos; ///< Turtlebot part's relative position wrt the chassis + chrono::ChVector3d m_offset; ///< offset for visualization mesh + chrono::ChColor m_color; ///< visualization asset color + chrono::ChSystem* m_system; ///< system which Turtlebot Part belongs to + + chrono::ChQuaternion<> m_rot; ///< Turtlebot part's relative rotation wrt the chassis + + std::string m_name; ///< subsystem name + std::string m_mesh_name; ///< visualization mesh name + real_t m_density; ///< Turtlebot part's density + + bool m_collide; ///< Turtlebot part's collision indicator + bool m_fixed; ///< Turtlebot part's fixed indication +}; + +} +} + + + +#endif +#endif //DIFF_DRIVE_ROBOT_PART_H