diff --git a/examples/example_14/example_14.cpp b/examples/example_14/example_14.cpp index 7247248..8fcb9d8 100644 --- a/examples/example_14/example_14.cpp +++ b/examples/example_14/example_14.cpp @@ -1,18 +1,16 @@ #include "bitrl/bitrl_config.h" -#ifdef BITRL_CHRONO +#if defined(BITRL_CHRONO) && defined(BITRL_IRRLICHT) #include "bitrl/bitrl_consts.h" #include "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" -#include "bitrl/envs/env_base.h" -#include "bitrl/envs/time_step.h" -#include "bitrl/envs/time_step_type.h" -#include "bitrl/envs/env_types.h" #include "bitrl/utils/bitrl_utils.h" -#include -#include +#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" +#include "bitrl/utils/render/irrlicht_utils.h" +#include +//#include #include #ifdef BITRL_LOG @@ -23,346 +21,95 @@ #include #include #include +#include namespace example14 { 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.0001; -const real_t SIM_TIME = 5.0; -const std::string WINDOW_TITLE( "Example 14"); - -constexpr uint_t STATE_SPACE_SIZE = 2; -constexpr uint_t ACTION_SPACE_SIZE = 1; - -using bitrl::TimeStepTp; -using bitrl::TimeStep; - -//typedef TimeStep > time_step_type; -typedef TimeStep time_step_type; -typedef bitrl::envs::ContinuousVectorStateContinuousVectorActionEnv space_type; - -std::shared_ptr build_wheel(const std::string &wheel_label, real_t radius, - real_t width, const chrono::ChVector3d& pos); - -void draw_world_axes(chrono::irrlicht::ChVisualSystemIrrlicht& vis, - double scale = 1.0) { - auto* driver = vis.GetVideoDriver(); - - // X axis (red) - driver->draw3DLine( - {0, 0, 0}, - {static_cast(scale), 0, 0}, - irr::video::SColor(255, 255, 0, 0)); - - // Y axis (green) - driver->draw3DLine( - {0, 0, 0}, - {0, static_cast(scale), 0}, - irr::video::SColor(255, 0, 255, 0)); - - // Z axis (blue) - driver->draw3DLine( - {0, 0, 0}, - {0, 0, static_cast(scale)}, - irr::video::SColor(255, 0, 0, 255)); -} - -// class to model the robot -class DiffDriveRobot -{ -public: - - // add the components of this robot to the system we simulate - void add_to_sys(chrono::ChSystemSMC& sys); - - // build the robot - void build(); - - // set the speeds of the motors - void set_speed(real_t speed); - - // reset the robot - void reset(); - - // the pose of the robot - bitrl::rb::bitrl_chrono::CHRONO_RobotPose& pose()noexcept{return pose_;} - -private: - - //The chassis of the robot - std::shared_ptr chassis_; - std::pair, std::shared_ptr> wheels_; - std::shared_ptr caster_wheel_; - bitrl::rb::bitrl_chrono::CHRONO_RobotPose pose_; - -}; - -void DiffDriveRobot::build() -{ - // build the chassis of the robot - chassis_ = chrono_types::make_shared(); - chassis_->SetMass(1.0); - chassis_->SetPos(chrono::ChVector3d(0.0, 0.0, 0.22)); +using bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot; - // allow the chassis to move - chassis_->SetFixed(false); - - // add visual shape for visualization - auto vis_shape = chrono_types::make_shared( - chrono::ChVector3d(0.4, 0.3, 0.05)); - chassis_ -> AddVisualShape(vis_shape); - - // build the wheels of the robot - wheels_.first = build_wheel("left_wheel", 0.06, 0.05, chrono::ChVector3d(0.0, 0.175, 0.16)); - wheels_.second = build_wheel("right_wheel", 0.06, 0.05, chrono::ChVector3d(0.0, -0.175, 0.16)); - caster_wheel_ = build_wheel("caster_wheel", 0.06, 0.05, chrono::ChVector3d(0.2, 0.0, 0.16)); - - // we want to tract the chassis pose - pose_.set_body(chassis_); - -} - -void -DiffDriveRobot::reset() -{ - chassis_ -> SetPos(chrono::ChVector3d(0.0, 0.0, 0.22)); - wheels_.first -> SetPos(chrono::ChVector3d(0.0, 0.175, 0.16)); - wheels_.second -> SetPos(chrono::ChVector3d(0.0, -0.175, 0.16)); - caster_wheel_ -> SetPos(chrono::ChVector3d(0.2, 0.0, 0.16)); -} - -std::shared_ptr build_wheel(const std::string &wheel_label, real_t radius, - real_t width, const chrono::ChVector3d& pos) -{ - - // 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(1.0); - wheel->SetPos(pos); - wheel->SetName(wheel_label); - wheel->SetRot(chrono::QUNIT); - wheel->SetFixed(false); - - auto visual_shape = chrono_types::make_shared(radius, width); - - chrono::ChQuaterniond qvis; - qvis.SetFromAngleAxis(chrono::CH_PI_2, chrono::VECT_X); - - chrono::ChFrame<> vis_frame(chrono::VNULL, qvis); - - wheel->AddVisualShape(visual_shape, vis_frame); - return wheel; -} - -void DiffDriveRobot::add_to_sys(chrono::ChSystemSMC& sys) +auto build_generic_material() { - sys.Add(chassis_); - sys.Add(wheels_.first); - sys.Add(wheels_.second); - sys.Add(caster_wheel_); - -} - -void DiffDriveRobot::set_speed(real_t speed) -{ - chassis_ -> SetAngVelLocal(chrono::VNULL); - chassis_ -> SetAngAccLocal(chrono::VNULL); - chassis_ -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); - wheels_.first -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); - wheels_.second -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); - caster_wheel_ -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); - + auto material = chrono_types::make_shared(); + material->SetFriction(0.8f); + material->SetRestitution(0.1f); + return material; } -class DiffDriveRobotEnv final: public bitrl::envs::EnvBase -{ -public: - - - typedef typename space_type::action_type action_type; - - DiffDriveRobotEnv(); - - virtual void make(const std::string &version, - const std::unordered_map &make_options, - const std::unordered_map &reset_options) override; - - virtual void close()override{} - - virtual time_step_type reset()override; - virtual time_step_type step(const action_type &/*action*/)override; - - void simulate(); - -private: - - DiffDriveRobot robot_; - chrono::ChSystemSMC sys_; - uint_t sim_counter_{0}; - real_t current_time_{0.0}; - void build_system_(); - -}; - -DiffDriveRobotEnv::DiffDriveRobotEnv() - : -bitrl::envs::EnvBase(bitrl::utils::uuid4(), "DiffDriveRobotEnv"), -robot_(), -sys_() -{} - -void DiffDriveRobotEnv::build_system_() +auto build_floor(std::shared_ptr material) { - // create material for the ground - 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.2, - 1000, - true, // visual - true, // collision - material - ); - ground->SetFixed(true); - ground->SetPos({0, 0, -0.1}); - - // set the gravity acceleration - sys_.SetGravitationalAcceleration(chrono::ChVector3d(0, 0.0, -bitrl::consts::maths::G)); - sys_.Add(ground); + auto floor = chrono_types::make_shared( + 5, 5, 0.1, // size + 1000, // density + true, true, material); // visual + collision + + floor->SetPos(chrono::ChVector3d(0,0,-0.05)); + floor->SetFixed(true); + floor->EnableCollision(true); + return floor; } -void -DiffDriveRobotEnv::make(const std::string &version, - const std::unordered_map &make_options, - const std::unordered_map &reset_options) -{ - - robot_.build(); - build_system_(); - - robot_.add_to_sys(sys_); - - this -> set_make_options_(make_options); - this -> set_reset_options_(reset_options); - this -> set_version_(version); - this -> make_created_(); - } -time_step_type -DiffDriveRobotEnv::reset() +int main() { - sim_counter_ = 1; - current_time_ = 0.0; - robot_.reset(); - robot_.set_speed(15.0); - - auto robot_position =robot_.pose().position(); - return time_step_type(TimeStepTp::FIRST, 0.0, robot_position); -} + using namespace example14; -time_step_type -DiffDriveRobotEnv::step(const action_type &/*action*/) -{ - if (sim_counter_ % 100 == 0) - { #ifdef BITRL_LOG - BOOST_LOG_TRIVIAL(info)<<"Reset simulation: "; + BOOST_LOG_TRIVIAL(info)<<"Starting simulation...: "; #endif - return reset(); - } - - sys_.DoStepDynamics(DT); - auto robot_position =robot_.pose().position(); - - sim_counter_++; - current_time_ += DT; - return time_step_type(TimeStepTp::MID, 1.0, robot_position); -} - - - -void DiffDriveRobotEnv::simulate() -{ - // create the object that handles the visualization - chrono::irrlicht::ChVisualSystemIrrlicht visual; - visual.AttachSystem(&sys_); - visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); //WINDOW_HEIGHT); - visual.SetWindowTitle(WINDOW_TITLE); - visual.Initialize(); - draw_world_axes(visual); - visual.AddLogo(); - visual.AddSkyBox(); - visual.AddCamera({0, -2, 1}, {0, 0, 0}); - visual.AddTypicalLights(); - visual.BindAll(); - - // we need this - while (visual.Run()) - { - - // Irrlicht must prepare frame to draw - visual.BeginScene(); - // .. draw items belonging to Irrlicht scene, if any - visual.Render(); + // create the system and attach the robot to it + chrono::ChSystemNSC system; + system.SetGravitationalAcceleration(chrono::ChVector3d(0,0,-bitrl::consts::maths::G)); + system.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET); - // .. draw a grid - tools::drawGrid(&visual, 0.5, 0.5); - draw_world_axes(visual, 1.5); + auto floor = build_floor(build_generic_material()); + system.Add(floor); - auto time_step = step( action_type()); #ifdef BITRL_LOG - BOOST_LOG_TRIVIAL(info)<<"At time: "< GetPos(); + //std::cout << position <(), - std::unordered_map()); - env.reset(); - - env.simulate(); - env.close(); return 0; } diff --git a/robots/bitrl_diff_drive_robot.json b/robots/bitrl_diff_drive_robot.json index cd32367..7c4dfa2 100644 --- a/robots/bitrl_diff_drive_robot.json +++ b/robots/bitrl_diff_drive_robot.json @@ -1,45 +1,84 @@ { "name": "Odysseus", - "mass": 1.0, - "mass_units": "kg", + "robot-data": { + "max_speed": 5.0, + "wheel_radius": 0.1, + "wheel_width": 0.05, + "axle_length": 1.0, + "density": 1000.0, + "caster_wheel_radius": 0.05 + }, + "material": { + "friction": 0.8, + "restitution": 0.1 + }, "chassis": { - "mass": 1.0, - "mass_units": "kg", - "position": [ 0.0, 0.0, 0.0], - "fixed": false, - "visual_shape": "shape-box", - "visual_position": [0.4, 0.3, 0.05] + "description": "chassis_i describes the length of the chassis in the x, y, z axis, pos_i the center of mass of the chassis", + "chassis_x": 0.5, + "chassis_y": 0.3, + "chassis_z": 0.1, + "pos_x": 0.0, + "pos_y": 0.0, + "pos_z": 0.0 }, "left-wheel": { - "mass": 1.0, - "mass_units": "kg", - "position": [ 0.0, 0.175, -0.02], - "radius": 0.06, - "width": 0.05, - "radius_units": "cm", - "visual_shape": "cylinder", - "rotation_axis": [1, 0, 0], - "rotation_angle_deg": 90 + "pos_x": 0.0, + "pos_y": 0.2, + "pos_z": 0.1 }, "right-wheel": { - "mass": 1.0, - "mass_units": "kg", - "position": [ 0.0, -0.175, -0.02], - "radius": 0.06, - "width": 0.05, - "radius_units": "cm", - "visual_shape": "cylinder", - "rotation_axis": [1, 0, 0], - "rotation_angle_deg": 90 + "pos_x": 0.0, + "pos_y": -0.2, + "pos_z": 0.1 }, "caster-wheel": { - "mass": 0.5, - "mass_units": "kg", - "position": [ -0.2, 0.0, -0.02], - "radius": 0.06, - "width": 0.05, - "visual_shape": "cylinder", - "rotation_axis": [1, 0, 0], - "rotation_angle_deg": 90 + "pos_x": 0.2, + "pos_y": 0.0, + "pos_z": 0.05 + }, + "sensors": { + "description": "The sensors of the robot. Leave the sensors attribute empty if no sensors. Or set all flags enble to false", + "sensors": { + "front-sonar": { + "idx": 0, + "enable": true, + "dimensions": [0.04, 0.04, 0.04], + "update_rate": 50.0, + "max_distance": 5.0, + "min_distance": 0.5, + "backend": "CHRONO", + "type": "ultrasonic-range", + "name": "Ultrasonic", + "color": [1.0, 0.0, 0.0], + "frame_pos": [0.27, 0.0, 0.05] + + }, + "rear-sonar": { + "idx": 1, + "enable": true, + "pos_x": 0.0, + "pos_y": 0.0, + "pos_z": 0.0, + "update_rate": 50.0, + "max_distance": 5.0, + "min_distance": 0.5, + "backend": "CHRONO", + "type": "ultrasonic-range", + "name": "Ultrasonic" + }, + "front-camera": { + "idx": 2, + "enable": true, + "pos_x": 0.0, + "pos_y": 0.0, + "pos_z": 0.0, + "update_rate": 50.0, + "max_distance": 5.0, + "min_distance": 0.5, + "backend": "CHRONO", + "type": "camera", + "name": "Ultrasonic" + } + } } } \ No newline at end of file diff --git a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp index 0fa9200..f0ce67a 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp @@ -1,517 +1,407 @@ -#include "bitrl/bitrl_config.h" +#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" + #ifdef BITRL_CHRONO -#include "bitrl/bitrl_types.h" -#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" +#include "bitrl/utils/io/json_file_reader.h" +#include "bitrl/extern/nlohmann/json/json.hpp" +#include "bitrl/sensors/ultrasonic_sensor.h" +#include -#include #include +#include +#include +#include +#include +#include -#include "chrono/physics/ChBodyEasy.h" -#include "chrono/assets/ChVisualShapeTriangleMesh.h" -#include "chrono/physics/ChMassProperties.h" +namespace bitrl +{ +namespace rb::bitrl_chrono +{ +namespace +{ +using json = nlohmann::json; +// helper structs to read data from JSON +struct MaterialData +{ + real_t friction; + real_t restitution; + MaterialData(const json& j) + : + friction(j.at("friction").get()), + restitution(j.at("restitution").get()) + {} +}; + +struct ChassisData +{ + real_t density{0.0}; + real_t wheel_radius{0.0}; + real_t wheel_width{0.0}; + real_t chassis_x; + real_t chassis_y; + real_t chassis_z; + real_t pos_x; + real_t pos_y; + real_t pos_z; + + ChassisData(const json& j) + : + chassis_x(j.at("chassis_x").get()), + chassis_y(j.at("chassis_y").get()), + chassis_z(j.at("chassis_z").get()), + pos_x(j.at("pos_x").get()), + pos_y(j.at("pos_y").get()), + pos_z(j.at("pos_z").get()) + {} + +}; + +struct WheelData +{ + real_t density; + real_t wheel_radius; + real_t wheel_width; + real_t pos_x; + real_t pos_y; + real_t pos_z; + + WheelData(const json& j) + : + pos_x(j.at("pos_x").get()), + pos_y(j.at("pos_y").get()), + pos_z(j.at("pos_z").get()) + {} +}; + +struct CasterWheelData +{ + real_t density; + real_t wheel_radius; + real_t pos_x; + real_t pos_y; + real_t pos_z; + CasterWheelData(const json& j) + : + pos_x(j.at("pos_x").get()), + pos_y(j.at("pos_y").get()), + pos_z(j.at("pos_z").get()) + {} +}; + +// helper functions to build the various entities of the robot +auto build_material(const MaterialData& md) +{ + auto material = chrono_types::make_shared(); + material->SetFriction(md.friction); + material->SetRestitution(md.restitution); + return material; +} +auto build_chassis(std::shared_ptr material, const ChassisData& cd) +{ + auto chassis = chrono_types::make_shared(cd.chassis_x, cd.chassis_y, cd.chassis_z, + cd.density, true, true, material); + chrono::ChVector3d pos(cd.pos_x, cd.pos_y, cd.wheel_radius + cd.chassis_z/2.0 + 0.01); + chassis->SetPos(pos); + chassis->EnableCollision(true); + return chassis; +} -namespace bitrl +auto build_wheel(std::shared_ptr material, const WheelData& wd) { -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(); - } + auto wheel = chrono_types::make_shared(chrono::ChAxis::Y, + wd.wheel_radius,wd.wheel_width, + wd.density, true, true, material); + + const chrono::ChVector3d pos(wd.pos_x, wd.pos_y, wd.pos_z); + wheel->SetPos(pos); + wheel->EnableCollision(true); + return wheel; } -// 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); +auto build_caster_wheel(std::shared_ptr material, + const CasterWheelData& cwd, + bool visualization=true, + bool collision=true + ) +{ + + auto caster = chrono_types::make_shared( + cwd.wheel_radius, // radius + cwd.density, // density + visualization, // visualization + collision, // collision + material); + + const chrono::ChVector3d pos(cwd.pos_x, cwd.pos_y, cwd.pos_z); + caster->SetPos(pos); + caster->EnableCollision(true); + return caster; +} + +auto build_caster_joint(std::shared_ptr caster, + std::shared_ptr chassis, + chrono::ChVector3d& pos) +{ + auto caster_joint = chrono_types::make_shared(); + caster_joint->Initialize( + caster, + chassis, + chrono::ChFrame<>(pos, chrono::QUNIT)); + return caster_joint; + } -// 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); +auto build_motor(std::shared_ptr wheel, + std::shared_ptr speed_func, + std::shared_ptr chassis, + const chrono::ChVector3d& pos) +{ + auto motor = chrono_types::make_shared(); + chrono::ChQuaternion<> rot = chrono::QuatFromAngleX(chrono::CH_PI_2); + motor->Initialize( + wheel, + chassis, + chrono::ChFrame<>(pos, rot) + ); + + motor->SetSpeedFunction(speed_func); + return motor; } -// 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; +template<> +void CHRONO_DiffDriveRobot::RobotData::read(const json& j) +{ + max_speed = j.at("max_speed").get(); + wheel_radius = j.at("wheel_radius").get(); + wheel_width = j.at("wheel_width").get(); + axle_length = j.at("axle_length").get(); + density = j.at("density").get(); + caster_wheel_radius = j.at("caster_wheel_radius").get(); } +namespace +{ + +using json = nlohmann::json; -CHRONO_DiffDriveRobot::CHRONO_DiffDriveRobot(chrono::ChSystem& system, - const chrono::ChVector3d& robot_pos, - const chrono::ChQuaternion<>& robot_rot, - std::shared_ptr wheel_mat) +struct RangeSensorData +{ + uint_t idx; + bool enable; + real_t update_rate; + real_t max_distance; + real_t min_distance; + std::string backend; + std::string name; + std::vector color; + std::vector frame_pos; + std::vector dimensions; + + RangeSensorData(const json& data) + : + idx(data.at("idx").get()), + enable(data.at("enable").get()), + update_rate(data.at("update_rate").get()), + max_distance(data.at("max_distance").get()), + min_distance(data.at("min_distance").get()), + backend(data.at("backend").get()), + name(data.at("name").get()), + color(data.at("color").get>()), + frame_pos(data.at("frame_pos").get>()), + dimensions(data.at("dimensions").get>()) + {} + +}; +} + +CHRONO_DiffDriveRobot::RobotSensorManager::RobotSensorManager() : - system_(&system), - robot_pos_(robot_pos), - robot_rot_(robot_rot), - wheel_material_(wheel_mat), - pose_() +sensor_manager_(0) +{} + +template<> +void CHRONO_DiffDriveRobot::RobotSensorManager::read(const json& j, std::shared_ptr body) +{ + + auto sensors = j.at("sensors"); + auto fsdata = RangeSensorData(sensors.at("front-sonar")); + + auto fs = sensors::UltrasonicSensor::create(fsdata.backend, fsdata.name); + sensor_manager_.add(fs); + + // get the backend and create the visual shape + auto backend_ptr = fs -> backend_ptr(); + + if (!backend_ptr) { - // 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); + throw std::runtime_error("Backend ptr pointer is not set"); } - // Create the contact materials - chassis_material_ = build_default_contact_material(contact_method); - if (!wheel_material_) - wheel_material_ = build_default_contact_material(contact_method); - + backend_ptr -> set_chrono_body(body); + chrono::ChFrame<> sensor_frame( + chrono::ChVector3d(fsdata.frame_pos[0], fsdata.frame_pos[1], fsdata.frame_pos[1]) + ); + + chrono::ChColor color(fsdata.color[0], fsdata.color[1], fsdata.color[2]); + std::unordered_map vis_shape_properties; + vis_shape_properties["chrono::ChColor"] = color; + vis_shape_properties["chrono::ChFrame<>"] = sensor_frame; + vis_shape_properties["x_length"] = fsdata.dimensions[0]; + vis_shape_properties["y_length"] = fsdata.dimensions[1]; + vis_shape_properties["z_length"] = fsdata.dimensions[2]; + backend_ptr -> add_visual_shape(vis_shape_properties); } +void CHRONO_DiffDriveRobot::add_to_sys(chrono::ChSystemNSC& sys) const +{ + if (!is_loaded_) + { + throw std::logic_error("Robot has not been loaded have you called load?"); + } + sys.Add(chassis_); + sys.Add(left_wheel_); + sys.Add(right_wheel_); + sys.Add(left_wheel_motor_); + sys.Add(right_wheel_motor_); + sys.Add(caster_wheel_); + sys.Add(caster_link_joint_); +} -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(); +void CHRONO_DiffDriveRobot::set_speed(real_t speed) +{ + if (!is_loaded_) + { + throw std::logic_error("Robot has not been loaded have you called load?"); } - // 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(); + // find out the current velocity vector + auto current_vel_vec = chassis_ -> GetPosDt(); + chrono::ChVector3d vel(speed, current_vel_vec[1], current_vel_vec[2]); + chassis_ -> SetPosDt(vel); + auto speed_ = vel.Length(); + + //set the motors speed + auto omega = speed_ / data_.wheel_radius; + set_motor_speed(omega); + +} + +void CHRONO_DiffDriveRobot::load(const std::filesystem::path& filename) +{ + + if (is_loaded_) + { + return; } - // 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()); + utils::io::JSONFileReader reader(filename); + reader.open(); + + data_.read(reader.get("robot-data")); + + auto md = reader.at("material"); + auto material = build_material(md); + + auto cd = reader.at("chassis"); + cd.wheel_radius = data_.wheel_radius; + cd.wheel_width = data_.wheel_width; + cd.density = data_.density; + chassis_ = build_chassis(material, cd); + + auto lwd = reader.at("left-wheel"); + lwd.wheel_radius = data_.wheel_radius; + lwd.wheel_width = data_.wheel_width; + lwd.density = data_.density; + left_wheel_ = build_wheel(material, lwd); + + auto rwd = reader.at("right-wheel"); + rwd.wheel_radius = data_.wheel_radius; + rwd.wheel_width = data_.wheel_width; + rwd.density = data_.density; + right_wheel_ = build_wheel(material, rwd); + + auto cwd = reader.at("caster-wheel"); + cwd.wheel_radius = data_.caster_wheel_radius; + cwd.density = data_.density; + caster_wheel_ = build_caster_wheel(material, cwd); + + chrono::ChVector3d pos(-0.25, 0, cwd.wheel_radius); + caster_link_joint_ = build_caster_joint(caster_wheel_, chassis_, pos); + + left_wheel_motor_speed_func_ = chrono_types::make_shared(0.0); + left_wheel_motor_ = build_motor(left_wheel_, + left_wheel_motor_speed_func_, + chassis_, chrono::ChVector3d(0,0.2,0.1)); - top_plate_ -> init(); + right_wheel_motor_speed_func_ = chrono_types::make_shared(0.0); + right_wheel_motor_ = build_motor(right_wheel_, right_wheel_motor_speed_func_, + chassis_, chrono::ChVector3d(0.0,-0.2,0.1)); - this -> build_motors_(); + // track the chassis for pose reports + pose_.set_body(chassis_); + + // read the sensors + sensor_manager_.read(reader.get("sensors"), chassis_); + + // set the loaded flag to true to signal + // successful loading + is_loaded_ = true; } -/// 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(uint_t m, real_t speed) +{ + if (!is_loaded_) + { + throw std::logic_error("Robot has not been loaded have you called load?"); } -} -void CHRONO_DiffDriveRobot::set_motor_speed(real_t rad_speed, uint_t id) { - motors_func_[id]->SetConstant(rad_speed); -} + if (m !=0 || m != 1) + { + throw std::logic_error("Invalid motor index. Index must be either 0 or 1"); + } + + if (m == 0) + { + left_wheel_motor_speed_func_ -> SetConstant(speed); + } + + if (m == 1) + { + right_wheel_motor_speed_func_ -> SetConstant(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(); +void CHRONO_DiffDriveRobot::set_motor_speed(real_t speed) +{ + set_motor_speed(0, speed); + set_motor_speed(1, speed); + + // now calculate the linear velocities of the motors + auto vl = speed * data_.wheel_radius; + auto vr = speed * data_.wheel_radius; + + auto v = 0.5 * (vl + vr); + auto omega = (vr - vl) * data_.axle_length; + + auto vx = v * std::cos(omega); + auto vy = v * std::sin(omega); + + chrono::ChVector3d vel(vx, vy, 0.0); + chassis_ -> SetPosDt(vel); } } } + #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 index 5cd698c..a77e9a0 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h +++ b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h @@ -1,17 +1,3 @@ - -// ============================================================================= -// 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 @@ -19,26 +5,25 @@ #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 "bitrl/sensors/sensor_manager.h" +#include "bitrl/sensors/sensor_base.h" -#include "chrono/physics/ChSystem.h" -#include "chrono/physics/ChLinkMotorRotationSpeed.h" -#include +#include +#include +#include +#include +#include namespace bitrl { -namespace rb::bitrl_chrono{ - +namespace rb::bitrl_chrono +{ /** - * @class CHRONO_DiffDriveRobotBase + * @class CHRONO_TurtleRobot * @ingroup rb_chrono * @brief Class for modelling a differential-drive robot using Chrono. * in fact the implementation of this class is taken from the @@ -46,72 +31,132 @@ namespace rb::bitrl_chrono{ * * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html */ -class CHRONO_DiffDriveRobot final { +class CHRONO_DiffDriveRobot +{ 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; + /// + /// Helper class to assemble some robot data + /// + struct RobotData + { + real_t max_speed; + real_t wheel_radius; + real_t wheel_width; + real_t axle_length; + real_t density; + real_t caster_wheel_radius; + + template + void read(const Archive& arch); + }; + + /// + /// Class to manage the sensors attached on this + /// + class RobotSensorManager + { + + public: + + /// + /// Constructor + /// + RobotSensorManager(); + + /// Load the sensors of the robot from the archive + /// @tparam Archive + /// @param arch + template + void read(const Archive& arch, std::shared_ptr body); + + /// + /// @return The number of registered senosrs + uint_t n_sensors()const noexcept{return sensor_manager_.n_sensors();} + + /// + /// @brief Add a new sensor to manage + /// @param sensor + /// + void add(std::shared_ptr sensor){sensor_manager_.add(sensor);} + + private: + + bitrl::sensors::SensorManager sensor_manager_; + + }; + + /// + /// Constructor + /// + CHRONO_DiffDriveRobot()=default; + + /// Load the robot details form the specified filename + void load(const std::filesystem::path& filename); + + /// Brief return the number of wheel motors this robot has + /// @return + /// + uint_t n_wheel_motors()const noexcept{return 2;} + + /// + /// Set the speed of the robot. + /// This function sets the x-component of the velocity vector + /// of the chassis + /// @param speed + void set_speed(real_t speed); + + /// + /// Set the speed of the m-th motor + /// m=0 => left motor + /// m=1 => right motor + void set_motor_speed(uint_t m, real_t speed); + + /// + /// Set both motors to the same speed + /// @param speed + /// + void set_motor_speed(real_t speed); + + /// /// Get the pose of the robot - const CHRONO_RobotPose& get_pose() const{return pose_;} - -private: + /// + const CHRONO_RobotPose& get_pose() const{return pose_; } - /// Build motors - void build_motors_(); + /// + /// Add the components of this robot to the given chrono::ChSystemNSC + /// @param sys + void add_to_sys(chrono::ChSystemNSC& sys) const; - /// Pointer to the Chrono system - chrono::ChSystem* system_; + /// Read/write reference to the senosr manager for this robot + /// @return + /// + RobotSensorManager & get_sensor_manager(){return sensor_manager_;} - /// 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 +private: - // model parts material - std::shared_ptr chassis_material_; ///< chassis contact material - std::shared_ptr wheel_material_; + /// the components of the robot + std::shared_ptr chassis_; + std::shared_ptr left_wheel_; + std::shared_ptr right_wheel_; + std::shared_ptr left_wheel_motor_; + std::shared_ptr left_wheel_motor_speed_func_; + std::shared_ptr right_wheel_motor_; + std::shared_ptr right_wheel_motor_speed_func_; + std::shared_ptr caster_wheel_; + std::shared_ptr caster_link_joint_; /// The pose of the chassis CHRONO_RobotPose pose_; + RobotData data_; + RobotSensorManager sensor_manager_; - bool dc_motor_control_ = false; + bool is_loaded_{false}; }; - - -} // namespace chrono } -#endif +} + #endif +#endif //DIFF_DRIVE_ROBOT_H 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/turtle_robot_chassis.cpp similarity index 95% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.cpp rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_chassis.cpp index ca2324d..b2ae7a6 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/turtle_robot_chassis.cpp @@ -2,12 +2,13 @@ #ifdef BITRL_CHRONO +#include "bitrl/bitrl_consts.h" +#include "bitrl/bitrl_types.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_chassis.h" + +#include -#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 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/turtle_robot_chassis.h similarity index 80% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_chassis.h index 1f36a2a..e2ae74e 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/turtle_robot_chassis.h @@ -1,22 +1,15 @@ -// -// Created by alex on 2/8/26. -// - -#ifndef DIFF_DRIVE_ROBOT_CHASSIS_H -#define DIFF_DRIVE_ROBOT_CHASSIS_H +#ifndef TURTLE_ROBOT_CHASSIS_H +#define TURTLE_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/turtle_robot_part.h" +#include -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" -#include "bitrl/bitrl_types.h" +#include namespace bitrl{ namespace rb::bitrl_chrono{ 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/turtle_robot_part.cpp similarity index 97% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.cpp index fd87d75..5e5eb99 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/turtle_robot_part.cpp @@ -2,8 +2,8 @@ #ifdef BITRL_CHRONO -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" #include "bitrl/bitrl_consts.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h" namespace bitrl{ namespace rb::bitrl_chrono{ 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/turtle_robot_part.h similarity index 95% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h index ec4c281..7cb3086 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/turtle_robot_part.h @@ -1,18 +1,19 @@ -#ifndef DIFF_DRIVE_ROBOT_PART_H -#define DIFF_DRIVE_ROBOT_PART_H +#ifndef TURTLE_ROBOT_PART_H +#define TURTLE_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" +#include +#include + +#include +#include namespace bitrl{ namespace rb::bitrl_chrono{ 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/turtle_robot_plates.cpp similarity index 99% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_plates.cpp index 654395d..d6c79de 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/turtle_robot_plates.cpp @@ -2,9 +2,9 @@ #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" +#include "bitrl/bitrl_types.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_plates.h" namespace bitrl { 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/turtle_robot_plates.h similarity index 94% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_plates.h index ed45053..30a3c10 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/turtle_robot_plates.h @@ -1,16 +1,15 @@ -#ifndef DIFF_DRIVE_ROBOT_PLATES_H -#define DIFF_DRIVE_ROBOT_PLATES_H +#ifndef TURTLE_ROBOT_PLATES_H +#define TURTLE_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" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h" +#include +#include namespace bitrl { 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/turtle_robot_rods.cpp similarity index 98% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_rods.cpp index effafc5..0c2085a 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/turtle_robot_rods.cpp @@ -2,8 +2,8 @@ #ifdef BITRL_CHRONO -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h" #include "bitrl/bitrl_consts.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_rods.h" namespace bitrl { 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/turtle_robot_rods.h similarity index 94% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_rods.h index dedf3ae..0c488b9 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/turtle_robot_rods.h @@ -1,5 +1,5 @@ -#ifndef DIFF_DRIVE_ROBOT_RODS_H -#define DIFF_DRIVE_ROBOT_RODS_H +#ifndef TURTLE_ROBOT_RODS_H +#define TURTLE_ROBOT_RODS_H #include "bitrl/bitrl_config.h" @@ -10,7 +10,7 @@ #include "chrono/physics/ChSystem.h" -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h" namespace bitrl { 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/turtle_robot_wheels.cpp similarity index 98% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_wheels.cpp index 8738eca..5213d72 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/turtle_robot_wheels.cpp @@ -2,9 +2,9 @@ #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/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_wheels.h" +#include "chrono/assets/ChColor.h" #include "bitrl/bitrl_consts.h" 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/turtle_robot_wheels.h similarity index 94% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_wheels.h index d2399b6..4941c47 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/turtle_robot_wheels.h @@ -1,5 +1,5 @@ -#ifndef DIFF_DRIVE_ROBOT_WHEELS_H -#define DIFF_DRIVE_ROBOT_WHEELS_H +#ifndef TURTLE_ROBOT_WHEELS_H +#define TURTLE_ROBOT_WHEELS_H #include "bitrl/bitrl_config.h" @@ -8,7 +8,7 @@ #include #include -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h" namespace bitrl{ namespace rb::bitrl_chrono { 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 deleted file mode 100644 index 2806005..0000000 --- a/src/bitrl/rigid_bodies/chrono_robots/tmp/chrono_robot_pose.h +++ /dev/null @@ -1,84 +0,0 @@ -#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 deleted file mode 100644 index e85c28d..0000000 --- a/src/bitrl/rigid_bodies/chrono_robots/tmp/org_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/tmp/org_chrono_diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.h deleted file mode 100644 index 390352f..0000000 --- a/src/bitrl/rigid_bodies/chrono_robots/tmp/org_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/tmp/rb_chrono_module.h b/src/bitrl/rigid_bodies/chrono_robots/tmp/rb_chrono_module.h deleted file mode 100644 index e1426e8..0000000 --- a/src/bitrl/rigid_bodies/chrono_robots/tmp/rb_chrono_module.h +++ /dev/null @@ -1,12 +0,0 @@ -#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 diff --git a/src/bitrl/rigid_bodies/chrono_robots/turtle_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/turtle_robot.cpp new file mode 100644 index 0000000..7346e05 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/turtle_robot.cpp @@ -0,0 +1,513 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/bitrl_types.h" +#include "bitrl/rigid_bodies/chrono_robots/turtle_robot.h" + +#include +#include + + + + + +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_TurtleRobot::CHRONO_TurtleRobot(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_TurtleRobot::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_TurtleRobot::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_TurtleRobot::set_motor_speed(real_t rad_speed, uint_t id) { + motors_func_[id]->SetConstant(rad_speed); +} + +chrono::ChVector3d CHRONO_TurtleRobot::get_active_wheel_speed(uint_t id)const { + return drive_wheels_[id]->get_body_ptr()->GetPosDt(); +} + +chrono::ChVector3d CHRONO_TurtleRobot::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/turtle_robot.h b/src/bitrl/rigid_bodies/chrono_robots/turtle_robot.h new file mode 100644 index 0000000..3493ca0 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/turtle_robot.h @@ -0,0 +1,102 @@ +#ifndef TURTLE_ROBOT_H +#define TURTLE_ROBOT_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/bitrl_types.h" +#include "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_chassis.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_plates.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_rods.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_wheels.h" + +#include +#include +#include + + +namespace bitrl +{ +namespace rb::bitrl_chrono{ + + +/** + * @class CHRONO_TurtleRobot + * @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_TurtleRobot final { +public: + CHRONO_TurtleRobot(chrono::ChSystem& system, + const chrono::ChVector3d& robot_pos, + const chrono::ChQuaternion<>& robot_rot, + std::shared_ptr wheel_mat = {}); + + + /// destructor + ~CHRONO_TurtleRobot()=default; + + /// Initialize the CHRONO_TurtleRobot 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/sensors/backends/chrono_ultrasound_backend.cpp b/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp index 0bfc77f..24b6672 100644 --- a/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp +++ b/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp @@ -5,9 +5,8 @@ #include "bitrl/utils/io/json_file_reader.h" #include "bitrl/sensors/backends/chrono_ultrasound_backend.h" -#include "chrono/collision/ChCollisionSystem.h" - +#include #ifdef BITRL_LOG #include @@ -20,15 +19,27 @@ namespace bitrl namespace sensors::backends { +// namespace{ +// using json = nlohmann::json; +// } + const std::string CHRONO_UltrasonicBackend::BACKEND_TYPE = "CHRONO_UltrasonicBackend"; +CHRONO_UltrasonicBackend::CHRONO_UltrasonicBackend() + : +RangeSensorBackendBase(CHRONO_UltrasonicBackend::BACKEND_TYPE), +sys_ptr_(nullptr) +{} + + CHRONO_UltrasonicBackend::CHRONO_UltrasonicBackend(chrono::ChSystem& sys_ptr, std::shared_ptr body) : RangeSensorBackendBase(CHRONO_UltrasonicBackend::BACKEND_TYPE), -sys_ptr_(&sys_ptr), -body_(body) -{} +sys_ptr_(&sys_ptr) +{ + this -> set_chrono_body(body); +} void CHRONO_UltrasonicBackend::load_from_json(const std::string& filename) { @@ -55,8 +66,6 @@ void CHRONO_UltrasonicBackend::load_from_json(const std::string& filename) for (size_t i = 0; i < 3; ++i) position_[i] = pos.at(i).get(); - - #ifdef BITRL_LOG BOOST_LOG_TRIVIAL(info)<<"Loaded sensor backend: CHRONO_UltrasonicBackend"; #endif @@ -93,6 +102,9 @@ std::vector CHRONO_UltrasonicBackend::read_values() return {this -> max_distance()}; } + + + } } diff --git a/src/bitrl/sensors/backends/chrono_ultrasound_backend.h b/src/bitrl/sensors/backends/chrono_ultrasound_backend.h index 6c18f36..e5f78d3 100644 --- a/src/bitrl/sensors/backends/chrono_ultrasound_backend.h +++ b/src/bitrl/sensors/backends/chrono_ultrasound_backend.h @@ -7,11 +7,13 @@ #include "bitrl/sensors/backends/range_sensor_backend_base.h" -#include "chrono/physics/ChSystem.h" -#include "chrono/physics/ChBody.h" +#include +#include #include #include +#include +#include namespace bitrl { @@ -32,9 +34,14 @@ class CHRONO_UltrasonicBackend final: public RangeSensorBackendBase */ static const std::string BACKEND_TYPE; - /** - * @brief Constructor - */ + /// + /// @brief Constructor + /// + CHRONO_UltrasonicBackend(); + + /// + /// @brief Constructor + /// CHRONO_UltrasonicBackend(chrono::ChSystem& sys_ptr, std::shared_ptr body); @@ -63,6 +70,7 @@ class CHRONO_UltrasonicBackend final: public RangeSensorBackendBase */ void load_from_json(const std::string& filename); + /** * @brief Set the position of the sensor * @param pos @@ -80,12 +88,7 @@ class CHRONO_UltrasonicBackend final: public RangeSensorBackendBase /** * @brief Pointer to the system this sensor is attached ot */ - chrono::ChSystem* sys_ptr_; - - /** - * @brief The body on which the sensor sits - */ - std::shared_ptr body_; + chrono::ChSystem* sys_ptr_{nullptr}; /** * @brief The position of the sensor diff --git a/src/bitrl/sensors/backends/mqtt_ultrasound_backend.cpp b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.cpp index a57f250..813ce10 100644 --- a/src/bitrl/sensors/backends/mqtt_ultrasound_backend.cpp +++ b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.cpp @@ -14,6 +14,12 @@ namespace sensors::backends { const std::string MQTT_UltrasonicBackend::BACKEND_TYPE="MQTT_UltrasonicBackend"; +MQTT_UltrasonicBackend::MQTT_UltrasonicBackend() + : +RangeSensorBackendBase(MQTT_UltrasonicBackend::BACKEND_TYPE), +subscriber_ptr_(nullptr) +{} + MQTT_UltrasonicBackend::MQTT_UltrasonicBackend(network::MqttSubscriber& subscriber) : diff --git a/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h index 1514507..d9bacfb 100644 --- a/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h +++ b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h @@ -10,6 +10,8 @@ #include "bitrl/sensors/backends/range_sensor_backend_base.h" #include "bitrl/sensors/messages/ultrasound.h" + + namespace bitrl{ namespace sensors::backends { @@ -30,9 +32,14 @@ class MQTT_UltrasonicBackend: public RangeSensorBackendBase */ static const std::string BACKEND_TYPE; - /** - * @brief Constructor - */ + /// + /// @brief Constructor + /// + MQTT_UltrasonicBackend(); + + /// + /// @brief Constructor + /// explicit MQTT_UltrasonicBackend(network::MqttSubscriber& subscriber); /** diff --git a/src/bitrl/sensors/backends/range_sensor_backend_base.cpp b/src/bitrl/sensors/backends/range_sensor_backend_base.cpp new file mode 100644 index 0000000..ef516b1 --- /dev/null +++ b/src/bitrl/sensors/backends/range_sensor_backend_base.cpp @@ -0,0 +1,52 @@ +#include "bitrl/sensors/backends/range_sensor_backend_base.h" +#include "bitrl/sensors/backends/chrono_ultrasound_backend.h" +#include "bitrl/sensors/backends/mqtt_ultrasound_backend.h" +#include "bitrl/utils/std_map_utils.h" + +#ifdef BITRL_CHRONO +#include +#endif + +#include +namespace bitrl +{ +namespace sensors::backends +{ +std::shared_ptr RangeSensorBackendBase::create(const std::string& backend_type) +{ + + if (backend_type == "CHRONO") + { + return std::make_shared(); + } + else if (backend_type == "MQTT") + { + return std::make_shared(); + } + + throw std::runtime_error("Unknown backend type: " + backend_type); +} + +#ifdef BITRL_CHRONO +void RangeSensorBackendBase::add_visual_shape(const std::unordered_map& shape_properties) +{ + auto x_length = utils::resolve("x_length", shape_properties); + auto y_length = utils::resolve("y_length", shape_properties); + auto z_length = utils::resolve("z_length", shape_properties); + auto color = utils::resolve("chrono::ChColor", shape_properties); + auto sensor_frame = utils::resolve>("chrono::ChFrame<>", shape_properties); + + auto sensor_vis = chrono_types::make_shared(x_length, y_length, z_length); // 4cm cube + sensor_vis->SetColor(color); //chrono::ChColor(1.0f, 0.0f, 0.0f)); // red + + // chrono::ChFrame<> sensor_frame( + // chrono::ChVector3d(cd.chassis_x / 2.0 + 0.02, 0.0, cd.chassis_z / 2.0) + // ); + + // Attach to chassis + this -> body_ -> AddVisualShape(sensor_vis, sensor_frame); +} +#endif + +} +} diff --git a/src/bitrl/sensors/backends/range_sensor_backend_base.h b/src/bitrl/sensors/backends/range_sensor_backend_base.h index 6b06261..285e6ed 100644 --- a/src/bitrl/sensors/backends/range_sensor_backend_base.h +++ b/src/bitrl/sensors/backends/range_sensor_backend_base.h @@ -1,10 +1,16 @@ #ifndef RANGE_SENSOR_BACKEND_BASE_H #define RANGE_SENSOR_BACKEND_BASE_H - +#include "bitrl/bitrl_config.h" #include "bitrl/bitrl_types.h" #include "bitrl/sensors/backends/sensor_backend_base.h" #include +#include + +#ifdef BITRL_CHRONO +#include +#include +#endif namespace bitrl { @@ -18,6 +24,10 @@ namespace sensors::backends class RangeSensorBackendBase : public SensorBackendBase { public: + + + static std::shared_ptr create(const std::string& backend_type); + /** * @return The maximum distance the range sensor can read */ @@ -52,6 +62,15 @@ class RangeSensorBackendBase : public SensorBackendBase */ void set_sensor_units(const std::string& units){sensor_units_ = units;} +#ifdef BITRL_CHRONO + /// + /// @brief Add a visual shape for the sensor in order to + /// be visualized with Irrlicht. Concrete classes may choose not + /// to implement this. In this case the sensor will not be visible + /// + virtual void add_visual_shape(const std::unordered_map& shape_properties)override; +#endif + protected: /** * @brief Constructor diff --git a/src/bitrl/sensors/backends/sensor_backend_base.h b/src/bitrl/sensors/backends/sensor_backend_base.h index 46198ee..36014ad 100644 --- a/src/bitrl/sensors/backends/sensor_backend_base.h +++ b/src/bitrl/sensors/backends/sensor_backend_base.h @@ -3,6 +3,14 @@ #include "bitrl/bitrl_types.h" #include "bitrl/bitrl_consts.h" +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO +#include +#include +#include +#endif + #include #include @@ -80,6 +88,19 @@ class SensorBackendBase */ std::string backend_type_str()const noexcept{return backend_type_;} +#ifdef BITRL_CHRONO + /// + /// @brief Add a visual shape for the sensor in order to + /// be visualized with Irrlicht. Concrete classes may choose not + /// to implement this. In this case the sensor will not be visible + /// + virtual void add_visual_shape(const std::unordered_map& shape_properties)=0; + + /// @brief Set the pointer to the body that this sensor is attached + /// @param body + void set_chrono_body(std::shared_ptr body){body_ = body;} +#endif + protected: /** * @brief Constructor @@ -87,6 +108,13 @@ class SensorBackendBase */ SensorBackendBase(const std::string& backend_type); +#ifdef BITRL_CHRONO + /// + /// @brief The body on which the sensor sits + /// + std::shared_ptr body_{nullptr}; +#endif + private: /** diff --git a/src/bitrl/sensors/sensor_base.h b/src/bitrl/sensors/sensor_base.h index 7083d5b..d4bfab6 100644 --- a/src/bitrl/sensors/sensor_base.h +++ b/src/bitrl/sensors/sensor_base.h @@ -5,6 +5,8 @@ #include "../bitrl_consts.h" #include "bitrl/bitrl_types.h" #include "bitrl/bitrl_consts.h" +#include "bitrl/sensors/backends/sensor_backend_base.h" + #include #include namespace bitrl @@ -92,6 +94,8 @@ class SensorBase */ std::string sensor_type()const noexcept{return sensor_type_;} + virtual std::shared_ptr backend_ptr()const{return backend_ptr_;} + protected: /** * @param sensor_type The type of the sensor @@ -104,6 +108,11 @@ class SensorBase */ std::vector values_; + /// + /// @brief Pointer to backend tha actually handles the sensor + /// + std::shared_ptr backend_ptr_; + private: /** diff --git a/src/bitrl/sensors/sensor_manager.h b/src/bitrl/sensors/sensor_manager.h index 8aefa5b..a6f6ada 100644 --- a/src/bitrl/sensors/sensor_manager.h +++ b/src/bitrl/sensors/sensor_manager.h @@ -22,6 +22,9 @@ class SensorManager: private boost::noncopyable { public: + typedef std::vector< std::shared_ptr>::iterator iterator; + typedef std::vector< std::shared_ptr >::const_iterator const_iterator; + explicit SensorManager(uint_t n_sensors); /** @@ -40,6 +43,12 @@ class SensorManager: private boost::noncopyable */ uint_t n_sensors() const { return sensors_.size(); } + iterator begin() { return sensors_.begin(); } + iterator end() { return sensors_.end(); } + + const_iterator begin() const { return sensors_.begin(); } + const_iterator end() const { return sensors_.end(); } + private: uint_t n_sensors_; diff --git a/src/bitrl/sensors/sensor_type_enum.h b/src/bitrl/sensors/sensor_type_enum.h index 1a0d599..74a2715 100644 --- a/src/bitrl/sensors/sensor_type_enum.h +++ b/src/bitrl/sensors/sensor_type_enum.h @@ -15,7 +15,7 @@ enum class SensorTypeEnum : uint_t INVALID_TYPE = 0, CAMERA = 1, ULTRASOUND = 2, - ODMETRY = 3, + ODOMETRY = 3, VECTOR_SENSOR = 4 }; } diff --git a/src/bitrl/sensors/ultrasonic_sensor.cpp b/src/bitrl/sensors/ultrasonic_sensor.cpp index b8e066e..e1d8a7f 100644 --- a/src/bitrl/sensors/ultrasonic_sensor.cpp +++ b/src/bitrl/sensors/ultrasonic_sensor.cpp @@ -1,5 +1,6 @@ #include "bitrl/bitrl_config.h" #include "bitrl/sensors/ultrasonic_sensor.h" +#include "bitrl/sensors/backends/range_sensor_backend_base.h" #ifdef BITRL_LOG #include @@ -13,6 +14,12 @@ namespace sensors { const std::string UltrasonicSensor::SENSOR_TYPE = "UltrasonicSensor"; +std::shared_ptr +UltrasonicSensor::create(const std::string& backend_type, const std::string& name) +{ + auto backend = sensors::backends::RangeSensorBackendBase::create(backend_type); + return std::make_shared(backend, name); +} void UltrasonicSensor::init() { diff --git a/src/bitrl/sensors/ultrasonic_sensor.h b/src/bitrl/sensors/ultrasonic_sensor.h index 6ab001d..49f8144 100644 --- a/src/bitrl/sensors/ultrasonic_sensor.h +++ b/src/bitrl/sensors/ultrasonic_sensor.h @@ -26,6 +26,12 @@ class UltrasonicSensor final: public SensorBase */ static const std::string SENSOR_TYPE; + /// + /// @param backend_type + /// @param name + /// @return + static std::shared_ptr create(const std::string& backend_type, const std::string& name); + /** * @brief Constructor * @param backend @@ -57,6 +63,9 @@ class UltrasonicSensor final: public SensorBase */ virtual std::string sensor_units()const; + + virtual std::shared_ptr backend_ptr()const override{return backend_;} + private: /** * @brief The backend used for this sensor diff --git a/src/bitrl/utils/io/csv_file_reader.cpp b/src/bitrl/utils/io/csv_file_reader.cpp index b0007c4..42ffac9 100644 --- a/src/bitrl/utils/io/csv_file_reader.cpp +++ b/src/bitrl/utils/io/csv_file_reader.cpp @@ -9,11 +9,11 @@ #include #endif +#include + namespace bitrl { -namespace utils -{ -namespace io +namespace utils::io { CSVFileReader::CSVFileReader(const std::string &file_name, const std::string delimeter) @@ -71,6 +71,5 @@ std::vector CSVFileReader::read_line_as_uint() return line_int; } -} // namespace io -} // namespace utils +} // namespace utils::io } // namespace bitrl diff --git a/src/bitrl/utils/io/csv_file_reader.h b/src/bitrl/utils/io/csv_file_reader.h index 8b99234..fd18b7f 100644 --- a/src/bitrl/utils/io/csv_file_reader.h +++ b/src/bitrl/utils/io/csv_file_reader.h @@ -4,8 +4,6 @@ #include "bitrl/bitrl_types.h" #include "bitrl/utils/io/file_reader_base.h" -#include -#include #include #include diff --git a/src/bitrl/utils/io/file_handler_base.h b/src/bitrl/utils/io/file_handler_base.h index ad0c90e..6e1fc81 100644 --- a/src/bitrl/utils/io/file_handler_base.h +++ b/src/bitrl/utils/io/file_handler_base.h @@ -1,6 +1,3 @@ -// SPDX-FileCopyrightText: 2024 -// SPDX-License-Identifier: Apache-2.0 - #ifndef FILE_HANDLER_BASE_H #define FILE_HANDLER_BASE_H @@ -8,15 +5,56 @@ #include "boost/noncopyable.hpp" #include +#include namespace bitrl { -namespace utils -{ -namespace io +namespace utils::io { /** - * @todo write docs + * @class FileHandlerBase + * @brief Abstract base class for file handling operations. + * + * This class provides a common interface and shared functionality for + * managing file input/output operations. It encapsulates a low-level + * file handler (e.g., std::ifstream, std::ofstream, or similar types) + * and enforces a consistent API for derived classes that implement + * specific file reading or writing behavior. + * + * The class is non-copyable (via boost::noncopyable) to prevent + * unintended duplication of file handles. + * + * @tparam HandlerType The underlying file stream/handler type. + * + * ### Responsibilities + * - Store and expose the file name and file format. + * - Manage the lifecycle of the file stream (open/close). + * - Provide access to the underlying file handler. + * - Ensure proper cleanup by closing the file in the destructor. + * + * ### Usage + * This class is intended to be inherited by concrete file handler + * implementations (e.g., readers or writers). Derived classes must + * implement the open() method to define how the file is opened. + * + * ### Example + * @code + * class MyFileReader : public FileHandlerBase { + * public: + * using FileHandlerBase::FileHandlerBase; + * + * void open() override { + * f_.open(file_name_, std::ios::in); + * } + * }; + * @endcode + * + * ### Notes + * - The destructor automatically calls close() to ensure the file + * is properly released. + * - Calling close() on an already closed file is safe. + * - The class does not handle error reporting; this should be + * implemented in derived classes if needed. */ template class FileHandlerBase : private boost::noncopyable { @@ -24,69 +62,117 @@ template class FileHandlerBase : private boost::noncopyab typedef HandlerType handler_type; /// - /// \brief Constructor + /// @brief Destructor. Ensures that the file is properly closed. + /// + /// @post The underlying file stream is closed. /// virtual ~FileHandlerBase(); /// - /// \brief Returns the type of the file + /// @brief Returns the type/format of the file. + /// + /// @return The file format as a FileFormats::Type value. + /// @post Does not modify object state. /// FileFormats::Type get_type() const noexcept { return t_; } /// - /// \brief Returns the underlying file stream + /// @brief Provides mutable access to the underlying file stream. + /// + /// @return Reference to the underlying file handler. + /// @pre The file may or may not be open. /// handler_type &get_file_stream() noexcept { return f_; } /// - /// \brief Returns the underlying file stream + /// @brief Provides read-only access to the underlying file stream. + /// + /// @return Const reference to the underlying file handler. /// const handler_type &get_file_stream() const noexcept { return f_; } /// - /// \brief Returns the filename that is used to write + /// @brief Returns the file name associated with this handler. + /// + /// @return The file name as a std::string. /// std::string get_filename() const noexcept { return file_name_; } - /** - * \brief Return true if and only if the file is open - * - */ + /// + /// @brief Returns the file path associated with this handler. + /// + /// @return The file name as a std::filesystem::path object. + /// + std::filesystem::path get_file_path() const noexcept { return file_name_; } + + /// + /// @brief Checks whether the file is currently open. + /// + /// @return true if the file stream is open, false otherwise. + /// bool is_open() const noexcept { return f_.is_open(); } - /** - * \brief Close the file. Return true if and only if the file - * was closed successfully false otherwise - * - */ + /// + /// @brief Closes the file if it is currently open. + /// + /// If the underlying file stream is open, this function attempts + /// to close it. If the file is already closed, no action is taken. + /// + /// @post The file stream is not open (is_open() == false). + /// \throws May propagate exceptions thrown by handler_type::close() + /// if the underlying stream supports throwing exceptions. + /// \note Calling this function multiple times is safe. + /// virtual void close(); - /** - * @brief Open the file - */ + /// + /// @brief Opens the file. + /// + /// This is a pure virtual function that must be implemented by + /// derived classes. Implementations should define how the file + /// is opened (e.g., read/write mode, binary/text mode, etc.). + /// + /// @pre The file is not already open, unless the derived class + /// explicitly supports reopening. + /// @post If successful, is_open() == true. + /// @throws Implementation-defined. Derived classes should document + /// any exceptions or error-handling behavior. + /// virtual void open() = 0; protected: /** - * @brief protected Constructor so that explicit instantiation of - * the class fails + * @brief Protected constructor. + * + * Initializes the file handler with a file name and format. + * + * @param file_name The name (or path) of the file to be handled. + * @param t The file format/type. + * + * @pre file_name should not be empty. + * @post The handler is initialized but the file is not open. */ FileHandlerBase(const std::string &file_name, FileFormats::Type t); /** - * @brief The name of the file to write - * - */ - + * @brief The name of the file to read from or write to. + * + * Represents the path used when opening the file. + */ std::string file_name_; /// - /// \brief The format of the file + /// \brief The format/type of the file. + /// + /// Immutable after construction. /// const FileFormats::Type t_; /** - * @brief The low level file handler + * @brief The underlying file stream/handler. + * + * This object represents the low-level file interface + * (e.g., std::ifstream, std::ofstream, etc.). */ handler_type f_; }; @@ -106,8 +192,7 @@ template void FileHandlerBase::close() f_.close(); } -} // namespace io -} // namespace utils +} // namespace utils::io } // namespace bitrl #endif // FILE_HANDLER_BASE_H diff --git a/src/bitrl/utils/io/file_reader_base.cpp b/src/bitrl/utils/io/file_reader_base.cpp index 75a8b55..21d1a5f 100644 --- a/src/bitrl/utils/io/file_reader_base.cpp +++ b/src/bitrl/utils/io/file_reader_base.cpp @@ -1,6 +1,10 @@ #include "bitrl/utils/io/file_reader_base.h" #include "bitrl/bitrl_config.h" +#ifdef BITRL_LOG +#include +#endif + namespace bitrl { namespace utils::io @@ -14,6 +18,12 @@ FileReaderBase::FileReaderBase(const std::string &file_name, FileFormats::Type t void FileReaderBase::open() { + auto file_name = this->get_filename(); + +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Reading file: "<get_file_stream(); if (!f.is_open()) @@ -21,13 +31,12 @@ void FileReaderBase::open() try { - f.open(this->get_filename(), std::ios_base::in); + f.open(file_name, std::ios_base::in); #ifdef BITRL_DEBUG if (!f.good()) { - auto file_name = this->get_filename(); std::string msg = "Failed to open file: " + file_name; assert(false && msg.c_str()); } @@ -38,7 +47,6 @@ void FileReaderBase::open() #ifdef BITRL_DEBUG std::string msg("Failed to open file: "); - auto file_name = this->get_filename(); msg += file_name; assert(false && msg.c_str()); #else diff --git a/src/bitrl/utils/io/json_file_reader.cpp b/src/bitrl/utils/io/json_file_reader.cpp index 5df9804..feb21e9 100644 --- a/src/bitrl/utils/io/json_file_reader.cpp +++ b/src/bitrl/utils/io/json_file_reader.cpp @@ -1,6 +1,3 @@ -// SPDX-FileCopyrightText: 2024 -// SPDX-License-Identifier: Apache-2.0 - #include "bitrl/utils/io/json_file_reader.h" #include "bitrl/utils/io/file_formats.h" @@ -14,9 +11,13 @@ JSONFileReader::JSONFileReader(const std::string &filename) { } +JSONFileReader::JSONFileReader(const std::filesystem::path &filename) + : +FileReaderBase(filename.string(), FileFormats::Type::JSON), data_() +{} + void JSONFileReader::open() { - this->FileReaderBase::open(); auto &f = this->get_file_stream(); data_ = json::parse(f); diff --git a/src/bitrl/utils/io/json_file_reader.h b/src/bitrl/utils/io/json_file_reader.h index 8668795..99ac4ae 100644 --- a/src/bitrl/utils/io/json_file_reader.h +++ b/src/bitrl/utils/io/json_file_reader.h @@ -3,6 +3,7 @@ #include "bitrl/extern/nlohmann/json/json.hpp" #include "bitrl/utils/io/file_reader_base.h" +#include namespace bitrl { @@ -24,6 +25,10 @@ class JSONFileReader final : public FileReaderBase */ JSONFileReader(const std::string &filename); + /// Read the JSON file from the given filename + /// @param filename The std::filesystem::path to read the file from + JSONFileReader(const std::filesystem::path &filename); + /** * @brief Attempts to open the file for reading */