diff --git a/CMakeLists.txt b/CMakeLists.txt index 12f79c0..0fb4bea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -163,6 +163,8 @@ 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/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 6b218da..c126c5d 100644 --- a/examples/example_13/example_13.cpp +++ b/examples/example_13/example_13.cpp @@ -10,14 +10,14 @@ #include #endif +#include "bitrl/bitrl_consts.h" #include "chrono/core/ChRealtimeStep.h" #include "chrono/physics/ChSystemNSC.h" -#include "chrono/physics/ChLinkMotorRotationSpeed.h" +#include #include +#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" #include -#include -#include #include namespace example_13 @@ -45,59 +45,34 @@ void prepare_visualization(chrono::irrlicht::ChVisualSystemIrrlicht& visual) visual.BindAll(); } + + + } // 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); + + 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); @@ -123,27 +98,9 @@ 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 realtime_timer.Spin(DT); @@ -151,7 +108,6 @@ int main() visual.EndScene(); } - return 0; } #else diff --git a/examples/example_13/example_13.md b/examples/example_13/example_13.md index eb3308c..057dd06 100644 --- a/examples/example_13/example_13.md +++ b/examples/example_13/example_13.md @@ -10,9 +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 +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" +#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 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: { + 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 add_revolute_joint(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 add_revolute_joint(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 add_lock_joint(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 add_motor(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) + : + 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 = system_->GetContactMethod(); + if (contact_method == chrono::ChContactMethod::NSC) { + chrono::ChCollisionModel::SetDefaultSuggestedEnvelope(0.01); + chrono::ChCollisionModel::SetDefaultSuggestedMargin(0.005); + } + + // Create the contact materials + chassis_material_ = build_default_contact_material(contact_method); + if (!wheel_material_) + wheel_material_ = build_default_contact_material(contact_method); + +} + + + +void CHRONO_DiffDriveRobot::init() { + // initialize robot chassis + 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 + 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 + 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 + 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 + real_t bt_plate_x = 0; + real_t bt_plate_y = 0; + real_t bt_plate_z = 0.14615; + + 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 + 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 + 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 + 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 + 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), chassis_->get_body_ptr()); + + top_plate_ -> init(); + + this -> build_motors_(); +} + +/// Initialize the complete rover and add all constraints +void CHRONO_DiffDriveRobot::build_motors_() { + + // redeclare necessary location variables + 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); + 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); + + 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)); + add_revolute_joint(passive_wheels_[0]->get_body_ptr(), chassis_->get_body_ptr(), chassis_->get_body_ptr(), system_, + chrono::ChVector3d(pwx, pwy, pwz), z2x); + + 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)); + 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 + // 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_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); + + 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)); + 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)); + 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)); + 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)); + 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)); + 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) { + motors_func_[id]->SetConstant(rad_speed); +} + +chrono::ChVector3d CHRONO_DiffDriveRobot::get_active_wheel_speed(uint_t id)const { + return drive_wheels_[id]->get_body_ptr()->GetPosDt(); +} + +chrono::ChVector3d CHRONO_DiffDriveRobot::get_active_wheel_angular_velocity(uint_t id)const { + return 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..5cd698c --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h @@ -0,0 +1,117 @@ + +// ============================================================================= +// 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 "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" + +#include "chrono/physics/ChSystem.h" +#include "chrono/physics/ChLinkMotorRotationSpeed.h" +#include + + +namespace bitrl +{ +namespace rb::bitrl_chrono{ + + +/** + * @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 final { +public: + CHRONO_DiffDriveRobot(chrono::ChSystem& system, + const chrono::ChVector3d& robot_pos, + const chrono::ChQuaternion<>& robot_rot, + std::shared_ptr wheel_mat = {}); + + + /// 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 + 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; + + /// Get the pose of the robot + const CHRONO_RobotPose& get_pose() const{return pose_;} + +private: + + /// Build motors + void build_motors_(); + + /// Pointer to the Chrono system + chrono::ChSystem* system_; + + /// 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 + + 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 + + chrono::ChQuaternion<> robot_rot_; ///< init robot rotation + chrono::ChVector3d robot_pos_; ///< init robot translation position + + std::vector> motors_; ///< vector to store motors + std::vector> motors_func_; ///< constant motor angular speed func + + // model parts material + 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; +}; + + +} // namespace chrono +} +#endif +#endif + 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 new file mode 100644 index 0000000..ca2324d --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.cpp @@ -0,0 +1,74 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + + +#include "chrono/assets/ChColor.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" + +namespace bitrl{ +namespace rb::bitrl_chrono +{ + +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("chassis", false, mat, system, body_pos, body_rot, nullptr, true) +{} + +void CHRONO_DiffDriveRobot_Chassis::init() { + + + if (this -> 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 + + real_t 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); + + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + + // Set inertia + body_->SetMass(mmass * density_); + body_->SetInertiaXX(density_ * principal_I); + body_->SetFrameRefToAbs(chrono::ChFrame<>(pos_, rot_)); + body_->SetFixed(fixed_); + + this -> add_collision_shapes(); + + body_->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::CHASSIS)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::ACTIVE_WHEEL)); + + this -> add_visualization_assets(); + system_->Add(body_); + this -> is_initialized_ = true; +} + +void CHRONO_DiffDriveRobot_Chassis::translate(const chrono::ChVector3d& shift) { + body_->SetPos(body_->GetPos() + shift); +} + + +} +} + +#endif 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 new file mode 100644 index 0000000..1f36a2a --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h @@ -0,0 +1,61 @@ +// +// 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 "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: + + // constructor + CHRONO_DiffDriveRobot_Chassis( + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot + ); + + ~CHRONO_DiffDriveRobot_Chassis()=default; + + /// Initialize the chassis + virtual void init() override; + + +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/turtle_bot/diff_drive_robot_part.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp new file mode 100644 index 0000000..fd87d75 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp @@ -0,0 +1,99 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" +#include "bitrl/bitrl_consts.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) + : +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 = get_vis_mesh_file(); + + auto trimesh = chrono::ChTriangleMeshConnected::CreateFromWavefrontFile(vis_mesh_file, true, true); + trimesh->Transform(offset_, chrono::ChMatrix33<>(1)); + auto trimesh_shape = chrono_types::make_shared(); + trimesh_shape->SetMesh(trimesh); + trimesh_shape->SetName(mesh_name_); + body_->AddVisualShape(trimesh_shape); + return; +} + +void CHRONO_DiffDriveRobot_Part::enable_collision(bool 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/" + 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(offset_, chrono::ChMatrix33<>(1)); + + auto shape = chrono_types::make_shared(contact_material_, trimesh, false, false, 0.005); + body_->AddCollisionShape(shape); + body_->EnableCollision(collide_); +} + +} +} +#endif \ No newline at end of file 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 new file mode 100644 index 0000000..ec4c281 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h @@ -0,0 +1,136 @@ + +#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 + * @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: + + + /// 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 name_; } + + /// Set the name of the part. + 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 body_; } + + /// Return the ChBody of the chassis wrt the Turtlebot part. + std::shared_ptr get_chassis_ptr() const { return chassis_; } + + /// Return the Position of the Turtlebot part. + const chrono::ChVector3d& get_pos() const { return body_->GetFrameRefToAbs().GetPos(); } + + /// Return the Rotation of the Turtlebot part. + 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(); + + 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 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<> rot_; ///< Turtlebot part's relative rotation wrt the chassis + + std::string name_; ///< subsystem name + std::string mesh_name_; ///< visualization mesh name + real_t density_; ///< Turtlebot part's density + + bool collide_; ///< Turtlebot part's collision indicator + bool fixed_; ///< Turtlebot part's fixed indication + bool is_initialized_{false}; +}; + +} +} + + + +#endif +#endif //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..654395d --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp @@ -0,0 +1,237 @@ +#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_plates.h" +#include "bitrl/bitrl_consts.h" + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ + + +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) + : + CHRONO_DiffDriveRobot_Part( "bottom_plate", false, mat, system, body_pos, body_rot, chassis, true) + {} + +void CHRONO_DiffDriveRobot_BottomPlate::init() { + + + 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 + + real_t 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); + + + 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 + body_->SetMass(mmass * density_); + body_->SetInertiaXX(density_ * principal_I); + + // set relative position to chassis + 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 + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); + + std::cout<<"Setting collision shapes..."<add_collision_shapes(); + + 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(); + + system_->Add(body_); + this -> is_initialized_ = true; +} + + +void CHRONO_DiffDriveRobot_BottomPlate::translate(const chrono::ChVector3d& shift) { + body_->SetPos(body_->GetPos() + shift); +} + +// ========================================================== +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) + : + 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() { + + 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 + + real_t 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); + + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + + // Set inertia + body_->SetMass(mmass * density_); + body_->SetInertiaXX(density_ * principal_I); + + // set relative position to chassis + 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 + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); + + this->add_collision_shapes(); + + 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(); + + system_->Add(body_); + this -> is_initialized_ = true; +} + + +void CHRONO_DiffDriveRobot_MiddlePlate::translate(const chrono::ChVector3d& shift) { + body_->SetPos(body_->GetPos() + shift); +} + +// ========================================================== +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) + : + 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() { + + 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 + + real_t 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); + + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + + // Set inertia + body_->SetMass(mmass * density_); + body_->SetInertiaXX(density_ * principal_I); + + // set relative position to chassis + 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 + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); + + this->add_collision_shapes(); + + 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(); + + system_->Add(body_); + this -> is_initialized_ = true; +} + +void CHRONO_DiffDriveRobot_TopPlate::translate(const chrono::ChVector3d& shift) { + body_->SetPos(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..ed45053 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h @@ -0,0 +1,112 @@ +#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 "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ + +/** + * @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(std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis); + virtual ~CHRONO_DiffDriveRobot_BottomPlate()=default; + + /// Initialize the wheel at the specified (absolute) position. + virtual void init() override; + + private: + /// Translate the chassis by the specified value. + void translate(const chrono::ChVector3d& shift); + +}; + + +/** + * @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( + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis); + + virtual ~CHRONO_DiffDriveRobot_MiddlePlate()=default; + + /// Initialize the wheel at the specified (absolute) position. + virtual void init()override; + + + private: + /// Translate the chassis by the specified value. + void translate(const chrono::ChVector3d& shift); + +}; + + +/** + * @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: + + /// 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); + virtual ~CHRONO_DiffDriveRobot_TopPlate() {} + + /// Initialize the wheel at the specified (absolute) position. + virtual void init()override; + + 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..effafc5 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp @@ -0,0 +1,156 @@ +#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, + + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis) + : +CHRONO_DiffDriveRobot_Part(name, false, mat, system, body_pos, body_rot, chassis, true) +{ + // 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() { + + 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 + + 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); + + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + + // Set inertia + body_->SetMass(mmass * density_); + body_->SetInertiaXX(density_ * principal_I); + + // set relative position to chassis + 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 + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); + + this->add_collision_shapes(); + + 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(); + system_->Add(body_); + this -> is_initialized_ = true; +} + + + +void CHRONO_DiffDriveRobot_Rod_Short::translate(const chrono::ChVector3d& shift) { + body_->SetPos(body_->GetPos() + shift); +} + +CHRONO_DiffDriveRobot_Rod_Long::CHRONO_DiffDriveRobot_Rod_Long(const std::string& name, + + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis) + : + CHRONO_DiffDriveRobot_Part(name, false, mat, system, body_pos, body_rot, chassis, true) +{ + // 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() { + + 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 + + real_t 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); + + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + + // Set inertia + body_->SetMass(mmass * density_); + body_->SetInertiaXX(density_ * principal_I); + + // set relative position to chassis + 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 + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); + + this->add_collision_shapes(); + + 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(); + + system_->Add(body_); + this -> is_initialized_ = true; +} + +void CHRONO_DiffDriveRobot_Rod_Long::translate(const chrono::ChVector3d& shift) { + body_->SetPos(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..dedf3ae --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h @@ -0,0 +1,81 @@ +#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 "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ + +/** + * @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, + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis); + + ~CHRONO_DiffDriveRobot_Rod_Short()=default; + + /// Initialize the wheel at the specified (absolute) position. + virtual void init()override; + +private: + /// Translate the chassis by the specified value. + void translate(const chrono::ChVector3d& shift); + +}; + +/** + * @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, + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis); + + virtual ~CHRONO_DiffDriveRobot_Rod_Long()=default; + + /// Initialize the wheel at the specified (absolute) position. + virtual void init() override; + + + +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..8738eca --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp @@ -0,0 +1,155 @@ +#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, + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis) + : + CHRONO_DiffDriveRobot_Part(name, true, mat, system, body_pos, body_rot, chassis, false) +{ + // 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() { + + + 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 + + real_t 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); + + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + + // Set inertia + body_->SetMass(mmass * density_); + body_->SetInertiaXX(density_ * principal_I); + + // set relative position to chassis + 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 + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); + + this -> add_collision_shapes(); + + body_->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::ACTIVE_WHEEL)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + + this -> add_visualization_assets(); + + system_->Add(body_); + this -> is_initialized_ = true; +} + +void CHRONO_DiffDriveRobot_ActiveWheel::translate(const chrono::ChVector3d& shift) { + body_->SetPos(body_->GetPos() + shift); +} + + +CHRONO_DiffDriveRobot_PassiveWheel::CHRONO_DiffDriveRobot_PassiveWheel(const std::string& name, + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis + ) + : + 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() { + + 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 + + real_t 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); + + body_->SetFrameCOMToRef(chrono::ChFrame<>(mcog, principal_inertia_rot)); + + // Set inertia + body_->SetMass(mmass * density_); + body_->SetInertiaXX(density_ * principal_I); + + // set relative position to chassis + 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 + body_->SetFrameRefToAbs(X_GC); + body_->SetFixed(fixed_); + + this -> add_collision_shapes(); + + body_->GetCollisionModel()->SetFamily(static_cast(CollisionFamily::PASSIVE_WHEEL)); + body_->GetCollisionModel()->DisallowCollisionsWith(static_cast(CollisionFamily::CHASSIS)); + + this -> add_visualization_assets(); + + system_->Add(body_); + this -> is_initialized_ = true; +} + +void CHRONO_DiffDriveRobot_PassiveWheel::translate(const chrono::ChVector3d& shift) { + body_->SetPos(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_wheels.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h new file mode 100644 index 0000000..d2399b6 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h @@ -0,0 +1,80 @@ +#ifndef DIFF_DRIVE_ROBOT_WHEELS_H +#define DIFF_DRIVE_ROBOT_WHEELS_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include +#include + +#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, + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis + ); + + // destructor + virtual ~CHRONO_DiffDriveRobot_ActiveWheel()=default; + + /// Initialize the wheel at the specified (absolute) position. + virtual void init() override; + +private: + /// Translate the chassis by the specified value. + void translate(const chrono::ChVector3d& shift); +}; + +/** + * @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, + + std::shared_ptr mat, + chrono::ChSystem* system, + const chrono::ChVector3d& body_pos, + const chrono::ChQuaternion<>& body_rot, + std::shared_ptr chassis + ); + virtual ~CHRONO_DiffDriveRobot_PassiveWheel()=default; + + /// Initialize the wheel at the specified (absolute) position. + virtual void init() override; + +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/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/chrono_diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.cpp similarity index 100% rename from src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp rename to src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.cpp diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.h similarity index 100% rename from src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h rename to src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_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