1 // ============================================================================= 2 // PROJECT CHRONO - http://projectchrono.org 3 // 4 // Copyright (c) 2021 projectchrono.org 5 // All rights reserved. 6 // 7 // Use of this source code is governed by a BSD-style license that can be found 8 // in the LICENSE file at the top level of the distribution and at 9 // http://projectchrono.org/license-chrono.txt. 10 // 11 // ============================================================================= 12 // Authors: Jason Zhou 13 // ============================================================================= 14 // 15 // Turtlebot Robot Class 16 // This is a modified version of the famous turtlebot 2e 17 // The geometries use the following resources as references: 18 // https://groups.google.com/g/sydney_ros/c/z05uQTCuDTQ 19 // https://grabcad.com/library/interbotix-turtlebot-2i-1 20 // https://www.turtlebot.com/turtlebot2/ 21 // 22 // ============================================================================= 23 24 #ifndef TURTLEBOT_H 25 #define TURTLEBOT_H 26 27 #include <array> 28 #include <fstream> 29 #include <string> 30 #include <unordered_map> 31 #include <vector> 32 33 #include "chrono/assets/ChColor.h" 34 #include "chrono/physics/ChLinkMotorRotationSpeed.h" 35 #include "chrono/physics/ChLinkDistance.h" 36 #include "chrono/physics/ChSystem.h" 37 #include "chrono/physics/ChShaft.h" 38 #include "chrono/physics/ChShaftsGear.h" 39 #include "chrono/physics/ChShaftsBody.h" 40 41 #include "chrono_models/ChApiModels.h" 42 43 #include "chrono/physics/ChInertiaUtils.h" 44 45 namespace chrono { 46 47 namespace turtlebot { 48 49 /// @addtogroup robot_models_turtlebot 50 /// @{ 51 52 /// turtlebot collision families. 53 namespace CollisionFamily { 54 enum Enum { 55 CHASSIS = 1, ///< chassis 56 ACTIVE_WHEEL = 2, ///< active cylinderical drive wheel 57 PASSIVE_WHEEL = 3, ///< passive cylinderical wheel 58 ROD = 4, ///< short and long supporting rods 59 BOTTOM_PLATE = 5, ///< bottom plate 60 MIDDLE_PLATE = 6, ///< middle plate 61 TOP_PLATE = 7 ///< top plate 62 }; 63 } 64 65 /// Turtlebot wheel identifiers. 66 enum WheelID { 67 LD, ///< left driven 68 RD, ///< right driven 69 }; 70 71 /// Base class definition of the Turtlebot Robot Part. 72 /// This class encapsulates base fields and functions. 73 class CH_MODELS_API Turtlebot_Part { 74 public: 75 Turtlebot_Part(const std::string& name, 76 bool fixed, 77 std::shared_ptr<ChMaterialSurface> mat, 78 ChSystem* system, 79 const ChVector<>& body_pos, 80 const ChQuaternion<>& body_rot, 81 std::shared_ptr<ChBodyAuxRef> chassis_body, 82 bool collide); ~Turtlebot_Part()83 virtual ~Turtlebot_Part() {} 84 85 /// Return the name of the part. GetName()86 const std::string& GetName() const { return m_name; } 87 88 /// Set the name of the part. SetName(const std::string & name)89 void SetName(const std::string& name) { m_name = name; } 90 91 /// Return the ChBody of the corresponding Turtlebot part. GetBody()92 std::shared_ptr<ChBodyAuxRef> GetBody() const { return m_body; } 93 94 /// Return the ChBody of the chassis wrt the Turtlebot part. GetChassis()95 std::shared_ptr<ChBodyAuxRef> GetChassis() const { return m_chassis; } 96 97 /// Return the Position of the Turtlebot part. GetPos()98 const ChVector<>& GetPos() const { return m_body->GetFrame_REF_to_abs().GetPos(); } 99 100 /// Return the Rotation of the Turtlebot part. GetRot()101 const ChQuaternion<>& GetRot() const { return m_body->GetFrame_REF_to_abs().GetRot(); } 102 103 protected: 104 /// Initialize the visulization mesh of the Turtlebot part. 105 void AddVisualizationAssets(); 106 107 /// Initialize the collision mesh of the Turtlebot part. 108 void AddCollisionShapes(); 109 110 /// Enable/disable collision. 111 void SetCollide(bool state); 112 113 std::string m_name; ///< subsystem name 114 std::shared_ptr<ChBodyAuxRef> m_body; ///< rigid body 115 std::shared_ptr<ChMaterialSurface> m_mat; ///< contact material (shared among all shapes) 116 117 std::string m_mesh_name; ///< visualization mesh name 118 ChVector<> m_offset; ///< offset for visualization mesh 119 ChColor m_color; ///< visualization asset color 120 ChSystem* m_system; ///< system which Turtlebot Part belongs to 121 std::shared_ptr<ChBodyAuxRef> m_chassis; ///< the chassis body for the robot 122 123 ChVector<> m_pos; ///< Turtlebot part's relative position wrt the chassis 124 ChQuaternion<> m_rot; ///< Turtlebot part's relative rotation wrt the chassis 125 double m_density; ///< Turtlebot part's density 126 127 bool m_collide; ///< Turtlebot part's collision indicator 128 bool m_fixed; ///< Turtlebot part's fixed indication 129 }; 130 131 /// Turtlebot Chassis class definition 132 class CH_MODELS_API Turtlebot_Chassis : public Turtlebot_Part { 133 public: 134 Turtlebot_Chassis(const std::string& name, 135 bool fixed, 136 std::shared_ptr<ChMaterialSurface> mat, 137 ChSystem* system, 138 const ChVector<>& body_pos, 139 const ChQuaternion<>& body_rot, 140 bool collide); ~Turtlebot_Chassis()141 ~Turtlebot_Chassis() {} 142 143 /// Initialize the chassis at the specified (absolute) position. 144 void Initialize(); 145 146 /// Enable/disable collision for the robot chassis. 147 void SetCollide(bool state); 148 149 private: 150 /// Translate the chassis by the specified value. 151 void Translate(const ChVector<>& shift); 152 friend class TurtleBot; 153 }; 154 155 /// Turtlebot Active Drive Wheel class definition 156 class CH_MODELS_API Turtlebot_ActiveWheel : public Turtlebot_Part { 157 public: 158 Turtlebot_ActiveWheel(const std::string& name, 159 bool fixed, 160 std::shared_ptr<ChMaterialSurface> mat, 161 ChSystem* system, 162 const ChVector<>& body_pos, 163 const ChQuaternion<>& body_rot, 164 std::shared_ptr<ChBodyAuxRef> chassis, 165 bool collide); ~Turtlebot_ActiveWheel()166 ~Turtlebot_ActiveWheel() {} 167 168 /// Initialize the wheel at the specified (absolute) position. 169 void Initialize(); 170 171 /// Enable/disable collision for the wheel. 172 void SetCollide(bool state); 173 174 private: 175 /// Translate the chassis by the specified value. 176 void Translate(const ChVector<>& shift); 177 friend class TurtleBot; 178 }; 179 180 /// Turtlebot Passive Driven Wheel class definition 181 class CH_MODELS_API Turtlebot_PassiveWheel : public Turtlebot_Part { 182 public: 183 Turtlebot_PassiveWheel(const std::string& name, 184 bool fixed, 185 std::shared_ptr<ChMaterialSurface> mat, 186 ChSystem* system, 187 const ChVector<>& body_pos, 188 const ChQuaternion<>& body_rot, 189 std::shared_ptr<ChBodyAuxRef> chassis, 190 bool collide); ~Turtlebot_PassiveWheel()191 ~Turtlebot_PassiveWheel() {} 192 193 /// Initialize the wheel at the specified (absolute) position. 194 void Initialize(); 195 196 /// Enable/disable collision for the wheel. 197 void SetCollide(bool state); 198 199 private: 200 /// Translate the chassis by the specified value. 201 void Translate(const ChVector<>& shift); 202 friend class TurtleBot; 203 }; 204 205 /// Short Supporting Rod class definition 206 class CH_MODELS_API Turtlebot_Rod_Short : public Turtlebot_Part { 207 public: 208 Turtlebot_Rod_Short(const std::string& name, 209 bool fixed, 210 std::shared_ptr<ChMaterialSurface> mat, 211 ChSystem* system, 212 const ChVector<>& body_pos, 213 const ChQuaternion<>& body_rot, 214 std::shared_ptr<ChBodyAuxRef> chassis, 215 bool collide); ~Turtlebot_Rod_Short()216 ~Turtlebot_Rod_Short() {} 217 218 /// Initialize the wheel at the specified (absolute) position. 219 void Initialize(); 220 221 /// Enable/disable collision for the wheel. 222 void SetCollide(bool state); 223 224 private: 225 /// Translate the chassis by the specified value. 226 void Translate(const ChVector<>& shift); 227 friend class TurtleBot; 228 }; 229 230 /// Turtlebot Bottom Plate class definition 231 class CH_MODELS_API Turtlebot_BottomPlate : public Turtlebot_Part { 232 public: 233 Turtlebot_BottomPlate(const std::string& name, 234 bool fixed, 235 std::shared_ptr<ChMaterialSurface> mat, 236 ChSystem* system, 237 const ChVector<>& body_pos, 238 const ChQuaternion<>& body_rot, 239 std::shared_ptr<ChBodyAuxRef> chassis, 240 bool collide); ~Turtlebot_BottomPlate()241 ~Turtlebot_BottomPlate() {} 242 243 /// Initialize the wheel at the specified (absolute) position. 244 void Initialize(); 245 246 /// Enable/disable collision for the wheel. 247 void SetCollide(bool state); 248 249 private: 250 /// Translate the chassis by the specified value. 251 void Translate(const ChVector<>& shift); 252 friend class TurtleBot; 253 }; 254 255 /// Turtlebot Middle Plate class definition 256 class CH_MODELS_API Turtlebot_MiddlePlate : public Turtlebot_Part { 257 public: 258 Turtlebot_MiddlePlate(const std::string& name, 259 bool fixed, 260 std::shared_ptr<ChMaterialSurface> mat, 261 ChSystem* system, 262 const ChVector<>& body_pos, 263 const ChQuaternion<>& body_rot, 264 std::shared_ptr<ChBodyAuxRef> chassis, 265 bool collide); ~Turtlebot_MiddlePlate()266 ~Turtlebot_MiddlePlate() {} 267 268 /// Initialize the wheel at the specified (absolute) position. 269 void Initialize(); 270 271 /// Enable/disable collision for the wheel. 272 void SetCollide(bool state); 273 274 private: 275 /// Translate the chassis by the specified value. 276 void Translate(const ChVector<>& shift); 277 friend class TurtleBot; 278 }; 279 280 /// Turtlebot Top Plate class definition 281 class CH_MODELS_API Turtlebot_TopPlate : public Turtlebot_Part { 282 public: 283 Turtlebot_TopPlate(const std::string& name, 284 bool fixed, 285 std::shared_ptr<ChMaterialSurface> mat, 286 ChSystem* system, 287 const ChVector<>& body_pos, 288 const ChQuaternion<>& body_rot, 289 std::shared_ptr<ChBodyAuxRef> chassis, 290 bool collide); ~Turtlebot_TopPlate()291 ~Turtlebot_TopPlate() {} 292 293 /// Initialize the wheel at the specified (absolute) position. 294 void Initialize(); 295 296 /// Enable/disable collision for the wheel. 297 void SetCollide(bool state); 298 299 private: 300 /// Translate the chassis by the specified value. 301 void Translate(const ChVector<>& shift); 302 friend class TurtleBot; 303 }; 304 305 /// Long Supporting Rod class definition 306 class CH_MODELS_API Turtlebot_Rod_Long : public Turtlebot_Part { 307 public: 308 Turtlebot_Rod_Long(const std::string& name, 309 bool fixed, 310 std::shared_ptr<ChMaterialSurface> mat, 311 ChSystem* system, 312 const ChVector<>& body_pos, 313 const ChQuaternion<>& body_rot, 314 std::shared_ptr<ChBodyAuxRef> chassis, 315 bool collide); ~Turtlebot_Rod_Long()316 ~Turtlebot_Rod_Long() {} 317 318 /// Initialize the wheel at the specified (absolute) position. 319 void Initialize(); 320 321 /// Enable/disable collision for the wheel. 322 void SetCollide(bool state); 323 324 private: 325 /// Translate the chassis by the specified value. 326 void Translate(const ChVector<>& shift); 327 friend class TurtleBot; 328 }; 329 330 /// Turtlebot Robot class 331 /// This class assemble and initialize a complete turtlebot robot 332 /// This class also handles general control commands of the robot 333 class CH_MODELS_API TurtleBot { 334 public: 335 TurtleBot(ChSystem* system, 336 const ChVector<>& robot_pos, 337 const ChQuaternion<>& robot_rot, 338 std::shared_ptr<ChMaterialSurface> wheel_mat = nullptr); 339 ~TurtleBot(); 340 341 /// Initialize the turtlebot robot using current parameters. 342 void Initialize(); 343 344 /// Set active drive wheel speed 345 void SetMotorSpeed(double rad_speed, WheelID id); 346 347 /// Get active drive wheel speed 348 ChVector<> GetActiveWheelSpeed(WheelID id); 349 350 /// Get active driver wheel angular velocity 351 ChVector<> GetActiveWheelAngVel(WheelID id); 352 353 private: 354 /// This function initializes all parameters for the robot. 355 /// Note: The robot will not be constructed in the ChSystem until Initialize() is called. 356 void Create(); 357 358 ChSystem* m_system; ///< pointer to the Chrono system 359 360 bool m_dc_motor_control = false; 361 362 std::shared_ptr<Turtlebot_Chassis> m_chassis; ///< robot chassis 363 std::vector<std::shared_ptr<Turtlebot_ActiveWheel>> m_drive_wheels; ///< 2 active robot drive wheels 364 std::vector<std::shared_ptr<Turtlebot_PassiveWheel>> m_passive_wheels; ///< 2 passive robot driven wheels 365 366 std::vector<std::shared_ptr<Turtlebot_Rod_Short>> m_1st_level_rods; ///< six first level supporting short rods 367 std::vector<std::shared_ptr<Turtlebot_Rod_Short>> m_2nd_level_rods; ///< six second level supporting short rods 368 std::vector<std::shared_ptr<Turtlebot_Rod_Long>> m_3rd_level_rods; ///< six third level support long rods 369 std::shared_ptr<Turtlebot_BottomPlate> m_bottom_plate; ///< bottom plate of the turtlebot robot 370 std::shared_ptr<Turtlebot_MiddlePlate> m_middle_plate; ///< middle plate of the turtlebot robot 371 std::shared_ptr<Turtlebot_TopPlate> m_top_plate; ///< top plate of the turtlebot robot 372 373 ChQuaternion<> m_robot_rot; ///< robot rotation 374 ChVector<> m_robot_pos; ///< robot translation position 375 376 std::vector<std::shared_ptr<ChLinkMotorRotationSpeed>> m_motors; ///< vector to store motors 377 378 std::vector<std::shared_ptr<ChFunction_Const>> m_motors_func; ///< constant motor angular speed func 379 380 // model parts material 381 std::shared_ptr<ChMaterialSurface> m_chassis_material; ///< chassis contact material 382 std::shared_ptr<ChMaterialSurface> m_wheel_material; ///< wheel contact material (shared across limbs) 383 }; 384 385 } // namespace turtlebot 386 } // namespace chrono 387 #endif 388