From ccca8f0519a9898557b6beb0e9305570206d0d06 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 8 Feb 2026 18:10:28 +0000 Subject: [PATCH 1/3] 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 From ed4e79e4672e18b6485edb3d4d1d838c547a0595 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 15 Feb 2026 21:27:39 +0000 Subject: [PATCH 2/3] finalised implementation for diff drive robot (#174) --- CMakeLists.txt | 1 + examples/example_13/example_13.cpp | 28 +- .../chrono_robots/diff_drive_robot.cpp | 519 ++++++++++++++++++ .../chrono_robots/diff_drive_robot.h | 121 ++++ .../impl/diff_drive_robot_active_wheel.cpp | 81 --- .../diff_drive_robot_chassis.cpp | 9 +- .../diff_drive_robot_chassis.h | 4 +- .../diff_drive_robot_part.cpp | 10 +- .../{ => turtle_bot}/diff_drive_robot_part.h | 0 .../turtle_bot/diff_drive_robot_plates.cpp | 214 ++++++++ .../impl/turtle_bot/diff_drive_robot_plates.h | 98 ++++ .../impl/turtle_bot/diff_drive_robot_rods.cpp | 149 +++++ .../impl/turtle_bot/diff_drive_robot_rods.h | 72 +++ .../turtle_bot/diff_drive_robot_wheels.cpp | 148 +++++ .../diff_drive_robot_wheels.h} | 31 +- .../chrono_robots/tmp/chrono_robot_pose.h | 84 +++ .../tmp/org_chrono_diff_drive_robot.cpp | 289 ++++++++++ .../tmp/org_chrono_diff_drive_robot.h | 179 ++++++ .../chrono_robots/tmp/rb_chrono_module.h | 12 + 19 files changed, 1937 insertions(+), 112 deletions(-) create mode 100644 src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp create mode 100644 src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h delete mode 100644 src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.cpp rename src/bitrl/rigid_bodies/chrono_robots/impl/{ => turtle_bot}/diff_drive_robot_chassis.cpp (83%) rename src/bitrl/rigid_bodies/chrono_robots/impl/{ => turtle_bot}/diff_drive_robot_chassis.h (90%) rename src/bitrl/rigid_bodies/chrono_robots/impl/{ => turtle_bot}/diff_drive_robot_part.cpp (79%) rename src/bitrl/rigid_bodies/chrono_robots/impl/{ => turtle_bot}/diff_drive_robot_part.h (100%) create mode 100644 src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp create mode 100644 src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h create mode 100644 src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp create mode 100644 src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h create mode 100644 src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp rename src/bitrl/rigid_bodies/chrono_robots/impl/{diff_drive_robot_active_wheel.h => turtle_bot/diff_drive_robot_wheels.h} (51%) create mode 100644 src/bitrl/rigid_bodies/chrono_robots/tmp/chrono_robot_pose.h create mode 100644 src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.cpp create mode 100644 src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.h create mode 100644 src/bitrl/rigid_bodies/chrono_robots/tmp/rb_chrono_module.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 36f6def..0fb4bea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -164,6 +164,7 @@ FILE(GLOB SRCS src/bitrl/*.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/chrono_robots/impl/turtle_bot/*.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 49157a0..4dff7ed 100644 --- a/examples/example_13/example_13.cpp +++ b/examples/example_13/example_13.cpp @@ -15,6 +15,7 @@ #include #include "chrono/physics/ChLinkMotorRotationSpeed.h" #include +#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" #include #include @@ -33,7 +34,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); @@ -44,32 +45,16 @@ const std::string WINDOW_TITLE( "Example 13"); 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.SetGravitationalAcceleration(chrono::ChVector3d(0, 0, -9.81)); @@ -84,6 +69,9 @@ int main() mfloor->GetVisualShape(0)->SetTexture(chrono::GetChronoDataFile("textures/concrete.jpg")); sys.Add(mfloor); + bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot robot(sys, + chrono::ChVector3d(0, 0, -0.45), chrono::QUNIT); + robot.init(); chrono::irrlicht::ChVisualSystemIrrlicht visual; prepare_visualization(visual); @@ -118,8 +106,6 @@ int main() visual.EndScene(); } - */ - return 0; } #else diff --git a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp new file mode 100644 index 0000000..b855251 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp @@ -0,0 +1,519 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" + +#include + +#include "chrono/physics/ChBodyEasy.h" +#include "chrono/assets/ChVisualShapeTriangleMesh.h" +#include "chrono/physics/ChMassProperties.h" + + + +namespace bitrl +{ +namespace rb::bitrl_chrono{ + + +// ============================================================================= +// Create default contact material for the robot +std::shared_ptr DefaultContactMaterial(chrono::ChContactMethod contact_method) { + float mu = 0.4f; // coefficient of friction + float cr = 0.0f; // coefficient of restitution + float Y = 2e7f; // Young's modulus + float nu = 0.3f; // Poisson ratio + float kn = 2e5f; // normal stiffness + float gn = 40.0f; // normal viscous damping + float kt = 2e5f; // tangential stiffness + float gt = 20.0f; // tangential viscous damping + + switch (contact_method) { + case chrono::ChContactMethod::NSC: { + auto matNSC = chrono_types::make_shared(); + matNSC->SetFriction(mu); + matNSC->SetRestitution(cr); + return matNSC; + } + case chrono::ChContactMethod::SMC: { + auto matSMC = chrono_types::make_shared(); + matSMC->SetFriction(mu); + matSMC->SetRestitution(cr); + matSMC->SetYoungModulus(Y); + matSMC->SetPoissonRatio(nu); + matSMC->SetKn(kn); + matSMC->SetGn(gn); + matSMC->SetKt(kt); + matSMC->SetGt(gt); + return matSMC; + } + default: + return std::shared_ptr(); + } +} + +// Add a revolute joint between body_1 and body_2 +// rel_joint_pos and rel_joint_rot are the position and the rotation of the revolute point +void AddRevoluteJoint(std::shared_ptr body_1, + std::shared_ptr body_2, + std::shared_ptr chassis, + chrono::ChSystem* system, + const chrono::ChVector3d& rel_joint_pos, + const chrono::ChQuaternion<>& rel_joint_rot) { + const chrono::ChFrame<>& X_GP = chassis->GetFrameRefToAbs(); // global -> parent + chrono::ChFrame<> X_PC(rel_joint_pos, rel_joint_rot); // parent -> child + chrono::ChFrame<> X_GC = X_GP * X_PC; // global -> child + + auto revo = chrono_types::make_shared(); + revo->Initialize(body_1, body_2, chrono::ChFrame<>(X_GC.GetCoordsys().pos, X_GC.GetCoordsys().rot)); + system->AddLink(revo); +} + +// Add a revolute joint between body_1 and body_2 +// rel_joint_pos and rel_joint_rot are the position and the rotation of the revolute point +void AddRevoluteJoint(std::shared_ptr body_1, + std::shared_ptr body_2, + std::shared_ptr chassis, + chrono::ChSystem* system, + const chrono::ChVector3d& rel_joint_pos, + const chrono::ChQuaternion<>& rel_joint_rot) { + const chrono::ChFrame<>& X_GP = chassis->GetFrameRefToAbs(); // global -> parent + chrono::ChFrame<> X_PC(rel_joint_pos, rel_joint_rot); // parent -> child + chrono::ChFrame<> X_GC = X_GP * X_PC; // global -> child + + auto revo = chrono_types::make_shared(); + revo->Initialize(body_1, body_2, chrono::ChFrame<>(X_GC.GetCoordsys().pos, X_GC.GetCoordsys().rot)); + system->AddLink(revo); +} + +// Add a revolute joint between body_1 and body_2 +// rel_joint_pos and rel_joint_rot are the position and the rotation of the revolute point +void AddLockJoint(std::shared_ptr body_1, + std::shared_ptr body_2, + std::shared_ptr chassis, + chrono::ChSystem* system, + const chrono::ChVector3d& rel_joint_pos, + const chrono::ChQuaternion<>& rel_joint_rot) { + const chrono::ChFrame<>& X_GP = chassis->GetFrameRefToAbs(); // global -> parent + chrono::ChFrame<> X_PC(rel_joint_pos, rel_joint_rot); // parent -> child + chrono::ChFrame<> X_GC = X_GP * X_PC; // global -> child + + // auto revo = chrono_types::make_shared(); + auto revo = chrono_types::make_shared(); + revo->Initialize(body_1, body_2, chrono::ChFrame<>(X_GC.GetCoordsys().pos, X_GC.GetCoordsys().rot)); + system->AddLink(revo); +} + +// Add a rotational speed controlled motor between body_1 and body_2 +// rel_joint_pos and rel_joint_rot are the position and the rotation of the motor +std::shared_ptr AddMotor(std::shared_ptr body_1, + std::shared_ptr body_2, + std::shared_ptr chassis, + chrono::ChSystem* system, + const chrono::ChVector3d& rel_joint_pos, + const chrono::ChQuaternion<>& rel_joint_rot, + std::shared_ptr speed_func) { + const chrono::ChFrame<>& X_GP = chassis->GetFrameRefToAbs(); // global -> parent + chrono::ChFrame<> X_PC(rel_joint_pos, rel_joint_rot); // parent -> child + chrono::ChFrame<> X_GC = X_GP * X_PC; // global -> child + + auto motor_angle = chrono_types::make_shared(); + motor_angle->Initialize(body_1, body_2, chrono::ChFrame<>(X_GC.GetCoordsys().pos, X_GC.GetCoordsys().rot)); + system->AddLink(motor_angle); + motor_angle->SetSpeedFunction(speed_func); + return motor_angle; +} + + +CHRONO_DiffDriveRobot::CHRONO_DiffDriveRobot(chrono::ChSystem& system, + const chrono::ChVector3d& robot_pos, + const chrono::ChQuaternion<>& robot_rot, + std::shared_ptr wheel_mat) + : m_system(&system), m_robot_pos(robot_pos), m_robot_rot(robot_rot), m_wheel_material(wheel_mat) { + // Set default collision model envelope commensurate with model dimensions. + // Note that an SMC system automatically sets envelope to 0. + auto contact_method = m_system->GetContactMethod(); + if (contact_method == chrono::ChContactMethod::NSC) { + chrono::ChCollisionModel::SetDefaultSuggestedEnvelope(0.01); + chrono::ChCollisionModel::SetDefaultSuggestedMargin(0.005); + } + + // Create the contact materials + m_chassis_material = DefaultContactMaterial(contact_method); + if (!m_wheel_material) + m_wheel_material = DefaultContactMaterial(contact_method); + + create(); +} + + + +void CHRONO_DiffDriveRobot::create() { + // initialize robot chassis + m_chassis = chrono_types::make_shared("chassis", false, m_chassis_material, m_system, + m_robot_pos, m_robot_rot, true); + + // active drive wheels' positions relative to the chassis + double dwx = 0; + double dwy = 0.11505; + double dwz = 0.03735; + m_drive_wheels.push_back(chrono_types::make_shared( + "LDWheel", false, m_wheel_material, m_system, chrono::ChVector3d(dwx, +dwy, dwz), chrono::ChQuaternion<>(1, 0, 0, 0), + m_chassis->get_body_ptr(), true)); + m_drive_wheels.push_back(chrono_types::make_shared( + "RDWheel", false, m_wheel_material, m_system, chrono::ChVector3d(dwx, -dwy, dwz), chrono::ChQuaternion<>(1, 0, 0, 0), + m_chassis->get_body_ptr(), true)); + + // passive driven wheels' positions relative to the chassis + double pwx = 0.11505; + double pwy = 0; + double pwz = 0.02015; + + m_passive_wheels.push_back(chrono_types::make_shared( + "FPWheel", false, m_wheel_material, m_system, chrono::ChVector3d(pwx, pwy, pwz), chrono::ChQuaternion<>(1, 0, 0, 0), + m_chassis->get_body_ptr(), true)); + m_passive_wheels.push_back(chrono_types::make_shared( + "RPWheel", false, m_wheel_material, m_system, chrono::ChVector3d(-pwx, pwy, pwz), chrono::ChQuaternion<>(1, 0, 0, 0), + m_chassis->get_body_ptr(), true)); + + // create the first level supporting rod + double rod_s_0_x = -0.0565; + double rod_s_0_y = 0.11992; + double rod_s_0_z = 0.09615; + + double rod_s_1_x = 0.0535; + double rod_s_1_y = 0.11992; + double rod_s_1_z = 0.09615; + + double rod_s_2_x = 0.11850; + double rod_s_2_y = 0.08192; + double rod_s_2_z = 0.09615; + + double rod_s_3_x = 0.11850; + double rod_s_3_y = -0.08192; + double rod_s_3_z = 0.09615; + + double rod_s_4_x = 0.0535; + double rod_s_4_y = -0.11992; + double rod_s_4_z = 0.09615; + + double rod_s_5_x = -0.0565; + double rod_s_5_y = -0.11992; + double rod_s_5_z = 0.09615; + + m_1st_level_rods.push_back(chrono_types::make_shared( + "0-bottom-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_s_0_x, rod_s_0_y, rod_s_0_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_1st_level_rods.push_back(chrono_types::make_shared( + "1-bottom-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_s_1_x, rod_s_1_y, rod_s_1_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_1st_level_rods.push_back(chrono_types::make_shared( + "2-bottom-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_s_2_x, rod_s_2_y, rod_s_2_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_1st_level_rods.push_back(chrono_types::make_shared( + "3-bottom-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_s_3_x, rod_s_3_y, rod_s_3_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_1st_level_rods.push_back(chrono_types::make_shared( + "4-bottom-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_s_4_x, rod_s_4_y, rod_s_4_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_1st_level_rods.push_back(chrono_types::make_shared( + "5-bottom-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_s_5_x, rod_s_5_y, rod_s_5_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + + // add the bottom plate + double bt_plate_x = 0; + double bt_plate_y = 0; + double bt_plate_z = 0.14615; + + m_bottom_plate = chrono_types::make_shared( + "bottom_plate", false, m_wheel_material, m_system, chrono::ChVector3d(bt_plate_x, bt_plate_y, bt_plate_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true); + + // create the second level support rod + double rod_m_0_x = -0.10394; + double rod_m_0_y = 0.09792; + double rod_m_0_z = 0.15015; + + double rod_m_1_x = -0.0015; + double rod_m_1_y = 0.16192; + double rod_m_1_z = 0.15015; + + double rod_m_2_x = 0.0687; + double rod_m_2_y = 0.13132; + double rod_m_2_z = 0.15015; + + double rod_m_3_x = 0.0687; + double rod_m_3_y = -0.13132; + double rod_m_3_z = 0.15015; + + double rod_m_4_x = -0.0015; + double rod_m_4_y = -0.16192; + double rod_m_4_z = 0.15015; + + double rod_m_5_x = -0.10394; + double rod_m_5_y = -0.09792; + double rod_m_5_z = 0.15015; + + m_2nd_level_rods.push_back(chrono_types::make_shared( + "0-middle-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_m_0_x, rod_m_0_y, rod_m_0_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_2nd_level_rods.push_back(chrono_types::make_shared( + "1-middle-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_m_1_x, rod_m_1_y, rod_m_1_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_2nd_level_rods.push_back(chrono_types::make_shared( + "2-middle-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_m_2_x, rod_m_2_y, rod_m_2_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_2nd_level_rods.push_back(chrono_types::make_shared( + "3-middle-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_m_3_x, rod_m_3_y, rod_m_3_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_2nd_level_rods.push_back(chrono_types::make_shared( + "4-middle-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_m_4_x, rod_m_4_y, rod_m_4_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_2nd_level_rods.push_back(chrono_types::make_shared( + "5-middle-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_m_5_x, rod_m_5_y, rod_m_5_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + + // add the middle plate + double mi_plate_x = 0; + double mi_plate_y = 0; + double mi_plate_z = 0.20015; + m_middle_plate = chrono_types::make_shared( + "middle_plate", false, m_wheel_material, m_system, chrono::ChVector3d(mi_plate_x, mi_plate_y, mi_plate_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true); + + // create the third level support rod + double rod_u_0_x = -0.10394; + double rod_u_0_y = 0.09792; + double rod_u_0_z = 0.20615; + + double rod_u_1_x = -0.0015; + double rod_u_1_y = 0.16192; + double rod_u_1_z = 0.20615; + + double rod_u_2_x = 0.0687; + double rod_u_2_y = 0.13132; + double rod_u_2_z = 0.20615; + + double rod_u_3_x = 0.0687; + double rod_u_3_y = -0.13132; + double rod_u_3_z = 0.20615; + + double rod_u_4_x = -0.0015; + double rod_u_4_y = -0.16192; + double rod_u_4_z = 0.20615; + + double rod_u_5_x = -0.10394; + double rod_u_5_y = -0.09792; + double rod_u_5_z = 0.20615; + + m_3rd_level_rods.push_back(chrono_types::make_shared( + "0-top-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_u_0_x, rod_u_0_y, rod_u_0_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_3rd_level_rods.push_back(chrono_types::make_shared( + "1-top-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_u_1_x, rod_u_1_y, rod_u_1_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_3rd_level_rods.push_back(chrono_types::make_shared( + "2-top-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_u_2_x, rod_u_2_y, rod_u_2_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_3rd_level_rods.push_back(chrono_types::make_shared( + "3-top-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_u_3_x, rod_u_3_y, rod_u_3_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_3rd_level_rods.push_back(chrono_types::make_shared( + "4-top-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_u_4_x, rod_u_4_y, rod_u_4_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + m_3rd_level_rods.push_back(chrono_types::make_shared( + "5-top-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_u_5_x, rod_u_5_y, rod_u_5_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + + // add the top plate + double top_plate_x = 0; + double top_plate_y = 0; + double top_plate_z = 0.40615; + m_top_plate = chrono_types::make_shared("top_plate", false, m_wheel_material, m_system, + chrono::ChVector3d(top_plate_x, top_plate_y, top_plate_z), + chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true); +} + +/// Initialize the complete rover and add all constraints +void CHRONO_DiffDriveRobot::init() { + + m_chassis->init(); + m_bottom_plate->init(); + m_middle_plate->init(); + m_top_plate->init(); + for (int i = 0; i < 2; i++) { + m_drive_wheels[i]->init(); + m_passive_wheels[i]->init(); + } + + for (int i = 0; i < 6; i++) { + m_1st_level_rods[i]->init(); + m_2nd_level_rods[i]->init(); + m_3rd_level_rods[i]->init(); + } + + // redeclare necessary location variables + double dwx = 0; + double dwy = 0.11505; + double dwz = 0.03735; + + double pwx = 0.11505; + double pwy = 0; + double pwz = 0.02015; + + double rod_s_0_x = -0.0565; + double rod_s_0_y = 0.11992; + double rod_s_0_z = 0.09615; + + double rod_s_1_x = 0.0535; + double rod_s_1_y = 0.11992; + double rod_s_1_z = 0.09615; + + double rod_s_2_x = 0.11850; + double rod_s_2_y = 0.08192; + double rod_s_2_z = 0.09615; + + double rod_s_3_x = 0.11850; + double rod_s_3_y = -0.08192; + double rod_s_3_z = 0.09615; + + double rod_s_4_x = 0.0535; + double rod_s_4_y = -0.11992; + double rod_s_4_z = 0.09615; + + double rod_s_5_x = -0.0565; + double rod_s_5_y = -0.11992; + double rod_s_5_z = 0.09615; + + double rod_m_0_x = -0.10394; + double rod_m_0_y = 0.09792; + double rod_m_0_z = 0.15015; + + double rod_m_1_x = -0.0015; + double rod_m_1_y = 0.16192; + double rod_m_1_z = 0.15015; + + double rod_m_2_x = 0.0687; + double rod_m_2_y = 0.13132; + double rod_m_2_z = 0.15015; + + double rod_m_3_x = 0.0687; + double rod_m_3_y = -0.13132; + double rod_m_3_z = 0.15015; + + double rod_m_4_x = -0.0015; + double rod_m_4_y = -0.16192; + double rod_m_4_z = 0.15015; + + double rod_m_5_x = -0.10394; + double rod_m_5_y = -0.09792; + double rod_m_5_z = 0.15015; + + double rod_u_0_x = -0.10394; + double rod_u_0_y = 0.09792; + double rod_u_0_z = 0.20615; + + double rod_u_1_x = -0.0015; + double rod_u_1_y = 0.16192; + double rod_u_1_z = 0.20615; + + double rod_u_2_x = 0.0687; + double rod_u_2_y = 0.13132; + double rod_u_2_z = 0.20615; + + double rod_u_3_x = 0.0687; + double rod_u_3_y = -0.13132; + double rod_u_3_z = 0.20615; + + double rod_u_4_x = -0.0015; + double rod_u_4_y = -0.16192; + double rod_u_4_z = 0.20615; + + double rod_u_5_x = -0.10394; + double rod_u_5_y = -0.09792; + double rod_u_5_z = 0.20615; + + // add motors and revolute joints on the active and passive wheels + auto const_speed_function_l = chrono_types::make_shared(-chrono::CH_PI); + auto const_speed_function_r = chrono_types::make_shared(-chrono::CH_PI); + m_motors_func.push_back(const_speed_function_l); + m_motors_func.push_back(const_speed_function_r); + + chrono::ChQuaternion<> z2y = chrono::QuatFromAngleX(chrono::CH_PI_2); + chrono::ChQuaternion<> z2x = chrono::QuatFromAngleY(-chrono::CH_PI_2); + + m_motors.push_back(AddMotor(m_drive_wheels[0]->get_body_ptr(), m_chassis->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + chrono::ChVector3d(dwx, dwy, dwz), z2y, const_speed_function_l)); + AddRevoluteJoint(m_passive_wheels[0]->get_body_ptr(), m_chassis->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + chrono::ChVector3d(pwx, pwy, pwz), z2x); + + m_motors.push_back(AddMotor(m_drive_wheels[1]->get_body_ptr(), m_chassis->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + chrono::ChVector3d(dwx, -dwy, dwz), z2y, const_speed_function_r)); + AddRevoluteJoint(m_passive_wheels[1]->get_body_ptr(), m_chassis->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + chrono::ChVector3d(-pwx, pwy, pwz), z2x); + + // add fixity on all rods and plates + // There are six constraints needed: + // chassis -> bottom rods + // bottom rods -> bottom plate + // bottom plate -> middle rods + // middle rods -> middle plate + // middle plate -> top rods + // top rods -> top plate + + chrono::ChVector3d bottom_rod_rel_pos[] = {chrono::ChVector3d(rod_s_0_x, rod_s_0_y, rod_s_0_z), // + chrono::ChVector3d(rod_s_1_x, rod_s_1_y, rod_s_1_z), // + chrono::ChVector3d(rod_s_2_x, rod_s_2_y, rod_s_2_z), // + chrono::ChVector3d(rod_s_3_x, rod_s_3_y, rod_s_3_z), // + chrono::ChVector3d(rod_s_4_x, rod_s_4_y, rod_s_4_z), // + chrono::ChVector3d(rod_s_5_x, rod_s_5_y, rod_s_5_z)}; + chrono::ChVector3d middle_rod_rel_pos[] = {chrono::ChVector3d(rod_m_0_x, rod_m_0_y, rod_m_0_z), // + chrono::ChVector3d(rod_m_1_x, rod_m_1_y, rod_m_1_z), // + chrono::ChVector3d(rod_m_2_x, rod_m_2_y, rod_m_2_z), // + chrono::ChVector3d(rod_m_3_x, rod_m_3_y, rod_m_3_z), // + chrono::ChVector3d(rod_m_4_x, rod_m_4_y, rod_m_4_z), // + chrono::ChVector3d(rod_m_5_x, rod_m_5_y, rod_m_5_z)}; + chrono::ChVector3d top_rod_rel_pos[] = {chrono::ChVector3d(rod_u_0_x, rod_u_0_y, rod_u_0_z), // + chrono::ChVector3d(rod_u_1_x, rod_u_1_y, rod_u_1_z), // + chrono::ChVector3d(rod_u_2_x, rod_u_2_y, rod_u_2_z), // + chrono::ChVector3d(rod_u_3_x, rod_u_3_y, rod_u_3_z), // + chrono::ChVector3d(rod_u_4_x, rod_u_4_y, rod_u_4_z), // + chrono::ChVector3d(rod_u_5_x, rod_u_5_y, rod_u_5_z)}; + + for (int i = 0; i < 6; i++) { + chrono::ChVector3d bottom_plate_rel_pos = bottom_rod_rel_pos[i] + chrono::ChVector3d(0, 0, 0.05); + chrono::ChVector3d middle_plate_rel_pos = middle_rod_rel_pos[i] + chrono::ChVector3d(0, 0, 0.05); + chrono::ChVector3d top_plate_rel_pos = top_rod_rel_pos[i] + chrono::ChVector3d(0, 0, 0.2); + + AddLockJoint(m_1st_level_rods[i]->get_body_ptr(), m_chassis->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + bottom_rod_rel_pos[i], chrono::ChQuaternion<>(1, 0, 0, 0)); + AddLockJoint(m_1st_level_rods[i]->get_body_ptr(), m_bottom_plate->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + bottom_plate_rel_pos, chrono::ChQuaternion<>(1, 0, 0, 0)); + AddLockJoint(m_2nd_level_rods[i]->get_body_ptr(), m_bottom_plate->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + bottom_rod_rel_pos[i], chrono::ChQuaternion<>(1, 0, 0, 0)); + AddLockJoint(m_2nd_level_rods[i]->get_body_ptr(), m_middle_plate->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + middle_plate_rel_pos, chrono::ChQuaternion<>(1, 0, 0, 0)); + AddLockJoint(m_3rd_level_rods[i]->get_body_ptr(), m_middle_plate->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + top_rod_rel_pos[i], chrono::ChQuaternion<>(1, 0, 0, 0)); + AddLockJoint(m_3rd_level_rods[i]->get_body_ptr(), m_top_plate->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + top_plate_rel_pos, chrono::ChQuaternion<>(1, 0, 0, 0)); + } +} + +void CHRONO_DiffDriveRobot::set_motor_speed(real_t rad_speed, uint_t id) { + m_motors_func[id]->SetConstant(rad_speed); +} + +chrono::ChVector3d CHRONO_DiffDriveRobot::get_active_wheel_speed(uint_t id)const { + return m_drive_wheels[id]->get_body_ptr()->GetPosDt(); +} + +chrono::ChVector3d CHRONO_DiffDriveRobot::get_active_wheel_angular_velocity(uint_t id)const { + return m_drive_wheels[id]->get_body_ptr()->GetAngVelParent(); +} + +} +} + +#endif diff --git a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h new file mode 100644 index 0000000..a42c39a --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h @@ -0,0 +1,121 @@ + +// ============================================================================= +// Authors: Jason Zhou +// ============================================================================= +// +// Turtlebot Robot Class +// This is a modified version of the famous turtlebot 2e +// The geometries use the following resources as references: +// https://groups.google.com/g/sydney_ros/c/z05uQTCuDTQ +// https://grabcad.com/library/interbotix-turtlebot-2i-1 +// https://www.turtlebot.com/turtlebot2/ +// +// ============================================================================= + +#ifndef DIFF_DRIVE_ROBOT_H +#define DIFF_DRIVE_ROBOT_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + + +#include "bitrl/bitrl_types.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h" + +#include "chrono/assets/ChColor.h" +#include "chrono/physics/ChSystem.h" +#include "chrono/physics/ChLinkMotorRotationSpeed.h" + +#include +#include +#include +#include +#include + + +namespace bitrl +{ +namespace rb::bitrl_chrono{ + + + +/// Turtlebot Robot class +/// This class assemble and initialize a complete turtlebot robot +/// This class also handles general control commands of the robot +/** + * @class CHRONO_DiffDriveRobotBase + */ + +/** + * @class CHRONO_DiffDriveRobotBase + * @ingroup rb_chrono + * @brief Class for modelling a differential-drive robot using Chrono. + * in fact the implementation of this class is taken from the + * Chrono::TurtleBot. + * + * for more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html + * + */ +class CHRONO_DiffDriveRobot { +public: + CHRONO_DiffDriveRobot(chrono::ChSystem& system, + const chrono::ChVector3d& robot_pos, + const chrono::ChQuaternion<>& robot_rot, + std::shared_ptr wheel_mat = {}); + + virtual ~CHRONO_DiffDriveRobot()=default; + + /// Initialize the turtlebot robot using current parameters. + void init(); + + /// Set active drive wheel speed + void set_motor_speed(real_t rad_speed, uint_t id); + + /// Get active drive wheel speed + chrono::ChVector3d get_active_wheel_speed(uint_t id) const; + + /// Get active driver wheel angular velocity + chrono::ChVector3d get_active_wheel_angular_velocity(uint_t id)const; + +private: + /// This function initializes all parameters for the robot. + /// Note: The robot will not be constructed in the ChSystem until Initialize() is called. + void create(); + + chrono::ChSystem* m_system; ///< pointer to the Chrono system + + bool m_dc_motor_control = false; + + std::shared_ptr m_chassis; ///< robot chassis + std::vector> m_drive_wheels; ///< 2 active robot drive wheels + std::vector> m_passive_wheels; ///< 2 passive robot driven wheels + + std::vector> m_1st_level_rods; ///< six first level supporting short rods + std::vector> m_2nd_level_rods; ///< six second level supporting short rods + std::vector> m_3rd_level_rods; ///< six third level support long rods + std::shared_ptr m_bottom_plate; ///< bottom plate of the turtlebot robot + std::shared_ptr m_middle_plate; ///< middle plate of the turtlebot robot + std::shared_ptr m_top_plate; ///< top plate of the turtlebot robot + + chrono::ChQuaternion<> m_robot_rot; ///< robot rotation + chrono::ChVector3d m_robot_pos; ///< robot translation position + + std::vector> m_motors; ///< vector to store motors + + std::vector> m_motors_func; ///< constant motor angular speed func + + // model parts material + std::shared_ptr m_chassis_material; ///< chassis contact material + std::shared_ptr m_wheel_material; ///< wheel contact material (shared across limbs) +}; + + +} // namespace chrono +} +#endif +#endif + 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 deleted file mode 100644 index 6176e40..0000000 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.cpp +++ /dev/null @@ -1,81 +0,0 @@ -#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_chassis.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.cpp similarity index 83% rename from src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.cpp rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.cpp index bbac13e..a2c6f0f 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.cpp @@ -5,7 +5,8 @@ #include "chrono/assets/ChColor.h" #include "chrono/physics/ChLinkMotorRotationSpeed.h" -#include "bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.h" +#include "bitrl/bitrl_consts.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h" namespace bitrl{ namespace rb::bitrl_chrono @@ -28,7 +29,11 @@ CHRONO_DiffDriveRobot_Chassis::CHRONO_DiffDriveRobot_Chassis(const std::string& } void CHRONO_DiffDriveRobot_Chassis::init() { - auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + + const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); + std::cout<<"Visualization mesh file: "<Transform(chrono::ChVector3d(0, 0, 0), chrono::ChMatrix33<>(1)); // scale to a different size trimesh->RepairDuplicateVertexes(1e-9); // if meshes are not watertight diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h similarity index 90% rename from src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h index 0b05fab..a91c0de 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_chassis.h +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h @@ -13,9 +13,9 @@ #include #include "chrono/physics/ChSystem.h" -//#include "chrono/physics/ChLinkMotorRotationSpeed.h" -#include "bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.h" + +#include "diff_drive_robot_part.h" #include "bitrl/bitrl_types.h" namespace bitrl{ diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp similarity index 79% rename from src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.cpp rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp index ce6987b..e26250c 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp @@ -2,7 +2,9 @@ #ifdef BITRL_CHRONO -#include "bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.h" +#include "diff_drive_robot_part.h" + +#include "bitrl/bitrl_consts.h" namespace bitrl{ namespace rb::bitrl_chrono{ @@ -28,7 +30,8 @@ CHRONO_DiffDriveRobot_Part::CHRONO_DiffDriveRobot_Part(const std::string& name, // Create Visulization assets void CHRONO_DiffDriveRobot_Part::add_visualization_assets() { - auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); + //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(); @@ -44,7 +47,8 @@ void CHRONO_DiffDriveRobot_Part::enable_collision(bool state) { // Add collision assets void CHRONO_DiffDriveRobot_Part::add_collision_shapes() { - auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); + //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)); diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h similarity index 100% rename from src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp new file mode 100644 index 0000000..87bdc1e --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp @@ -0,0 +1,214 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h" +#include "bitrl/bitrl_consts.h" + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ + + +CHRONO_DiffDriveRobot_BottomPlate::CHRONO_DiffDriveRobot_BottomPlate(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 = "plate_1"; + m_offset = chrono::ChVector3d(0, 0, 0); + m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); + m_density = 20; +} + +void CHRONO_DiffDriveRobot_BottomPlate::init() { + const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); + //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::BOTTOM_PLATE)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ROD)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + + this->add_visualization_assets(); + + m_system->Add(m_body); +} + +void CHRONO_DiffDriveRobot_BottomPlate::enable_collision(bool state) { + m_collide = state; + m_body->EnableCollision(state); +} + +void CHRONO_DiffDriveRobot_BottomPlate::translate(const chrono::ChVector3d& shift) { + m_body->SetPos(m_body->GetPos() + shift); +} + +// ========================================================== +CHRONO_DiffDriveRobot_MiddlePlate::CHRONO_DiffDriveRobot_MiddlePlate(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 = "plate_2"; + m_offset = chrono::ChVector3d(0, 0, 0); + m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); + m_density = 20; +} + +void CHRONO_DiffDriveRobot_MiddlePlate::init() { + const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); + + //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::MIDDLE_PLATE)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ROD)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + + this->add_visualization_assets(); + + m_system->Add(m_body); +} + +void CHRONO_DiffDriveRobot_MiddlePlate::enable_collision(bool state) { + m_collide = state; + m_body->EnableCollision(state); +} + +void CHRONO_DiffDriveRobot_MiddlePlate::translate(const chrono::ChVector3d& shift) { + m_body->SetPos(m_body->GetPos() + shift); +} + +// ========================================================== +CHRONO_DiffDriveRobot_TopPlate::CHRONO_DiffDriveRobot_TopPlate(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 = "plate_3"; + m_offset = chrono::ChVector3d(0, 0, 0); + m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); + m_density = 20; +} + +void CHRONO_DiffDriveRobot_TopPlate::init() { + const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); + //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::TOP_PLATE)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ROD)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::MIDDLE_PLATE)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::BOTTOM_PLATE)); + + this->add_visualization_assets(); + + m_system->Add(m_body); +} + +void CHRONO_DiffDriveRobot_TopPlate::enable_collision(bool state) { + m_collide = state; + m_body->EnableCollision(state); +} + +void CHRONO_DiffDriveRobot_TopPlate::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/turtle_bot/diff_drive_robot_plates.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h new file mode 100644 index 0000000..08457cf --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h @@ -0,0 +1,98 @@ +#ifndef DIFF_DRIVE_ROBOT_PLATES_H +#define DIFF_DRIVE_ROBOT_PLATES_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include +#include + +#include "chrono/physics/ChSystem.h" + +#include "diff_drive_robot_part.h" + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ +/// Turtlebot Bottom Plate class definition +class CHRONO_DiffDriveRobot_BottomPlate : public CHRONO_DiffDriveRobot_Part { + public: + CHRONO_DiffDriveRobot_BottomPlate(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); + virtual ~CHRONO_DiffDriveRobot_BottomPlate()=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); + +}; + +/// Turtlebot Middle Plate class definition +class CHRONO_DiffDriveRobot_MiddlePlate : public CHRONO_DiffDriveRobot_Part { + public: + CHRONO_DiffDriveRobot_MiddlePlate(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); + virtual ~CHRONO_DiffDriveRobot_MiddlePlate()=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); + +}; + +/// Turtlebot Top Plate class definition +class CHRONO_DiffDriveRobot_TopPlate : public CHRONO_DiffDriveRobot_Part { + public: + CHRONO_DiffDriveRobot_TopPlate(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); + virtual ~CHRONO_DiffDriveRobot_TopPlate() {} + + /// 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 + +#endif //DIFF_DRIVE_ROBOT_PLATES_H diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp new file mode 100644 index 0000000..f1fb868 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp @@ -0,0 +1,149 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h" +#include "bitrl/bitrl_consts.h" + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ + +CHRONO_DiffDriveRobot_Rod_Short::CHRONO_DiffDriveRobot_Rod_Short(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 = "support_rod_short"; + m_offset = chrono::ChVector3d(0, 0, 0); + m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); + m_density = 100; +} + +void CHRONO_DiffDriveRobot_Rod_Short::init() { + const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); + //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::ROD)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::BOTTOM_PLATE)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::MIDDLE_PLATE)); + + this->add_visualization_assets(); + m_system->Add(m_body); +} + +void CHRONO_DiffDriveRobot_Rod_Short::enable_collision(bool state) { + m_collide = state; + m_body->EnableCollision(state); +} + +void CHRONO_DiffDriveRobot_Rod_Short::translate(const chrono::ChVector3d& shift) { + m_body->SetPos(m_body->GetPos() + shift); +} + +CHRONO_DiffDriveRobot_Rod_Long::CHRONO_DiffDriveRobot_Rod_Long(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 = "support_rod_long"; + m_offset = chrono::ChVector3d(0, 0, 0); + m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); + m_density = 100; +} + +void CHRONO_DiffDriveRobot_Rod_Long::init() { + const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); + //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + //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::ROD)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::BOTTOM_PLATE)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::MIDDLE_PLATE)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ROD)); + + this->add_visualization_assets(); + + m_system->Add(m_body); +} + +void CHRONO_DiffDriveRobot_Rod_Long::enable_collision(bool state) { + m_collide = state; + m_body->EnableCollision(state); +} + +void CHRONO_DiffDriveRobot_Rod_Long::translate(const chrono::ChVector3d& shift) { + m_body->SetPos(m_body->GetPos() + shift); +} + + +} +} +#endif diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h new file mode 100644 index 0000000..b5b9540 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h @@ -0,0 +1,72 @@ +#ifndef DIFF_DRIVE_ROBOT_RODS_H +#define DIFF_DRIVE_ROBOT_RODS_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include +#include + +#include "chrono/physics/ChSystem.h" + +#include "diff_drive_robot_part.h" + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ + +/// Short Supporting Rod class definition +class CHRONO_DiffDriveRobot_Rod_Short : public CHRONO_DiffDriveRobot_Part { +public: + CHRONO_DiffDriveRobot_Rod_Short(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_Rod_Short()=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); + +}; + + +/// Long Supporting Rod class definition +class CHRONO_DiffDriveRobot_Rod_Long : public CHRONO_DiffDriveRobot_Part { +public: + CHRONO_DiffDriveRobot_Rod_Long(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); + virtual ~CHRONO_DiffDriveRobot_Rod_Long()=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 +#endif //DIFF_DRIVE_ROBOT_ROD_SHORT_H diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp new file mode 100644 index 0000000..949ebd0 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp @@ -0,0 +1,148 @@ +#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/turtle_bot/diff_drive_robot_wheels.h" + +#include "bitrl/bitrl_consts.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() { + const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); + //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + //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); +} + + +CHRONO_DiffDriveRobot_PassiveWheel::CHRONO_DiffDriveRobot_PassiveWheel(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 = "passive_wheel"; + m_offset = chrono::ChVector3d(0, 0, 0); + m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); + m_density = 200; +} + +void CHRONO_DiffDriveRobot_PassiveWheel::init() { + const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); + //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::PASSIVE_WHEEL)); + m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + + this -> add_visualization_assets(); + + m_system->Add(m_body); +} + +void CHRONO_DiffDriveRobot_PassiveWheel::enable_collision(bool state) { + m_collide = state; + m_body->EnableCollision(state); +} + +void CHRONO_DiffDriveRobot_PassiveWheel::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/turtle_bot/diff_drive_robot_wheels.h similarity index 51% rename from src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h index 4afb72e..a99d0d5 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_active_wheel.h +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h @@ -1,5 +1,5 @@ -#ifndef DIFF_DRIVE_ROBOT_ACTIVE_WHEEL_H -#define DIFF_DRIVE_ROBOT_ACTIVE_WHEEL_H +#ifndef DIFF_DRIVE_ROBOT_WHEELS_H +#define DIFF_DRIVE_ROBOT_WHEELS_H #include "bitrl/bitrl_config.h" @@ -8,7 +8,7 @@ #include #include -#include "bitrl/rigid_bodies/chrono_robots/impl/diff_drive_robot_part.h" +#include "diff_drive_robot_part.h" namespace bitrl{ namespace rb::bitrl_chrono { @@ -38,6 +38,31 @@ class CHRONO_DiffDriveRobot_ActiveWheel : public CHRONO_DiffDriveRobot_Part { void translate(const chrono::ChVector3d& shift); }; +/// Turtlebot Passive Driven Wheel class definition +class CHRONO_DiffDriveRobot_PassiveWheel : public CHRONO_DiffDriveRobot_Part { +public: + CHRONO_DiffDriveRobot_PassiveWheel(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); + virtual ~CHRONO_DiffDriveRobot_PassiveWheel()=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); + +}; + } } diff --git a/src/bitrl/rigid_bodies/chrono_robots/tmp/chrono_robot_pose.h b/src/bitrl/rigid_bodies/chrono_robots/tmp/chrono_robot_pose.h new file mode 100644 index 0000000..2806005 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/tmp/chrono_robot_pose.h @@ -0,0 +1,84 @@ +#ifndef CHRONO_ROBOT_POSE_H +#define CHRONO_ROBOT_POSE_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO +#include "bitrl/bitrl_types.h" +#include +#include +namespace bitrl +{ +namespace rb::bitrl_chrono +{ + +/** + * @class CHRONO_RobotPose + * @ingroup rb_chrono + * @brief Wrapper for representing the state of a CHRONO_Robot + */ +class CHRONO_RobotPose +{ +public: + + /** + *@brief Constructor. Initialize by passing a pointer to the chrono::ChBody + * to monitor + */ + explicit CHRONO_RobotPose(std::shared_ptr robot_ptr=nullptr); + + /** + * Set/Reset the chrono::ChBody to monitor + * @param robot_ptr + */ + void set_body(std::shared_ptr robot_ptr){robot_ptr_ = robot_ptr;} + + /** + * @brief Return the position vector. This is the position of the CoM of the body + */ + const chrono::ChVector3d& position() const {return robot_ptr_ -> GetPos();} + + /** + * @return The linear velocity of the monitored body + */ + const chrono::ChVector3d& velocity()const {return robot_ptr_ -> GetLinVel();} + + /** + * @return The linear velocity of the monitored body + */ + const chrono::ChVector3d& acceleration()const {return robot_ptr_ -> GetLinAcc();} + + /** + * @brief Return the rotation matrix + */ + const chrono::ChMatrix33d& rotation_matrix() const {return robot_ptr_ -> GetRotMat();} + + /** + * @brief Return the transformation of the local to the world coordinats + */ + chrono::ChVector3d to_world_coords(chrono::ChVector3d point){return robot_ptr_ -> GetFrameRefToAbs().TransformPointLocalToParent(point);} + + /** + * @brief Transform the world point to local coordinates + */ + chrono::ChVector3d to_local_coords(chrono::ChVector3d point){return robot_ptr_ -> GetFrameRefToAbs().TransformPointParentToLocal(point);} +private: + + /** + * @brief Pointer to the chrono::ChBody to monitor + */ + std::shared_ptr robot_ptr_; + +}; + +inline +CHRONO_RobotPose::CHRONO_RobotPose(std::shared_ptr robot_ptr) + : +robot_ptr_(robot_ptr) +{} +} +} + +#endif + +#endif //CHRONO_ROBOT_STATE_H diff --git a/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.cpp new file mode 100644 index 0000000..e85c28d --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.cpp @@ -0,0 +1,289 @@ +#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/tmp/org_chrono_diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.h new file mode 100644 index 0000000..390352f --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.h @@ -0,0 +1,179 @@ +#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/tmp/rb_chrono_module.h b/src/bitrl/rigid_bodies/chrono_robots/tmp/rb_chrono_module.h new file mode 100644 index 0000000..e1426e8 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/tmp/rb_chrono_module.h @@ -0,0 +1,12 @@ +#ifndef RB_CHRONO_MODULE_H +#define RB_CHRONO_MODULE_H + +/** + * @defgroup rb_chrono rb::chrono + * @brief Chrono-based robotics components. + * + * This module contains robot models, simulation utilities, + * and physics-based components built on Project Chrono. + */ + +#endif //RB_CHRONO_MODULE_H From f253e8d23ef4bfed8f60d8697ee0669f5d1c193a Mon Sep 17 00:00:00 2001 From: pockerman Date: Sat, 21 Feb 2026 13:49:06 +0000 Subject: [PATCH 3/3] Refactor diff drive robot implementation --- examples/example_13/example_13.cpp | 8 +- examples/example_13/example_13.md | 126 +++- .../chrono_robots/diff_drive_robot.cpp | 588 +++++++++--------- .../chrono_robots/diff_drive_robot.h | 76 ++- .../turtle_bot/diff_drive_robot_chassis.cpp | 63 +- .../turtle_bot/diff_drive_robot_chassis.h | 28 +- .../impl/turtle_bot/diff_drive_robot_part.cpp | 85 ++- .../impl/turtle_bot/diff_drive_robot_part.h | 98 ++- .../turtle_bot/diff_drive_robot_plates.cpp | 199 +++--- .../impl/turtle_bot/diff_drive_robot_plates.h | 72 ++- .../impl/turtle_bot/diff_drive_robot_rods.cpp | 123 ++-- .../impl/turtle_bot/diff_drive_robot_rods.h | 43 +- .../turtle_bot/diff_drive_robot_wheels.cpp | 117 ++-- .../impl/turtle_bot/diff_drive_robot_wheels.h | 42 +- 14 files changed, 968 insertions(+), 700 deletions(-) diff --git a/examples/example_13/example_13.cpp b/examples/example_13/example_13.cpp index 4dff7ed..c126c5d 100644 --- a/examples/example_13/example_13.cpp +++ b/examples/example_13/example_13.cpp @@ -10,16 +10,14 @@ #include #endif +#include "bitrl/bitrl_consts.h" #include "chrono/core/ChRealtimeStep.h" #include "chrono/physics/ChSystemNSC.h" #include -#include "chrono/physics/ChLinkMotorRotationSpeed.h" #include #include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" #include -#include -#include #include namespace example_13 @@ -71,7 +69,10 @@ int main() bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot robot(sys, chrono::ChVector3d(0, 0, -0.45), chrono::QUNIT); + robot.init(); + robot.set_motor_speed(bitrl::consts::maths::PI, 0); + robot.set_motor_speed(bitrl::consts::maths::PI, 1); chrono::irrlicht::ChVisualSystemIrrlicht visual; prepare_visualization(visual); @@ -99,6 +100,7 @@ int main() // ADVANCE SYSTEM STATE BY ONE STEP sys.DoStepDynamics(DT); + // Enforce soft real-time realtime_timer.Spin(DT); diff --git a/examples/example_13/example_13.md b/examples/example_13/example_13.md index 7670577..057dd06 100644 --- a/examples/example_13/example_13.md +++ b/examples/example_13/example_13.md @@ -10,10 +10,132 @@ 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. The robot we will develop follows the -Turtlebot robot model defined in Chrono. +In this example we will use the \ref bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot "bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot" class +to simulate a differential drive system using Chrono. The \ref bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot "bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot" +follows the Turtlebot robot model defined in Chrono. +You will need the mesh files from the Chrono project in order to visualize the robot. These files should be place in the +\ref ROBOTS_DATA_DIR directory under the _diff_drive_robot_ subdirectory of the project. Below is the driver code. @code{.cpp} +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/bitrl_types.h" + + +#ifdef BITRL_LOG +#define BOOST_LOG_DYN_LINK +#include +#endif + +#include "chrono/core/ChRealtimeStep.h" +#include "chrono/physics/ChSystemNSC.h" +#include +#include +#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" + +#include +#include + +namespace example_13 +{ +using namespace bitrl; +using namespace chrono::irrlicht; + +// constants we will be using further below +const uint_t WINDOW_HEIGHT = 800; +const uint_t WINDOW_WIDTH = 1024; +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) +{ +visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); //WINDOW_HEIGHT); +visual.SetWindowTitle(WINDOW_TITLE); +visual.Initialize(); + + visual.AddLogo(); + visual.AddSkyBox(); + visual.AddCamera({0, -2, 1}, {0, 0, 0}); + visual.AddTypicalLights(); + visual.BindAll(); +} + +} // namespace example_13 + +int main() +{ +using namespace example_13; +chrono::ChSystemNSC sys; +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); + + bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot robot(sys, + chrono::ChVector3d(0, 0, -0.45), chrono::QUNIT); + + robot.init(); + robot.set_motor_speed(bitrl::consts::maths::PI, 0); + robot.set_motor_speed(bitrl::consts::maths::PI, 1); + + chrono::irrlicht::ChVisualSystemIrrlicht visual; + prepare_visualization(visual); + visual.AttachSystem(&sys); + + // Simulation loop + + // Timer for enforcing soft real-time + chrono::ChRealtimeStepTimer realtime_timer; + + // bool removed = false; + while (visual.Run()) { + // Irrlicht must prepare frame to draw + visual.BeginScene(); + + // Irrlicht now draws simple lines in 3D world representing a + // skeleton of the mechanism, in this instant: + // + // .. draw items belonging to Irrlicht scene, if any + visual.Render(); + // .. draw a grid + tools::drawGrid(&visual, 0.5, 0.5); + // .. draw GUI items belonging to Irrlicht screen, if any + visual.GetGUIEnvironment()->drawAll(); + + // ADVANCE SYSTEM STATE BY ONE STEP + sys.DoStepDynamics(DT); + + // Enforce soft real-time + realtime_timer.Spin(DT); + + // Irrlicht must finish drawing the frame + visual.EndScene(); + } + + return 0; +} +#else +#include +int main() +{ +std::cerr<<"You need PROJECTCHRONO configured with " +<<"bitrl in order to run this example " +<<"Reconfigure bitrl and set ENABLE_CHRONO=ON"< #include #include "chrono/physics/ChBodyEasy.h" @@ -12,6 +14,8 @@ + + namespace bitrl { namespace rb::bitrl_chrono{ @@ -19,15 +23,15 @@ namespace rb::bitrl_chrono{ // ============================================================================= // Create default contact material for the robot -std::shared_ptr DefaultContactMaterial(chrono::ChContactMethod contact_method) { - float mu = 0.4f; // coefficient of friction - float cr = 0.0f; // coefficient of restitution - float Y = 2e7f; // Young's modulus - float nu = 0.3f; // Poisson ratio - float kn = 2e5f; // normal stiffness - float gn = 40.0f; // normal viscous damping - float kt = 2e5f; // tangential stiffness - float gt = 20.0f; // tangential viscous damping +std::shared_ptr build_default_contact_material(chrono::ChContactMethod contact_method) { + float_t mu = 0.4f; // coefficient of friction + float_t cr = 0.0f; // coefficient of restitution + float_t Y = 2e7f; // Young's modulus + float_t nu = 0.3f; // Poisson ratio + float_t kn = 2e5f; // normal stiffness + float_t gn = 40.0f; // normal viscous damping + float_t kt = 2e5f; // tangential stiffness + float_t gt = 20.0f; // tangential viscous damping switch (contact_method) { case chrono::ChContactMethod::NSC: { @@ -55,7 +59,7 @@ std::shared_ptr DefaultContactMaterial(chrono::ChCont // Add a revolute joint between body_1 and body_2 // rel_joint_pos and rel_joint_rot are the position and the rotation of the revolute point -void AddRevoluteJoint(std::shared_ptr body_1, +void add_revolute_joint(std::shared_ptr body_1, std::shared_ptr body_2, std::shared_ptr chassis, chrono::ChSystem* system, @@ -72,7 +76,7 @@ void AddRevoluteJoint(std::shared_ptr body_1, // Add a revolute joint between body_1 and body_2 // rel_joint_pos and rel_joint_rot are the position and the rotation of the revolute point -void AddRevoluteJoint(std::shared_ptr body_1, +void add_revolute_joint(std::shared_ptr body_1, std::shared_ptr body_2, std::shared_ptr chassis, chrono::ChSystem* system, @@ -89,7 +93,7 @@ void AddRevoluteJoint(std::shared_ptr body_1, // Add a revolute joint between body_1 and body_2 // rel_joint_pos and rel_joint_rot are the position and the rotation of the revolute point -void AddLockJoint(std::shared_ptr body_1, +void add_lock_joint(std::shared_ptr body_1, std::shared_ptr body_2, std::shared_ptr chassis, chrono::ChSystem* system, @@ -107,7 +111,7 @@ void AddLockJoint(std::shared_ptr body_1, // Add a rotational speed controlled motor between body_1 and body_2 // rel_joint_pos and rel_joint_rot are the position and the rotation of the motor -std::shared_ptr AddMotor(std::shared_ptr body_1, +std::shared_ptr add_motor(std::shared_ptr body_1, std::shared_ptr body_2, std::shared_ptr chassis, chrono::ChSystem* system, @@ -130,327 +134,321 @@ CHRONO_DiffDriveRobot::CHRONO_DiffDriveRobot(chrono::ChSystem& system, const chrono::ChVector3d& robot_pos, const chrono::ChQuaternion<>& robot_rot, std::shared_ptr wheel_mat) - : m_system(&system), m_robot_pos(robot_pos), m_robot_rot(robot_rot), m_wheel_material(wheel_mat) { + : + system_(&system), + robot_pos_(robot_pos), + robot_rot_(robot_rot), + wheel_material_(wheel_mat), + pose_() + { // Set default collision model envelope commensurate with model dimensions. // Note that an SMC system automatically sets envelope to 0. - auto contact_method = m_system->GetContactMethod(); + auto contact_method = system_->GetContactMethod(); if (contact_method == chrono::ChContactMethod::NSC) { chrono::ChCollisionModel::SetDefaultSuggestedEnvelope(0.01); chrono::ChCollisionModel::SetDefaultSuggestedMargin(0.005); } // Create the contact materials - m_chassis_material = DefaultContactMaterial(contact_method); - if (!m_wheel_material) - m_wheel_material = DefaultContactMaterial(contact_method); + chassis_material_ = build_default_contact_material(contact_method); + if (!wheel_material_) + wheel_material_ = build_default_contact_material(contact_method); - create(); } -void CHRONO_DiffDriveRobot::create() { +void CHRONO_DiffDriveRobot::init() { // initialize robot chassis - m_chassis = chrono_types::make_shared("chassis", false, m_chassis_material, m_system, - m_robot_pos, m_robot_rot, true); + chassis_ = chrono_types::make_shared(chassis_material_, system_, + robot_pos_, robot_rot_); + chassis_ -> init(); + + // set the pointer of the Chrono body that + // the pose should trac + pose_.set_body(chassis_->get_body_ptr()); // active drive wheels' positions relative to the chassis - double dwx = 0; - double dwy = 0.11505; - double dwz = 0.03735; - m_drive_wheels.push_back(chrono_types::make_shared( - "LDWheel", false, m_wheel_material, m_system, chrono::ChVector3d(dwx, +dwy, dwz), chrono::ChQuaternion<>(1, 0, 0, 0), - m_chassis->get_body_ptr(), true)); - m_drive_wheels.push_back(chrono_types::make_shared( - "RDWheel", false, m_wheel_material, m_system, chrono::ChVector3d(dwx, -dwy, dwz), chrono::ChQuaternion<>(1, 0, 0, 0), - m_chassis->get_body_ptr(), true)); + real_t dwx = 0; + real_t dwy = 0.11505; + real_t dwz = 0.03735; + drive_wheels_.push_back(chrono_types::make_shared( + "LDWheel", wheel_material_, system_, chrono::ChVector3d(dwx, +dwy, dwz), chrono::ChQuaternion<>(1, 0, 0, 0), + chassis_->get_body_ptr())); + drive_wheels_.push_back(chrono_types::make_shared( + "RDWheel", wheel_material_, system_, chrono::ChVector3d(dwx, -dwy, dwz), chrono::ChQuaternion<>(1, 0, 0, 0), + chassis_->get_body_ptr())); // passive driven wheels' positions relative to the chassis - double pwx = 0.11505; - double pwy = 0; - double pwz = 0.02015; - - m_passive_wheels.push_back(chrono_types::make_shared( - "FPWheel", false, m_wheel_material, m_system, chrono::ChVector3d(pwx, pwy, pwz), chrono::ChQuaternion<>(1, 0, 0, 0), - m_chassis->get_body_ptr(), true)); - m_passive_wheels.push_back(chrono_types::make_shared( - "RPWheel", false, m_wheel_material, m_system, chrono::ChVector3d(-pwx, pwy, pwz), chrono::ChQuaternion<>(1, 0, 0, 0), - m_chassis->get_body_ptr(), true)); + real_t pwx = 0.11505; + real_t pwy = 0; + real_t pwz = 0.02015; + + passive_wheels_.push_back(chrono_types::make_shared( + "FPWheel", wheel_material_, system_, chrono::ChVector3d(pwx, pwy, pwz), chrono::ChQuaternion<>(1, 0, 0, 0), + chassis_->get_body_ptr())); + passive_wheels_.push_back(chrono_types::make_shared( + "RPWheel", wheel_material_, system_, chrono::ChVector3d(-pwx, pwy, pwz), chrono::ChQuaternion<>(1, 0, 0, 0), + chassis_->get_body_ptr())); + + for (int_t i = 0; i < 2; i++) { + drive_wheels_[i]->init(); + passive_wheels_[i]->init(); + } // create the first level supporting rod - double rod_s_0_x = -0.0565; - double rod_s_0_y = 0.11992; - double rod_s_0_z = 0.09615; - - double rod_s_1_x = 0.0535; - double rod_s_1_y = 0.11992; - double rod_s_1_z = 0.09615; - - double rod_s_2_x = 0.11850; - double rod_s_2_y = 0.08192; - double rod_s_2_z = 0.09615; - - double rod_s_3_x = 0.11850; - double rod_s_3_y = -0.08192; - double rod_s_3_z = 0.09615; - - double rod_s_4_x = 0.0535; - double rod_s_4_y = -0.11992; - double rod_s_4_z = 0.09615; - - double rod_s_5_x = -0.0565; - double rod_s_5_y = -0.11992; - double rod_s_5_z = 0.09615; - - m_1st_level_rods.push_back(chrono_types::make_shared( - "0-bottom-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_s_0_x, rod_s_0_y, rod_s_0_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_1st_level_rods.push_back(chrono_types::make_shared( - "1-bottom-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_s_1_x, rod_s_1_y, rod_s_1_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_1st_level_rods.push_back(chrono_types::make_shared( - "2-bottom-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_s_2_x, rod_s_2_y, rod_s_2_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_1st_level_rods.push_back(chrono_types::make_shared( - "3-bottom-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_s_3_x, rod_s_3_y, rod_s_3_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_1st_level_rods.push_back(chrono_types::make_shared( - "4-bottom-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_s_4_x, rod_s_4_y, rod_s_4_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_1st_level_rods.push_back(chrono_types::make_shared( - "5-bottom-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_s_5_x, rod_s_5_y, rod_s_5_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + real_t rod_s_0_x = -0.0565; + real_t rod_s_0_y = 0.11992; + real_t rod_s_0_z = 0.09615; + + real_t rod_s_1_x = 0.0535; + real_t rod_s_1_y = 0.11992; + real_t rod_s_1_z = 0.09615; + + real_t rod_s_2_x = 0.11850; + real_t rod_s_2_y = 0.08192; + real_t rod_s_2_z = 0.09615; + + real_t rod_s_3_x = 0.11850; + real_t rod_s_3_y = -0.08192; + real_t rod_s_3_z = 0.09615; + + real_t rod_s_4_x = 0.0535; + real_t rod_s_4_y = -0.11992; + real_t rod_s_4_z = 0.09615; + + real_t rod_s_5_x = -0.0565; + real_t rod_s_5_y = -0.11992; + real_t rod_s_5_z = 0.09615; + + m_1st_level_rods_.push_back(chrono_types::make_shared( + "0-bottom-rod", wheel_material_, system_, chrono::ChVector3d(rod_s_0_x, rod_s_0_y, rod_s_0_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_1st_level_rods_.push_back(chrono_types::make_shared( + "1-bottom-rod", wheel_material_, system_, chrono::ChVector3d(rod_s_1_x, rod_s_1_y, rod_s_1_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_1st_level_rods_.push_back(chrono_types::make_shared( + "2-bottom-rod", wheel_material_, system_, chrono::ChVector3d(rod_s_2_x, rod_s_2_y, rod_s_2_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_1st_level_rods_.push_back(chrono_types::make_shared( + "3-bottom-rod", wheel_material_, system_, chrono::ChVector3d(rod_s_3_x, rod_s_3_y, rod_s_3_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_1st_level_rods_.push_back(chrono_types::make_shared( + "4-bottom-rod", wheel_material_, system_, chrono::ChVector3d(rod_s_4_x, rod_s_4_y, rod_s_4_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_1st_level_rods_.push_back(chrono_types::make_shared( + "5-bottom-rod", wheel_material_, system_, chrono::ChVector3d(rod_s_5_x, rod_s_5_y, rod_s_5_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); // add the bottom plate - double bt_plate_x = 0; - double bt_plate_y = 0; - double bt_plate_z = 0.14615; + real_t bt_plate_x = 0; + real_t bt_plate_y = 0; + real_t bt_plate_z = 0.14615; - m_bottom_plate = chrono_types::make_shared( - "bottom_plate", false, m_wheel_material, m_system, chrono::ChVector3d(bt_plate_x, bt_plate_y, bt_plate_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true); + bottom_plate_ = chrono_types::make_shared(wheel_material_, system_, + chrono::ChVector3d(bt_plate_x, bt_plate_y, bt_plate_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr()); + bottom_plate_ -> init(); // create the second level support rod - double rod_m_0_x = -0.10394; - double rod_m_0_y = 0.09792; - double rod_m_0_z = 0.15015; - - double rod_m_1_x = -0.0015; - double rod_m_1_y = 0.16192; - double rod_m_1_z = 0.15015; - - double rod_m_2_x = 0.0687; - double rod_m_2_y = 0.13132; - double rod_m_2_z = 0.15015; - - double rod_m_3_x = 0.0687; - double rod_m_3_y = -0.13132; - double rod_m_3_z = 0.15015; - - double rod_m_4_x = -0.0015; - double rod_m_4_y = -0.16192; - double rod_m_4_z = 0.15015; - - double rod_m_5_x = -0.10394; - double rod_m_5_y = -0.09792; - double rod_m_5_z = 0.15015; - - m_2nd_level_rods.push_back(chrono_types::make_shared( - "0-middle-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_m_0_x, rod_m_0_y, rod_m_0_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_2nd_level_rods.push_back(chrono_types::make_shared( - "1-middle-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_m_1_x, rod_m_1_y, rod_m_1_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_2nd_level_rods.push_back(chrono_types::make_shared( - "2-middle-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_m_2_x, rod_m_2_y, rod_m_2_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_2nd_level_rods.push_back(chrono_types::make_shared( - "3-middle-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_m_3_x, rod_m_3_y, rod_m_3_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_2nd_level_rods.push_back(chrono_types::make_shared( - "4-middle-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_m_4_x, rod_m_4_y, rod_m_4_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_2nd_level_rods.push_back(chrono_types::make_shared( - "5-middle-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_m_5_x, rod_m_5_y, rod_m_5_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + real_t rod_m_0_x = -0.10394; + real_t rod_m_0_y = 0.09792; + real_t rod_m_0_z = 0.15015; + + real_t rod_m_1_x = -0.0015; + real_t rod_m_1_y = 0.16192; + real_t rod_m_1_z = 0.15015; + + real_t rod_m_2_x = 0.0687; + real_t rod_m_2_y = 0.13132; + real_t rod_m_2_z = 0.15015; + + real_t rod_m_3_x = 0.0687; + real_t rod_m_3_y = -0.13132; + real_t rod_m_3_z = 0.15015; + + real_t rod_m_4_x = -0.0015; + real_t rod_m_4_y = -0.16192; + real_t rod_m_4_z = 0.15015; + + real_t rod_m_5_x = -0.10394; + real_t rod_m_5_y = -0.09792; + real_t rod_m_5_z = 0.15015; + + m_2nd_level_rods_.push_back(chrono_types::make_shared( + "0-middle-rod", wheel_material_, system_, chrono::ChVector3d(rod_m_0_x, rod_m_0_y, rod_m_0_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_2nd_level_rods_.push_back(chrono_types::make_shared( + "1-middle-rod", wheel_material_, system_, chrono::ChVector3d(rod_m_1_x, rod_m_1_y, rod_m_1_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_2nd_level_rods_.push_back(chrono_types::make_shared( + "2-middle-rod", wheel_material_, system_, chrono::ChVector3d(rod_m_2_x, rod_m_2_y, rod_m_2_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_2nd_level_rods_.push_back(chrono_types::make_shared( + "3-middle-rod", wheel_material_, system_, chrono::ChVector3d(rod_m_3_x, rod_m_3_y, rod_m_3_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_2nd_level_rods_.push_back(chrono_types::make_shared( + "4-middle-rod", wheel_material_, system_, chrono::ChVector3d(rod_m_4_x, rod_m_4_y, rod_m_4_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_2nd_level_rods_.push_back(chrono_types::make_shared( + "5-middle-rod", wheel_material_, system_, chrono::ChVector3d(rod_m_5_x, rod_m_5_y, rod_m_5_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); // add the middle plate - double mi_plate_x = 0; - double mi_plate_y = 0; - double mi_plate_z = 0.20015; - m_middle_plate = chrono_types::make_shared( - "middle_plate", false, m_wheel_material, m_system, chrono::ChVector3d(mi_plate_x, mi_plate_y, mi_plate_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true); + real_t mi_plate_x = 0; + real_t mi_plate_y = 0; + real_t mi_plate_z = 0.20015; + middle_plate_ = chrono_types::make_shared( + wheel_material_, system_, chrono::ChVector3d(mi_plate_x, mi_plate_y, mi_plate_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr()); + middle_plate_ -> init(); // create the third level support rod - double rod_u_0_x = -0.10394; - double rod_u_0_y = 0.09792; - double rod_u_0_z = 0.20615; - - double rod_u_1_x = -0.0015; - double rod_u_1_y = 0.16192; - double rod_u_1_z = 0.20615; - - double rod_u_2_x = 0.0687; - double rod_u_2_y = 0.13132; - double rod_u_2_z = 0.20615; - - double rod_u_3_x = 0.0687; - double rod_u_3_y = -0.13132; - double rod_u_3_z = 0.20615; - - double rod_u_4_x = -0.0015; - double rod_u_4_y = -0.16192; - double rod_u_4_z = 0.20615; - - double rod_u_5_x = -0.10394; - double rod_u_5_y = -0.09792; - double rod_u_5_z = 0.20615; - - m_3rd_level_rods.push_back(chrono_types::make_shared( - "0-top-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_u_0_x, rod_u_0_y, rod_u_0_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_3rd_level_rods.push_back(chrono_types::make_shared( - "1-top-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_u_1_x, rod_u_1_y, rod_u_1_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_3rd_level_rods.push_back(chrono_types::make_shared( - "2-top-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_u_2_x, rod_u_2_y, rod_u_2_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_3rd_level_rods.push_back(chrono_types::make_shared( - "3-top-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_u_3_x, rod_u_3_y, rod_u_3_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_3rd_level_rods.push_back(chrono_types::make_shared( - "4-top-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_u_4_x, rod_u_4_y, rod_u_4_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); - m_3rd_level_rods.push_back(chrono_types::make_shared( - "5-top-rod", false, m_wheel_material, m_system, chrono::ChVector3d(rod_u_5_x, rod_u_5_y, rod_u_5_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true)); + real_t rod_u_0_x = -0.10394; + real_t rod_u_0_y = 0.09792; + real_t rod_u_0_z = 0.20615; + + real_t rod_u_1_x = -0.0015; + real_t rod_u_1_y = 0.16192; + real_t rod_u_1_z = 0.20615; + + real_t rod_u_2_x = 0.0687; + real_t rod_u_2_y = 0.13132; + real_t rod_u_2_z = 0.20615; + + real_t rod_u_3_x = 0.0687; + real_t rod_u_3_y = -0.13132; + real_t rod_u_3_z = 0.20615; + + real_t rod_u_4_x = -0.0015; + real_t rod_u_4_y = -0.16192; + real_t rod_u_4_z = 0.20615; + + real_t rod_u_5_x = -0.10394; + real_t rod_u_5_y = -0.09792; + real_t rod_u_5_z = 0.20615; + + m_3rd_level_rods_.push_back(chrono_types::make_shared( + "0-top-rod", wheel_material_, system_, chrono::ChVector3d(rod_u_0_x, rod_u_0_y, rod_u_0_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_3rd_level_rods_.push_back(chrono_types::make_shared( + "1-top-rod", wheel_material_, system_, chrono::ChVector3d(rod_u_1_x, rod_u_1_y, rod_u_1_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_3rd_level_rods_.push_back(chrono_types::make_shared( + "2-top-rod", wheel_material_, system_, chrono::ChVector3d(rod_u_2_x, rod_u_2_y, rod_u_2_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_3rd_level_rods_.push_back(chrono_types::make_shared( + "3-top-rod", wheel_material_, system_, chrono::ChVector3d(rod_u_3_x, rod_u_3_y, rod_u_3_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_3rd_level_rods_.push_back(chrono_types::make_shared( + "4-top-rod", wheel_material_, system_, chrono::ChVector3d(rod_u_4_x, rod_u_4_y, rod_u_4_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + m_3rd_level_rods_.push_back(chrono_types::make_shared( + "5-top-rod", wheel_material_, system_, chrono::ChVector3d(rod_u_5_x, rod_u_5_y, rod_u_5_z), + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr())); + + + for (int_t i = 0; i < 6; i++) { + m_1st_level_rods_[i]->init(); + m_2nd_level_rods_[i]->init(); + m_3rd_level_rods_[i]->init(); + } // add the top plate - double top_plate_x = 0; - double top_plate_y = 0; - double top_plate_z = 0.40615; - m_top_plate = chrono_types::make_shared("top_plate", false, m_wheel_material, m_system, + real_t top_plate_x = 0; + real_t top_plate_y = 0; + real_t top_plate_z = 0.40615; + top_plate_ = chrono_types::make_shared(wheel_material_, system_, chrono::ChVector3d(top_plate_x, top_plate_y, top_plate_z), - chrono::ChQuaternion<>(1, 0, 0, 0), m_chassis->get_body_ptr(), true); -} + chrono::ChQuaternion<>(1, 0, 0, 0), chassis_->get_body_ptr()); -/// Initialize the complete rover and add all constraints -void CHRONO_DiffDriveRobot::init() { + top_plate_ -> init(); - m_chassis->init(); - m_bottom_plate->init(); - m_middle_plate->init(); - m_top_plate->init(); - for (int i = 0; i < 2; i++) { - m_drive_wheels[i]->init(); - m_passive_wheels[i]->init(); - } + this -> build_motors_(); +} - for (int i = 0; i < 6; i++) { - m_1st_level_rods[i]->init(); - m_2nd_level_rods[i]->init(); - m_3rd_level_rods[i]->init(); - } +/// Initialize the complete rover and add all constraints +void CHRONO_DiffDriveRobot::build_motors_() { // redeclare necessary location variables - double dwx = 0; - double dwy = 0.11505; - double dwz = 0.03735; - - double pwx = 0.11505; - double pwy = 0; - double pwz = 0.02015; - - double rod_s_0_x = -0.0565; - double rod_s_0_y = 0.11992; - double rod_s_0_z = 0.09615; - - double rod_s_1_x = 0.0535; - double rod_s_1_y = 0.11992; - double rod_s_1_z = 0.09615; - - double rod_s_2_x = 0.11850; - double rod_s_2_y = 0.08192; - double rod_s_2_z = 0.09615; - - double rod_s_3_x = 0.11850; - double rod_s_3_y = -0.08192; - double rod_s_3_z = 0.09615; - - double rod_s_4_x = 0.0535; - double rod_s_4_y = -0.11992; - double rod_s_4_z = 0.09615; - - double rod_s_5_x = -0.0565; - double rod_s_5_y = -0.11992; - double rod_s_5_z = 0.09615; - - double rod_m_0_x = -0.10394; - double rod_m_0_y = 0.09792; - double rod_m_0_z = 0.15015; - - double rod_m_1_x = -0.0015; - double rod_m_1_y = 0.16192; - double rod_m_1_z = 0.15015; - - double rod_m_2_x = 0.0687; - double rod_m_2_y = 0.13132; - double rod_m_2_z = 0.15015; - - double rod_m_3_x = 0.0687; - double rod_m_3_y = -0.13132; - double rod_m_3_z = 0.15015; - - double rod_m_4_x = -0.0015; - double rod_m_4_y = -0.16192; - double rod_m_4_z = 0.15015; - - double rod_m_5_x = -0.10394; - double rod_m_5_y = -0.09792; - double rod_m_5_z = 0.15015; - - double rod_u_0_x = -0.10394; - double rod_u_0_y = 0.09792; - double rod_u_0_z = 0.20615; - - double rod_u_1_x = -0.0015; - double rod_u_1_y = 0.16192; - double rod_u_1_z = 0.20615; - - double rod_u_2_x = 0.0687; - double rod_u_2_y = 0.13132; - double rod_u_2_z = 0.20615; - - double rod_u_3_x = 0.0687; - double rod_u_3_y = -0.13132; - double rod_u_3_z = 0.20615; - - double rod_u_4_x = -0.0015; - double rod_u_4_y = -0.16192; - double rod_u_4_z = 0.20615; - - double rod_u_5_x = -0.10394; - double rod_u_5_y = -0.09792; - double rod_u_5_z = 0.20615; + real_t dwx = 0; + real_t dwy = 0.11505; + real_t dwz = 0.03735; + real_t pwx = 0.11505; + real_t pwy = 0; + real_t pwz = 0.02015; + real_t rod_s_0_x = -0.0565; + real_t rod_s_0_y = 0.11992; + real_t rod_s_0_z = 0.09615; + real_t rod_s_1_x = 0.0535; + real_t rod_s_1_y = 0.11992; + real_t rod_s_1_z = 0.09615; + real_t rod_s_2_x = 0.11850; + real_t rod_s_2_y = 0.08192; + real_t rod_s_2_z = 0.09615; + real_t rod_s_3_x = 0.11850; + real_t rod_s_3_y = -0.08192; + real_t rod_s_3_z = 0.09615; + real_t rod_s_4_x = 0.0535; + real_t rod_s_4_y = -0.11992; + real_t rod_s_4_z = 0.09615; + real_t rod_s_5_x = -0.0565; + real_t rod_s_5_y = -0.11992; + real_t rod_s_5_z = 0.09615; + real_t rod_m_0_x = -0.10394; + real_t rod_m_0_y = 0.09792; + real_t rod_m_0_z = 0.15015; + real_t rod_m_1_x = -0.0015; + real_t rod_m_1_y = 0.16192; + real_t rod_m_1_z = 0.15015; + real_t rod_m_2_x = 0.0687; + real_t rod_m_2_y = 0.13132; + real_t rod_m_2_z = 0.15015; + real_t rod_m_3_x = 0.0687; + real_t rod_m_3_y = -0.13132; + real_t rod_m_3_z = 0.15015; + real_t rod_m_4_x = -0.0015; + real_t rod_m_4_y = -0.16192; + real_t rod_m_4_z = 0.15015; + real_t rod_m_5_x = -0.10394; + real_t rod_m_5_y = -0.09792; + real_t rod_m_5_z = 0.15015; + real_t rod_u_0_x = -0.10394; + real_t rod_u_0_y = 0.09792; + real_t rod_u_0_z = 0.20615; + real_t rod_u_1_x = -0.0015; + real_t rod_u_1_y = 0.16192; + real_t rod_u_1_z = 0.20615; + real_t rod_u_2_x = 0.0687; + real_t rod_u_2_y = 0.13132; + real_t rod_u_2_z = 0.20615; + real_t rod_u_3_x = 0.0687; + real_t rod_u_3_y = -0.13132; + real_t rod_u_3_z = 0.20615; + real_t rod_u_4_x = -0.0015; + real_t rod_u_4_y = -0.16192; + real_t rod_u_4_z = 0.20615; + real_t rod_u_5_x = -0.10394; + real_t rod_u_5_y = -0.09792; + real_t rod_u_5_z = 0.20615; // add motors and revolute joints on the active and passive wheels auto const_speed_function_l = chrono_types::make_shared(-chrono::CH_PI); auto const_speed_function_r = chrono_types::make_shared(-chrono::CH_PI); - m_motors_func.push_back(const_speed_function_l); - m_motors_func.push_back(const_speed_function_r); + motors_func_.push_back(const_speed_function_l); + motors_func_.push_back(const_speed_function_r); chrono::ChQuaternion<> z2y = chrono::QuatFromAngleX(chrono::CH_PI_2); chrono::ChQuaternion<> z2x = chrono::QuatFromAngleY(-chrono::CH_PI_2); - m_motors.push_back(AddMotor(m_drive_wheels[0]->get_body_ptr(), m_chassis->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + motors_.push_back(add_motor(drive_wheels_[0]->get_body_ptr(), chassis_->get_body_ptr(), chassis_->get_body_ptr(), system_, chrono::ChVector3d(dwx, dwy, dwz), z2y, const_speed_function_l)); - AddRevoluteJoint(m_passive_wheels[0]->get_body_ptr(), m_chassis->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + add_revolute_joint(passive_wheels_[0]->get_body_ptr(), chassis_->get_body_ptr(), chassis_->get_body_ptr(), system_, chrono::ChVector3d(pwx, pwy, pwz), z2x); - m_motors.push_back(AddMotor(m_drive_wheels[1]->get_body_ptr(), m_chassis->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + motors_.push_back(add_motor(drive_wheels_[1]->get_body_ptr(), chassis_->get_body_ptr(), chassis_->get_body_ptr(), system_, chrono::ChVector3d(dwx, -dwy, dwz), z2y, const_speed_function_r)); - AddRevoluteJoint(m_passive_wheels[1]->get_body_ptr(), m_chassis->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + add_revolute_joint(passive_wheels_[1]->get_body_ptr(), chassis_->get_body_ptr(), chassis_->get_body_ptr(), system_, chrono::ChVector3d(-pwx, pwy, pwz), z2x); // add fixity on all rods and plates @@ -481,36 +479,36 @@ void CHRONO_DiffDriveRobot::init() { chrono::ChVector3d(rod_u_4_x, rod_u_4_y, rod_u_4_z), // chrono::ChVector3d(rod_u_5_x, rod_u_5_y, rod_u_5_z)}; - for (int i = 0; i < 6; i++) { + for (int_t i = 0; i < 6; i++) { chrono::ChVector3d bottom_plate_rel_pos = bottom_rod_rel_pos[i] + chrono::ChVector3d(0, 0, 0.05); chrono::ChVector3d middle_plate_rel_pos = middle_rod_rel_pos[i] + chrono::ChVector3d(0, 0, 0.05); chrono::ChVector3d top_plate_rel_pos = top_rod_rel_pos[i] + chrono::ChVector3d(0, 0, 0.2); - AddLockJoint(m_1st_level_rods[i]->get_body_ptr(), m_chassis->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + add_lock_joint(m_1st_level_rods_[i]->get_body_ptr(), chassis_->get_body_ptr(), chassis_->get_body_ptr(), system_, bottom_rod_rel_pos[i], chrono::ChQuaternion<>(1, 0, 0, 0)); - AddLockJoint(m_1st_level_rods[i]->get_body_ptr(), m_bottom_plate->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + add_lock_joint(m_1st_level_rods_[i]->get_body_ptr(), bottom_plate_->get_body_ptr(), chassis_->get_body_ptr(), system_, bottom_plate_rel_pos, chrono::ChQuaternion<>(1, 0, 0, 0)); - AddLockJoint(m_2nd_level_rods[i]->get_body_ptr(), m_bottom_plate->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + add_lock_joint(m_2nd_level_rods_[i]->get_body_ptr(), bottom_plate_->get_body_ptr(), chassis_->get_body_ptr(), system_, bottom_rod_rel_pos[i], chrono::ChQuaternion<>(1, 0, 0, 0)); - AddLockJoint(m_2nd_level_rods[i]->get_body_ptr(), m_middle_plate->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + add_lock_joint(m_2nd_level_rods_[i]->get_body_ptr(), middle_plate_->get_body_ptr(), chassis_->get_body_ptr(), system_, middle_plate_rel_pos, chrono::ChQuaternion<>(1, 0, 0, 0)); - AddLockJoint(m_3rd_level_rods[i]->get_body_ptr(), m_middle_plate->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + add_lock_joint(m_3rd_level_rods_[i]->get_body_ptr(), middle_plate_->get_body_ptr(), chassis_->get_body_ptr(), system_, top_rod_rel_pos[i], chrono::ChQuaternion<>(1, 0, 0, 0)); - AddLockJoint(m_3rd_level_rods[i]->get_body_ptr(), m_top_plate->get_body_ptr(), m_chassis->get_body_ptr(), m_system, + add_lock_joint(m_3rd_level_rods_[i]->get_body_ptr(), top_plate_->get_body_ptr(), chassis_->get_body_ptr(), system_, top_plate_rel_pos, chrono::ChQuaternion<>(1, 0, 0, 0)); } } void CHRONO_DiffDriveRobot::set_motor_speed(real_t rad_speed, uint_t id) { - m_motors_func[id]->SetConstant(rad_speed); + motors_func_[id]->SetConstant(rad_speed); } chrono::ChVector3d CHRONO_DiffDriveRobot::get_active_wheel_speed(uint_t id)const { - return m_drive_wheels[id]->get_body_ptr()->GetPosDt(); + return drive_wheels_[id]->get_body_ptr()->GetPosDt(); } chrono::ChVector3d CHRONO_DiffDriveRobot::get_active_wheel_angular_velocity(uint_t id)const { - return m_drive_wheels[id]->get_body_ptr()->GetAngVelParent(); + return drive_wheels_[id]->get_body_ptr()->GetAngVelParent(); } } diff --git a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h index a42c39a..5cd698c 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h +++ b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h @@ -25,15 +25,10 @@ #include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h" #include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h" #include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h" +#include "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" -#include "chrono/assets/ChColor.h" #include "chrono/physics/ChSystem.h" #include "chrono/physics/ChLinkMotorRotationSpeed.h" - -#include -#include -#include -#include #include @@ -42,14 +37,6 @@ namespace bitrl namespace rb::bitrl_chrono{ - -/// Turtlebot Robot class -/// This class assemble and initialize a complete turtlebot robot -/// This class also handles general control commands of the robot -/** - * @class CHRONO_DiffDriveRobotBase - */ - /** * @class CHRONO_DiffDriveRobotBase * @ingroup rb_chrono @@ -57,19 +44,21 @@ namespace rb::bitrl_chrono{ * in fact the implementation of this class is taken from the * Chrono::TurtleBot. * - * for more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html - * + * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html */ -class CHRONO_DiffDriveRobot { +class CHRONO_DiffDriveRobot final { public: CHRONO_DiffDriveRobot(chrono::ChSystem& system, const chrono::ChVector3d& robot_pos, const chrono::ChQuaternion<>& robot_rot, std::shared_ptr wheel_mat = {}); - virtual ~CHRONO_DiffDriveRobot()=default; - /// Initialize the turtlebot robot using current parameters. + /// destructor + ~CHRONO_DiffDriveRobot()=default; + + /// Initialize the CHRONO_DiffDriveRobot robot + /// The robot will not be constructed in the ChSystem until init() is called. void init(); /// Set active drive wheel speed @@ -81,36 +70,43 @@ class CHRONO_DiffDriveRobot { /// Get active driver wheel angular velocity chrono::ChVector3d get_active_wheel_angular_velocity(uint_t id)const; -private: - /// This function initializes all parameters for the robot. - /// Note: The robot will not be constructed in the ChSystem until Initialize() is called. - void create(); + /// Get the pose of the robot + const CHRONO_RobotPose& get_pose() const{return pose_;} - chrono::ChSystem* m_system; ///< pointer to the Chrono system +private: - bool m_dc_motor_control = false; + /// Build motors + void build_motors_(); - std::shared_ptr m_chassis; ///< robot chassis - std::vector> m_drive_wheels; ///< 2 active robot drive wheels - std::vector> m_passive_wheels; ///< 2 passive robot driven wheels + /// Pointer to the Chrono system + chrono::ChSystem* system_; - std::vector> m_1st_level_rods; ///< six first level supporting short rods - std::vector> m_2nd_level_rods; ///< six second level supporting short rods - std::vector> m_3rd_level_rods; ///< six third level support long rods - std::shared_ptr m_bottom_plate; ///< bottom plate of the turtlebot robot - std::shared_ptr m_middle_plate; ///< middle plate of the turtlebot robot - std::shared_ptr m_top_plate; ///< top plate of the turtlebot robot + /// robot chassis. We track the robot pose based on this + std::shared_ptr chassis_; + std::vector> drive_wheels_; ///< 2 active robot drive wheels + std::vector> passive_wheels_; ///< 2 passive robot driven wheels - chrono::ChQuaternion<> m_robot_rot; ///< robot rotation - chrono::ChVector3d m_robot_pos; ///< robot translation position + std::vector> m_1st_level_rods_; ///< six first level supporting short rods + std::vector> m_2nd_level_rods_; ///< six second level supporting short rods + std::vector> m_3rd_level_rods_; ///< six third level support long rods + std::shared_ptr bottom_plate_; ///< bottom plate of the turtlebot robot + std::shared_ptr middle_plate_; ///< middle plate of the turtlebot robot + std::shared_ptr top_plate_; ///< top plate of the turtlebot robot - std::vector> m_motors; ///< vector to store motors + chrono::ChQuaternion<> robot_rot_; ///< init robot rotation + chrono::ChVector3d robot_pos_; ///< init robot translation position - std::vector> m_motors_func; ///< constant motor angular speed func + std::vector> motors_; ///< vector to store motors + std::vector> motors_func_; ///< constant motor angular speed func // model parts material - std::shared_ptr m_chassis_material; ///< chassis contact material - std::shared_ptr m_wheel_material; ///< wheel contact material (shared across limbs) + std::shared_ptr chassis_material_; ///< chassis contact material + std::shared_ptr wheel_material_; + + /// The pose of the chassis + CHRONO_RobotPose pose_; + + bool dc_motor_control_ = false; }; diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.cpp index a2c6f0f..ca2324d 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.cpp @@ -2,9 +2,10 @@ #ifdef BITRL_CHRONO + #include "chrono/assets/ChColor.h" -#include "chrono/physics/ChLinkMotorRotationSpeed.h" +#include "bitrl/bitrl_types.h" #include "bitrl/bitrl_consts.h" #include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h" @@ -12,33 +13,31 @@ 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_Chassis::CHRONO_DiffDriveRobot_Chassis(std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot) : - 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; -} + CHRONO_DiffDriveRobot_Part("chassis", false, mat, system, body_pos, body_rot, nullptr, true) +{} void CHRONO_DiffDriveRobot_Chassis::init() { - const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); - std::cout<<"Visualization mesh file: "< is_initialized_) + { + return; + } + + this -> do_init_("chassis", chrono::ChVector3d(0, 0, 0), + chrono::ChColor(0.4f, 0.4f, 0.7f), 100.0); + + const std::string vis_mesh_file = get_vis_mesh_file(); 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; + real_t mmass; chrono::ChVector3d mcog; chrono::ChMatrix33<> minertia; trimesh->ComputeMassProperties(true, mmass, mcog, minertia); @@ -46,30 +45,26 @@ void CHRONO_DiffDriveRobot_Chassis::init() { chrono::ChVector3d principal_I; chrono::ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot); - m_body->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + 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); + body_->SetMass(mmass * density_); + body_->SetInertiaXX(density_ * principal_I); + body_->SetFrameRefToAbs(chrono::ChFrame<>(pos_, rot_)); + body_->SetFixed(fixed_); this -> add_collision_shapes(); - m_body->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::CHASSIS)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ACTIVE_WHEEL)); + body_->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::CHASSIS)); + 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); + system_->Add(body_); + this -> is_initialized_ = true; } void CHRONO_DiffDriveRobot_Chassis::translate(const chrono::ChVector3d& shift) { - m_body->SetPos(m_body->GetPos() + shift); + body_->SetPos(body_->GetPos() + shift); } diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h index a91c0de..1f36a2a 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h @@ -15,28 +15,38 @@ #include "chrono/physics/ChSystem.h" -#include "diff_drive_robot_part.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" #include "bitrl/bitrl_types.h" namespace bitrl{ namespace rb::bitrl_chrono{ + +/** + * @class CHRONO_DiffDriveRobot_Chassis + * @ingroup rb_chrono + * @brief Class for representing the chassi of the DiffDriveRobot. + * This class is a copy of the implementation provided by Chrono with + * a few additions. + * + * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html + */ class CHRONO_DiffDriveRobot_Chassis : public CHRONO_DiffDriveRobot_Part { public: - CHRONO_DiffDriveRobot_Chassis(const std::string& name, - bool fixed, + + // constructor + CHRONO_DiffDriveRobot_Chassis( std::shared_ptr mat, chrono::ChSystem* system, const chrono::ChVector3d& body_pos, - const chrono::ChQuaternion<>& body_rot, - bool collide); + const chrono::ChQuaternion<>& body_rot + ); + ~CHRONO_DiffDriveRobot_Chassis()=default; - /// Initialize the chassis at the specified (absolute) position. - void init(); + /// Initialize the chassis + virtual void init() override; - /// Enable/disable collision for the robot chassis. - void enable_collision(bool state); private: /// Translate the chassis by the specified value. diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp index e26250c..fd87d75 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp @@ -2,8 +2,7 @@ #ifdef BITRL_CHRONO -#include "diff_drive_robot_part.h" - +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" #include "bitrl/bitrl_consts.h" namespace bitrl{ @@ -16,45 +15,83 @@ CHRONO_DiffDriveRobot_Part::CHRONO_DiffDriveRobot_Part(const std::string& name, 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; + bool collide) + : +body_(nullptr), +contact_material_(mat), +chassis_(chassis_body), +pos_(body_pos), +rot_(body_rot), +system_(system), +name_(name), +mesh_name_(bitrl::consts::INVALID_STR), +density_(1.0), +collide_(collide), +fixed_(fixed), +is_initialized_(false) +{ + //init_body_(name); + // body_ = chrono_types::make_shared(); + // body_->SetName(name + "_body"); + // chassis_ = chassis_body; + // contact_material_ = mat; + // pos_ = body_pos; + // rot_ = body_rot; + // system_ = system; + // collide_ = collide; + // fixed_ = fixed; +} + + +void CHRONO_DiffDriveRobot_Part::do_init_(const std::string& mesh_name, const chrono::ChVector3d& offset, + const chrono::ChColor& color, real_t density) +{ + body_ = chrono_types::make_shared(); + body_->SetName(name_ + "_body"); + set_density(density); + set_mesh_name(mesh_name); + color_ = color; + offset_ = offset; +} + +std::string CHRONO_DiffDriveRobot_Part::get_vis_mesh_file()const +{ + if (mesh_name_ == bitrl::consts::INVALID_STR) + { + throw std::runtime_error("The mesh name file is not specified"); + } + + return std::string(consts::ROBOTS_DIR + "/diff_drive_robot/" + mesh_name_ + ".obj"); } // Create Visulization assets void CHRONO_DiffDriveRobot_Part::add_visualization_assets() { - const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); - //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + const std::string vis_mesh_file = get_vis_mesh_file(); + auto trimesh = chrono::ChTriangleMeshConnected::CreateFromWavefrontFile(vis_mesh_file, true, true); - trimesh->Transform(m_offset, chrono::ChMatrix33<>(1)); + trimesh->Transform(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); + trimesh_shape->SetName(mesh_name_); + body_->AddVisualShape(trimesh_shape); return; } void CHRONO_DiffDriveRobot_Part::enable_collision(bool state) { - m_collide = state; + collide_ = state; + body_ -> EnableCollision(state); } // Add collision assets void CHRONO_DiffDriveRobot_Part::add_collision_shapes() { - const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); - //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + //const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + mesh_name_ + ".obj"); + const std::string vis_mesh_file = get_vis_mesh_file(); auto trimesh = chrono::ChTriangleMeshConnected::CreateFromWavefrontFile(vis_mesh_file, false, false); - trimesh->Transform(m_offset, chrono::ChMatrix33<>(1)); + trimesh->Transform(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); + auto shape = chrono_types::make_shared(contact_material_, trimesh, false, false, 0.005); + body_->AddCollisionShape(shape); + body_->EnableCollision(collide_); } } diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h index fd2b5f1..ec4c281 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h @@ -28,65 +28,103 @@ enum class CollisionFamily : int_t TOP_PLATE = 7 ///< top plate }; +/** + * @class CHRONO_DiffDriveRobot_Part + * @ingroup rb_chrono + * @brief Base class for modelling the various parts of the DiffDriveRobot. + * This class is a copy of the implementation provided by Chrono with + * a few additions. + * + * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html + */ + 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; + /// Initialize the chassis at the specified (absolute) position. + virtual void init()=0; + /// Return the name of the part. - const std::string& get_name() const { return m_name; } + const std::string& get_name() const { return name_; } /// Set the name of the part. - void set_name(const std::string& name) { m_name = name; } + void set_name(const std::string& name) { name_ = name; } + + /// Set the mesh file name + void set_mesh_name(const std::string& name) { mesh_name_ = name; } + + /// Get the mesh file name + std::string get_mesh_name() const { return mesh_name_; } + + /// Returns the full path for the mesh visualization. + /// File root is consts::ROBOTS_DIR + std::string get_vis_mesh_file()const; + + /// Return the density of this part + real_t get_density()const{return density_; } + + /// Set the density + void set_density(real_t density) { density_ = density; } /// Return the ChBody of the corresponding Turtlebot part. - std::shared_ptr get_body_ptr() const { return m_body; } + std::shared_ptr get_body_ptr() const { return body_; } /// Return the ChBody of the chassis wrt the Turtlebot part. - std::shared_ptr get_chassis_ptr() const { return m_chassis; } + std::shared_ptr get_chassis_ptr() const { return chassis_; } /// Return the Position of the Turtlebot part. - const chrono::ChVector3d& get_pos() const { return m_body->GetFrameRefToAbs().GetPos(); } + const chrono::ChVector3d& get_pos() const { return body_->GetFrameRefToAbs().GetPos(); } /// Return the Rotation of the Turtlebot part. - const chrono::ChQuaternion<>& get_rotation() const { return m_body->GetFrameRefToAbs().GetRot(); } + const chrono::ChQuaternion<>& get_rotation() const { return body_->GetFrameRefToAbs().GetRot(); } + + /// Enable/disable collision. + void enable_collision(bool state); protected: + + 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); + + // Initialize the body_ Called upon constructing + // an instance of this class + void do_init_(const std::string& mesh_name, const chrono::ChVector3d& offset, + const chrono::ChColor& color, real_t density); + /// 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 + std::shared_ptr body_; ///< rigid body + std::shared_ptr contact_material_; ///< contact material (shared among all shapes) + std::shared_ptr 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::ChVector3d pos_; ///< Turtlebot part's relative position wrt the chassis + chrono::ChVector3d offset_; ///< offset for visualization mesh + chrono::ChColor color_; ///< visualization asset color + chrono::ChSystem* system_; ///< system which Turtlebot Part belongs to - chrono::ChQuaternion<> m_rot; ///< Turtlebot part's relative rotation wrt the chassis + chrono::ChQuaternion<> 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 + std::string name_; ///< subsystem name + std::string mesh_name_; ///< visualization mesh name + real_t density_; ///< Turtlebot part's density - bool m_collide; ///< Turtlebot part's collision indicator - bool m_fixed; ///< Turtlebot part's fixed indication + bool collide_; ///< Turtlebot part's collision indicator + bool fixed_; ///< Turtlebot part's fixed indication + bool is_initialized_{false}; }; } diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp index 87bdc1e..654395d 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp @@ -2,6 +2,7 @@ #ifdef BITRL_CHRONO +#include "bitrl/bitrl_types.h" #include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h" #include "bitrl/bitrl_consts.h" @@ -11,30 +12,38 @@ namespace rb::bitrl_chrono { -CHRONO_DiffDriveRobot_BottomPlate::CHRONO_DiffDriveRobot_BottomPlate(const std::string& name, - bool fixed, +CHRONO_DiffDriveRobot_BottomPlate::CHRONO_DiffDriveRobot_BottomPlate( std::shared_ptr mat, chrono::ChSystem* system, const chrono::ChVector3d& body_pos, const chrono::ChQuaternion<>& body_rot, - std::shared_ptr chassis, - bool collide) + std::shared_ptr chassis) : - CHRONO_DiffDriveRobot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) { - m_mesh_name = "plate_1"; - m_offset = chrono::ChVector3d(0, 0, 0); - m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); - m_density = 20; -} + CHRONO_DiffDriveRobot_Part( "bottom_plate", false, mat, system, body_pos, body_rot, chassis, true) + {} void CHRONO_DiffDriveRobot_BottomPlate::init() { - const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); - //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + + + if (this -> is_initialized_) + { + return; + } + + this -> do_init_("plate_1", chrono::ChVector3d(0, 0, 0), + chrono::ChColor(0.4f, 0.4f, 0.7f), 20.0); + + const std::string vis_mesh_file = get_vis_mesh_file(); + + + std::cout<<"Mesh file: "<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; + real_t mmass; chrono::ChVector3d mcog; chrono::ChMatrix33<> minertia; trimesh->ComputeMassProperties(true, mmass, mcog, minertia); @@ -42,65 +51,80 @@ void CHRONO_DiffDriveRobot_BottomPlate::init() { chrono::ChVector3d principal_I; chrono::ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot); - m_body->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + + std::cout<<"Populating body_: "< body_ == nullptr){ + throw std::logic_error("NULL body_ pointer"); + } + + if(this -> chassis_ == nullptr){ + throw std::logic_error("NULL chassis_ pointer"); + } + + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); // Set inertia - m_body->SetMass(mmass * m_density); - m_body->SetInertiaXX(m_density * principal_I); + body_->SetMass(mmass * density_); + body_->SetInertiaXX(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 + const chrono::ChFrame<>& X_GP = chassis_->GetFrameRefToAbs(); // global -> parent + chrono::ChFrame<> X_PC(pos_, rot_); // parent -> child chrono::ChFrame<> X_GC = X_GP * X_PC; // global -> child - m_body->SetFrameRefToAbs(X_GC); - m_body->SetFixed(m_fixed); + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); + + std::cout<<"Setting collision shapes..."<add_collision_shapes(); - m_body->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::BOTTOM_PLATE)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ROD)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + body_->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::BOTTOM_PLATE)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ROD)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); this->add_visualization_assets(); - m_system->Add(m_body); + system_->Add(body_); + this -> is_initialized_ = true; } -void CHRONO_DiffDriveRobot_BottomPlate::enable_collision(bool state) { - m_collide = state; - m_body->EnableCollision(state); -} void CHRONO_DiffDriveRobot_BottomPlate::translate(const chrono::ChVector3d& shift) { - m_body->SetPos(m_body->GetPos() + shift); + body_->SetPos(body_->GetPos() + shift); } // ========================================================== -CHRONO_DiffDriveRobot_MiddlePlate::CHRONO_DiffDriveRobot_MiddlePlate(const std::string& name, - bool fixed, +CHRONO_DiffDriveRobot_MiddlePlate::CHRONO_DiffDriveRobot_MiddlePlate( std::shared_ptr mat, chrono::ChSystem* system, const chrono::ChVector3d& body_pos, const chrono::ChQuaternion<>& body_rot, - std::shared_ptr chassis, - bool collide) + std::shared_ptr chassis) : - CHRONO_DiffDriveRobot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) { - m_mesh_name = "plate_2"; - m_offset = chrono::ChVector3d(0, 0, 0); - m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); - m_density = 20; + CHRONO_DiffDriveRobot_Part("middle_plate", false, mat, system, body_pos, body_rot, chassis, true) { + // mesh_name_ = "plate_2"; + // offset_ = chrono::ChVector3d(0, 0, 0); + // color_ = chrono::ChColor(0.4f, 0.4f, 0.7f); + // density_ = 20; } void CHRONO_DiffDriveRobot_MiddlePlate::init() { - const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); - //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + if (this -> is_initialized_) + { + return; + } + + this -> do_init_("plate_2", chrono::ChVector3d(0, 0, 0), + chrono::ChColor(0.4f, 0.4f, 0.7f), 20.0); + + const std::string vis_mesh_file = get_vis_mesh_file(); 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; + real_t mmass; chrono::ChVector3d mcog; chrono::ChMatrix33<> minertia; trimesh->ComputeMassProperties(true, mmass, mcog, minertia); @@ -108,64 +132,67 @@ void CHRONO_DiffDriveRobot_MiddlePlate::init() { chrono::ChVector3d principal_I; chrono::ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot); - m_body->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); // Set inertia - m_body->SetMass(mmass * m_density); - m_body->SetInertiaXX(m_density * principal_I); + body_->SetMass(mmass * density_); + body_->SetInertiaXX(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 + const chrono::ChFrame<>& X_GP = chassis_->GetFrameRefToAbs(); // global -> parent + chrono::ChFrame<> X_PC(pos_, rot_); // parent -> child chrono::ChFrame<> X_GC = X_GP * X_PC; // global -> child - m_body->SetFrameRefToAbs(X_GC); - m_body->SetFixed(m_fixed); + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); this->add_collision_shapes(); - m_body->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::MIDDLE_PLATE)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ROD)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + body_->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::MIDDLE_PLATE)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ROD)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); this->add_visualization_assets(); - m_system->Add(m_body); + system_->Add(body_); + this -> is_initialized_ = true; } -void CHRONO_DiffDriveRobot_MiddlePlate::enable_collision(bool state) { - m_collide = state; - m_body->EnableCollision(state); -} void CHRONO_DiffDriveRobot_MiddlePlate::translate(const chrono::ChVector3d& shift) { - m_body->SetPos(m_body->GetPos() + shift); + body_->SetPos(body_->GetPos() + shift); } // ========================================================== -CHRONO_DiffDriveRobot_TopPlate::CHRONO_DiffDriveRobot_TopPlate(const std::string& name, - bool fixed, +CHRONO_DiffDriveRobot_TopPlate::CHRONO_DiffDriveRobot_TopPlate( std::shared_ptr mat, chrono::ChSystem* system, const chrono::ChVector3d& body_pos, const chrono::ChQuaternion<>& body_rot, - std::shared_ptr chassis, - bool collide) + std::shared_ptr chassis) : - CHRONO_DiffDriveRobot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) { - m_mesh_name = "plate_3"; - m_offset = chrono::ChVector3d(0, 0, 0); - m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); - m_density = 20; + CHRONO_DiffDriveRobot_Part("top_plate", false, mat, system, body_pos, body_rot, chassis, true) { + // mesh_name_ = "plate_3"; + // offset_ = chrono::ChVector3d(0, 0, 0); + // color_ = chrono::ChColor(0.4f, 0.4f, 0.7f); + // density_ = 20; } void CHRONO_DiffDriveRobot_TopPlate::init() { - const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); - //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + + if (this -> is_initialized_) + { + return; + } + + this -> do_init_("plate_3", chrono::ChVector3d(0, 0, 0), + chrono::ChColor(0.4f, 0.4f, 0.7f), 20.0); + + const std::string vis_mesh_file = get_vis_mesh_file(); 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; + real_t mmass; chrono::ChVector3d mcog; chrono::ChMatrix33<> minertia; trimesh->ComputeMassProperties(true, mmass, mcog, minertia); @@ -173,39 +200,35 @@ void CHRONO_DiffDriveRobot_TopPlate::init() { chrono::ChVector3d principal_I; chrono::ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot); - m_body->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); // Set inertia - m_body->SetMass(mmass * m_density); - m_body->SetInertiaXX(m_density * principal_I); + body_->SetMass(mmass * density_); + body_->SetInertiaXX(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 + const chrono::ChFrame<>& X_GP = chassis_->GetFrameRefToAbs(); // global -> parent + chrono::ChFrame<> X_PC(pos_, rot_); // parent -> child chrono::ChFrame<> X_GC = X_GP * X_PC; // global -> child - m_body->SetFrameRefToAbs(X_GC); - m_body->SetFixed(m_fixed); + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); this->add_collision_shapes(); - m_body->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::TOP_PLATE)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ROD)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::MIDDLE_PLATE)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::BOTTOM_PLATE)); + body_->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::TOP_PLATE)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ROD)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::MIDDLE_PLATE)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::BOTTOM_PLATE)); this->add_visualization_assets(); - m_system->Add(m_body); -} - -void CHRONO_DiffDriveRobot_TopPlate::enable_collision(bool state) { - m_collide = state; - m_body->EnableCollision(state); + system_->Add(body_); + this -> is_initialized_ = true; } void CHRONO_DiffDriveRobot_TopPlate::translate(const chrono::ChVector3d& shift) { - m_body->SetPos(m_body->GetPos() + shift); + body_->SetPos(body_->GetPos() + shift); } } diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h index 08457cf..ed45053 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h @@ -10,30 +10,33 @@ #include "chrono/physics/ChSystem.h" -#include "diff_drive_robot_part.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" namespace bitrl { namespace rb::bitrl_chrono { -/// Turtlebot Bottom Plate class definition + +/** + * @class CHRONO_DiffDriveRobot_BottomPlate + * @ingroup rb_chrono + * @brief Class for representing the bottom plate of the DiffDriveRobot. + * This class is a copy of the implementation provided by Chrono with + * a few additions. + * + * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html + */ class CHRONO_DiffDriveRobot_BottomPlate : public CHRONO_DiffDriveRobot_Part { public: - CHRONO_DiffDriveRobot_BottomPlate(const std::string& name, - bool fixed, - std::shared_ptr mat, + CHRONO_DiffDriveRobot_BottomPlate(std::shared_ptr mat, chrono::ChSystem* system, const chrono::ChVector3d& body_pos, const chrono::ChQuaternion<>& body_rot, - std::shared_ptr chassis, - bool collide); + std::shared_ptr chassis); virtual ~CHRONO_DiffDriveRobot_BottomPlate()=default; /// Initialize the wheel at the specified (absolute) position. - void init(); - - /// Enable/disable collision for the wheel. - void enable_collision(bool state); + virtual void init() override; private: /// Translate the chassis by the specified value. @@ -41,24 +44,30 @@ class CHRONO_DiffDriveRobot_BottomPlate : public CHRONO_DiffDriveRobot_Part { }; -/// Turtlebot Middle Plate class definition + +/** + * @class CHRONO_DiffDriveRobot_MiddlePlate + * @ingroup rb_chrono + * @brief Class for representing the middle plate of the DiffDriveRobot. + * This class is a copy of the implementation provided by Chrono with + * a few additions. + * + * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html + */ class CHRONO_DiffDriveRobot_MiddlePlate : public CHRONO_DiffDriveRobot_Part { public: - CHRONO_DiffDriveRobot_MiddlePlate(const std::string& name, - bool fixed, + CHRONO_DiffDriveRobot_MiddlePlate( std::shared_ptr mat, chrono::ChSystem* system, const chrono::ChVector3d& body_pos, const chrono::ChQuaternion<>& body_rot, - std::shared_ptr chassis, - bool collide); + std::shared_ptr chassis); + virtual ~CHRONO_DiffDriveRobot_MiddlePlate()=default; /// Initialize the wheel at the specified (absolute) position. - void init(); + virtual void init()override; - /// Enable/disable collision for the wheel. - void enable_collision(bool state); private: /// Translate the chassis by the specified value. @@ -66,24 +75,29 @@ class CHRONO_DiffDriveRobot_MiddlePlate : public CHRONO_DiffDriveRobot_Part { }; -/// Turtlebot Top Plate class definition + +/** + * @class CHRONO_DiffDriveRobot_TopPlate + * @ingroup rb_chrono + * @brief Class for representing the top plate of the DiffDriveRobot. + * This class is a copy of the implementation provided by Chrono with + * a few additions. + * + * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html + */ class CHRONO_DiffDriveRobot_TopPlate : public CHRONO_DiffDriveRobot_Part { public: - CHRONO_DiffDriveRobot_TopPlate(const std::string& name, - bool fixed, - std::shared_ptr mat, + + /// Constructor + CHRONO_DiffDriveRobot_TopPlate(std::shared_ptr mat, chrono::ChSystem* system, const chrono::ChVector3d& body_pos, const chrono::ChQuaternion<>& body_rot, - std::shared_ptr chassis, - bool collide); + std::shared_ptr chassis); virtual ~CHRONO_DiffDriveRobot_TopPlate() {} /// Initialize the wheel at the specified (absolute) position. - void init(); - - /// Enable/disable collision for the wheel. - void enable_collision(bool state); + virtual void init()override; private: /// Translate the chassis by the specified value. diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp index f1fb868..effafc5 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp @@ -11,25 +11,33 @@ namespace rb::bitrl_chrono { CHRONO_DiffDriveRobot_Rod_Short::CHRONO_DiffDriveRobot_Rod_Short(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) + std::shared_ptr chassis) : -CHRONO_DiffDriveRobot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) +CHRONO_DiffDriveRobot_Part(name, false, mat, system, body_pos, body_rot, chassis, true) { - m_mesh_name = "support_rod_short"; - m_offset = chrono::ChVector3d(0, 0, 0); - m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); - m_density = 100; + // mesh_name_ = "support_rod_short"; + // offset_ = chrono::ChVector3d(0, 0, 0); + // color_ = chrono::ChColor(0.4f, 0.4f, 0.7f); + // density_ = 100; } void CHRONO_DiffDriveRobot_Rod_Short::init() { - const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); - //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + + if (this -> is_initialized_) + { + return; + } + + this -> do_init_("support_rod_short", chrono::ChVector3d(0, 0, 0), + chrono::ChColor(0.4f, 0.4f, 0.7f), 100.0); + + const std::string vis_mesh_file = get_vis_mesh_file(); + 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 @@ -42,65 +50,68 @@ void CHRONO_DiffDriveRobot_Rod_Short::init() { chrono::ChVector3d principal_I; chrono::ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot); - m_body->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); // Set inertia - m_body->SetMass(mmass * m_density); - m_body->SetInertiaXX(m_density * principal_I); + body_->SetMass(mmass * density_); + body_->SetInertiaXX(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 + const chrono::ChFrame<>& X_GP = chassis_->GetFrameRefToAbs(); // global -> parent + chrono::ChFrame<> X_PC(pos_, rot_); // parent -> child chrono::ChFrame<> X_GC = X_GP * X_PC; // global -> child - m_body->SetFrameRefToAbs(X_GC); - m_body->SetFixed(m_fixed); + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); this->add_collision_shapes(); - m_body->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::ROD)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::BOTTOM_PLATE)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::MIDDLE_PLATE)); + body_->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::ROD)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::BOTTOM_PLATE)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::MIDDLE_PLATE)); this->add_visualization_assets(); - m_system->Add(m_body); + system_->Add(body_); + this -> is_initialized_ = true; } -void CHRONO_DiffDriveRobot_Rod_Short::enable_collision(bool state) { - m_collide = state; - m_body->EnableCollision(state); -} + void CHRONO_DiffDriveRobot_Rod_Short::translate(const chrono::ChVector3d& shift) { - m_body->SetPos(m_body->GetPos() + shift); + body_->SetPos(body_->GetPos() + shift); } CHRONO_DiffDriveRobot_Rod_Long::CHRONO_DiffDriveRobot_Rod_Long(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) + std::shared_ptr chassis) : - CHRONO_DiffDriveRobot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) + CHRONO_DiffDriveRobot_Part(name, false, mat, system, body_pos, body_rot, chassis, true) { - m_mesh_name = "support_rod_long"; - m_offset = chrono::ChVector3d(0, 0, 0); - m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); - m_density = 100; + // mesh_name_ = "support_rod_long"; + // offset_ = chrono::ChVector3d(0, 0, 0); + // color_ = chrono::ChColor(0.4f, 0.4f, 0.7f); + // density_ = 100; } void CHRONO_DiffDriveRobot_Rod_Long::init() { - const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); - //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); - //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + + if (this -> is_initialized_) + { + return; + } + + this -> do_init_("support_rod_long", chrono::ChVector3d(0, 0, 0), + chrono::ChColor(0.4f, 0.4f, 0.7f), 100.0); + const std::string vis_mesh_file = get_vis_mesh_file(); 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; + real_t mmass; chrono::ChVector3d mcog; chrono::ChMatrix33<> minertia; trimesh->ComputeMassProperties(true, mmass, mcog, minertia); @@ -108,39 +119,35 @@ void CHRONO_DiffDriveRobot_Rod_Long::init() { chrono::ChVector3d principal_I; chrono::ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot); - m_body->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); // Set inertia - m_body->SetMass(mmass * m_density); - m_body->SetInertiaXX(m_density * principal_I); + body_->SetMass(mmass * density_); + body_->SetInertiaXX(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 + const chrono::ChFrame<>& X_GP = chassis_->GetFrameRefToAbs(); // global -> parent + chrono::ChFrame<> X_PC(pos_, rot_); // parent -> child chrono::ChFrame<> X_GC = X_GP * X_PC; // global -> child - m_body->SetFrameRefToAbs(X_GC); - m_body->SetFixed(m_fixed); + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); this->add_collision_shapes(); - m_body->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::ROD)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::BOTTOM_PLATE)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::MIDDLE_PLATE)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ROD)); + body_->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::ROD)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::BOTTOM_PLATE)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::MIDDLE_PLATE)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ROD)); this->add_visualization_assets(); - m_system->Add(m_body); -} - -void CHRONO_DiffDriveRobot_Rod_Long::enable_collision(bool state) { - m_collide = state; - m_body->EnableCollision(state); + system_->Add(body_); + this -> is_initialized_ = true; } void CHRONO_DiffDriveRobot_Rod_Long::translate(const chrono::ChVector3d& shift) { - m_body->SetPos(m_body->GetPos() + shift); + body_->SetPos(body_->GetPos() + shift); } diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h index b5b9540..dedf3ae 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h @@ -10,31 +10,35 @@ #include "chrono/physics/ChSystem.h" -#include "diff_drive_robot_part.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" namespace bitrl { namespace rb::bitrl_chrono { -/// Short Supporting Rod class definition +/** + * @class CHRONO_DiffDriveRobot_Rod_Short + * @ingroup rb_chrono + * @brief Class for representing the short rods of the DiffDriveRobot. + * This class is a copy of the implementation provided by Chrono with + * a few additions. + * + * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html + */ class CHRONO_DiffDriveRobot_Rod_Short : public CHRONO_DiffDriveRobot_Part { public: CHRONO_DiffDriveRobot_Rod_Short(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); + std::shared_ptr chassis); + ~CHRONO_DiffDriveRobot_Rod_Short()=default; /// Initialize the wheel at the specified (absolute) position. - void init(); - - /// Enable/disable collision for the wheel. - void enable_collision(bool state); + virtual void init()override; private: /// Translate the chassis by the specified value. @@ -42,25 +46,30 @@ class CHRONO_DiffDriveRobot_Rod_Short : public CHRONO_DiffDriveRobot_Part { }; - -/// Long Supporting Rod class definition +/** + * @class CHRONO_DiffDriveRobot_Rod_Long + * @ingroup rb_chrono + * @brief Class for representing the long rods of the DiffDriveRobot. + * This class is a copy of the implementation provided by Chrono with + * a few additions. + * + * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html + */ class CHRONO_DiffDriveRobot_Rod_Long : public CHRONO_DiffDriveRobot_Part { public: CHRONO_DiffDriveRobot_Rod_Long(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); + std::shared_ptr chassis); + virtual ~CHRONO_DiffDriveRobot_Rod_Long()=default; /// Initialize the wheel at the specified (absolute) position. - void init(); + virtual void init() override; + - /// Enable/disable collision for the wheel. - void enable_collision(bool state); private: /// Translate the chassis by the specified value. diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp index 949ebd0..8738eca 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp @@ -15,31 +15,37 @@ 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) + std::shared_ptr chassis) : - CHRONO_DiffDriveRobot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) + CHRONO_DiffDriveRobot_Part(name, true, mat, system, body_pos, body_rot, chassis, false) { - 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; + // mesh_name_ = "active_wheel"; + // offset_ = chrono::ChVector3d(0, 0, 0); + // color_ = chrono::ChColor(0.4f, 0.4f, 0.7f); + // density_ = 200; } void CHRONO_DiffDriveRobot_ActiveWheel::init() { - const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); - //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); - //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + + + if (this -> is_initialized_) + { + return; + } + + this -> do_init_("active_wheel", chrono::ChVector3d(0, 0, 0), + chrono::ChColor(0.4f, 0.4f, 0.7f), 200.0); + + const std::string vis_mesh_file = get_vis_mesh_file(); 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; + real_t mmass; chrono::ChVector3d mcog; chrono::ChMatrix33<> minertia; trimesh->ComputeMassProperties(true, mmass, mcog, minertia); @@ -47,63 +53,68 @@ void CHRONO_DiffDriveRobot_ActiveWheel::init() { chrono::ChVector3d principal_I; chrono::ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot); - m_body->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); // Set inertia - m_body->SetMass(mmass * m_density); - m_body->SetInertiaXX(m_density * principal_I); + body_->SetMass(mmass * density_); + body_->SetInertiaXX(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 + const chrono::ChFrame<>& X_GP = chassis_->GetFrameRefToAbs(); // global -> parent + chrono::ChFrame<> X_PC(pos_, rot_); // parent -> child chrono::ChFrame<> X_GC = X_GP * X_PC; // global -> child - m_body->SetFrameRefToAbs(X_GC); - m_body->SetFixed(m_fixed); + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); this -> add_collision_shapes(); - m_body->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::ACTIVE_WHEEL)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + body_->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::ACTIVE_WHEEL)); + 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); + system_->Add(body_); + this -> is_initialized_ = true; } void CHRONO_DiffDriveRobot_ActiveWheel::translate(const chrono::ChVector3d& shift) { - m_body->SetPos(m_body->GetPos() + shift); + body_->SetPos(body_->GetPos() + shift); } CHRONO_DiffDriveRobot_PassiveWheel::CHRONO_DiffDriveRobot_PassiveWheel(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) + std::shared_ptr chassis + ) : - CHRONO_DiffDriveRobot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) { - m_mesh_name = "passive_wheel"; - m_offset = chrono::ChVector3d(0, 0, 0); - m_color = chrono::ChColor(0.4f, 0.4f, 0.7f); - m_density = 200; + CHRONO_DiffDriveRobot_Part(name, false, mat, system, body_pos, body_rot, chassis, true) { + // mesh_name_ = "passive_wheel"; + // offset_ = chrono::ChVector3d(0, 0, 0); + // color_ = chrono::ChColor(0.4f, 0.4f, 0.7f); + // density_ = 200; } void CHRONO_DiffDriveRobot_PassiveWheel::init() { - const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + m_mesh_name + ".obj"); - //auto vis_mesh_file = chrono::GetChronoDataFile("robot/turtlebot/" + m_mesh_name + ".obj"); + + if (this -> is_initialized_) + { + return; + } + + this -> do_init_("passive_wheel", chrono::ChVector3d(0, 0, 0), + chrono::ChColor(0.4f, 0.4f, 0.7f), 200.0); + + //const std::string vis_mesh_file(consts::ROBOTS_DIR + "/diff_drive_robot/" + mesh_name_ + ".obj"); + const std::string vis_mesh_file = get_vis_mesh_file(); + 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; + real_t mmass; chrono::ChVector3d mcog; chrono::ChMatrix33<> minertia; trimesh->ComputeMassProperties(true, mmass, mcog, minertia); @@ -111,36 +122,32 @@ void CHRONO_DiffDriveRobot_PassiveWheel::init() { chrono::ChVector3d principal_I; chrono::ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot); - m_body->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); // Set inertia - m_body->SetMass(mmass * m_density); - m_body->SetInertiaXX(m_density * principal_I); + body_->SetMass(mmass * density_); + body_->SetInertiaXX(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 + const chrono::ChFrame<>& X_GP = chassis_->GetFrameRefToAbs(); // global -> parent + chrono::ChFrame<> X_PC(pos_, rot_); // parent -> child chrono::ChFrame<> X_GC = X_GP * X_PC; // global -> child - m_body->SetFrameRefToAbs(X_GC); - m_body->SetFixed(m_fixed); + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); this -> add_collision_shapes(); - m_body->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::PASSIVE_WHEEL)); - m_body->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + body_->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::PASSIVE_WHEEL)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); this -> add_visualization_assets(); - m_system->Add(m_body); -} - -void CHRONO_DiffDriveRobot_PassiveWheel::enable_collision(bool state) { - m_collide = state; - m_body->EnableCollision(state); + system_->Add(body_); + this -> is_initialized_ = true; } void CHRONO_DiffDriveRobot_PassiveWheel::translate(const chrono::ChVector3d& shift) { - m_body->SetPos(m_body->GetPos() + shift); + body_->SetPos(body_->GetPos() + shift); } } diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h index a99d0d5..d2399b6 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h @@ -8,54 +8,64 @@ #include #include -#include "diff_drive_robot_part.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" namespace bitrl{ namespace rb::bitrl_chrono { +/** + * @class CHRONO_DiffDriveRobot_ActiveWheel + * @ingroup rb_chrono + * @brief Class for representing motorised wheel of the DiffDriveRobot. + * This class is a copy of the implementation provided by Chrono with + * a few additions. + * + * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html + */ 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); + std::shared_ptr chassis + ); // 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); + virtual void init() override; private: /// Translate the chassis by the specified value. void translate(const chrono::ChVector3d& shift); }; -/// Turtlebot Passive Driven Wheel class definition +/** + * @class CHRONO_DiffDriveRobot_PassiveWheel + * @ingroup rb_chrono + * @brief Class for representing a passive wheel of the DiffDriveRobot. + * This class is a copy of the implementation provided by Chrono with + * a few additions. + * + * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html + */ class CHRONO_DiffDriveRobot_PassiveWheel : public CHRONO_DiffDriveRobot_Part { public: CHRONO_DiffDriveRobot_PassiveWheel(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); + std::shared_ptr chassis + ); virtual ~CHRONO_DiffDriveRobot_PassiveWheel()=default; /// Initialize the wheel at the specified (absolute) position. - void init(); - - /// Enable/disable collision for the wheel. - void enable_collision(bool state); + virtual void init() override; private: /// Translate the chassis by the specified value.