1 // ============================================================================= 2 // PROJECT CHRONO - http://projectchrono.org 3 // 4 // Copyright (c) 2018 projectchrono.org 5 // All right 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 // Author: Radu Serban 13 // ============================================================================= 14 // 15 // RoboSimian model classes. 16 // 17 // For a description of this robot, see: 18 // Satzinger B.W., Lau C., Byl M., Byl K. (2016) 19 // Experimental Results for Dexterous Quadruped Locomotion Planning with RoboSimian. 20 // In: Hsieh M., Khatib O., Kumar V. (eds) Experimental Robotics. 21 // Springer Tracts in Advanced Robotics, vol 109. Springer, Cham. 22 // https://doi.org/10.1007/978-3-319-23778-7_3 23 // 24 // ============================================================================= 25 26 #ifndef ROBOSIMIAN_H 27 #define ROBOSIMIAN_H 28 29 #include <array> 30 #include <fstream> 31 #include <string> 32 #include <unordered_map> 33 #include <vector> 34 35 #include "chrono/assets/ChColor.h" 36 #include "chrono/physics/ChLinkMotorRotation.h" 37 #include "chrono/physics/ChSystem.h" 38 39 #include "chrono_models/ChApiModels.h" 40 41 namespace chrono { 42 43 /// Namespace with classes for the RoboSimian model. 44 namespace robosimian { 45 46 /// @addtogroup robot_models_robosimian 47 /// @{ 48 49 // ----------------------------------------------------------------------------- 50 // Various definitions 51 // ----------------------------------------------------------------------------- 52 53 /// RoboSimian limb identifiers. 54 enum LimbID { 55 FR = 0, ///< front right 56 RR = 1, ///< rear right 57 RL = 2, ///< rear left 58 FL = 3 ///< front left 59 }; 60 61 /// Visualization type for a RoboSimian part. 62 enum class VisualizationType { 63 NONE, ///< no visualization 64 COLLISION, ///< render primitive collision shapes 65 MESH ///< render meshes 66 }; 67 68 /// RoboSimian collision families. 69 namespace CollisionFamily { 70 enum Enum { 71 LIMB_FR = 1, ///< front-right limb 72 LIMB_RR = 2, ///< rear-right limb 73 LIMB_RL = 3, ///< rear-left limb 74 LIMB_FL = 4, ///< front-left limb 75 CHASSIS = 5, ///< chassis (torso) 76 SLED = 6, ///< sled 77 WHEEL_DD = 7 ///< direct-drive wheels 78 }; 79 } 80 81 /// RoboSimian collision flags (specify which part carries collision shapes). 82 namespace CollisionFlags { 83 enum Enum { 84 NONE = 0, ///< no collision shapes on any body 85 CHASSIS = 1 << 0, ///< chassis (torso) 86 SLED = 1 << 1, ///< sled 87 LIMBS = 1 << 2, ///< all limb bodies (excluding final wheels) 88 WHEELS = 1 << 3, ///< all wheels 89 ALL = 0xFFFF ///< collision enabled on all bodies 90 }; 91 } 92 93 /// RoboSimian actuation modes. 94 enum class ActuationMode { 95 ANGLE, ///< prescribe time-series for joint angle 96 SPEED ///< prescribe time-series for joint angular speed 97 }; 98 99 /// RoboSimian locomotion modes. 100 enum class LocomotionMode { 101 WALK, ///< walking 102 SCULL, ///< crawl/slide on the sled 103 INCHWORM, ///< inchworm-type movement 104 DRIVE ///< driving 105 }; 106 107 // ----------------------------------------------------------------------------- 108 // Definition of a part (body + collision shapes + visualization assets) 109 // ----------------------------------------------------------------------------- 110 111 struct CH_MODELS_API BoxShape { BoxShapeBoxShape112 BoxShape(const chrono::ChVector<>& pos, const chrono::ChQuaternion<>& rot, const chrono::ChVector<>& dims) 113 : m_pos(pos), m_rot(rot), m_dims(dims) {} 114 chrono::ChVector<> m_pos; 115 chrono::ChQuaternion<> m_rot; 116 chrono::ChVector<> m_dims; 117 }; 118 119 struct CH_MODELS_API SphereShape { SphereShapeSphereShape120 SphereShape(const chrono::ChVector<>& pos, double radius) : m_pos(pos), m_radius(radius) {} 121 chrono::ChVector<> m_pos; 122 double m_radius; 123 }; 124 125 struct CH_MODELS_API CylinderShape { CylinderShapeCylinderShape126 CylinderShape(const chrono::ChVector<>& pos, const chrono::ChQuaternion<>& rot, double radius, double length) 127 : m_pos(pos), m_rot(rot), m_radius(radius), m_length(length) {} 128 chrono::ChVector<> m_pos; 129 chrono::ChQuaternion<> m_rot; 130 double m_radius; 131 double m_length; 132 }; 133 134 struct CH_MODELS_API MeshShape { 135 enum class Type { CONVEX_HULL, TRIANGLE_SOUP, NODE_CLOUD }; MeshShapeMeshShape136 MeshShape(const chrono::ChVector<>& pos, const chrono::ChQuaternion<>& rot, const std::string& name, Type type) 137 : m_pos(pos), m_rot(rot), m_name(name), m_type(type) {} 138 chrono::ChVector<> m_pos; 139 chrono::ChQuaternion<> m_rot; 140 std::string m_name; 141 Type m_type; 142 }; 143 144 /// RoboSimian part. 145 /// A robot part encapsulates a Chrono body with its collision and visualization shapes. 146 class CH_MODELS_API RS_Part { 147 public: 148 RS_Part(const std::string& name, std::shared_ptr<ChMaterialSurface> mat, chrono::ChSystem* system); ~RS_Part()149 virtual ~RS_Part() {} 150 GetName()151 const std::string& GetName() const { return m_name; } SetName(const std::string & name)152 void SetName(const std::string& name) { m_name = name; } 153 void SetVisualizationType(VisualizationType vis); 154 GetBody()155 std::shared_ptr<chrono::ChBodyAuxRef> GetBody() const { return m_body; } GetPos()156 const chrono::ChVector<>& GetPos() const { return m_body->GetFrame_REF_to_abs().GetPos(); } GetRot()157 const chrono::ChQuaternion<>& GetRot() const { return m_body->GetFrame_REF_to_abs().GetRot(); } 158 159 protected: 160 void AddVisualizationAssets(VisualizationType vis); 161 void AddCollisionShapes(); 162 163 std::string m_name; ///< subsystem name 164 std::shared_ptr<chrono::ChBodyAuxRef> m_body; ///< rigid body 165 std::shared_ptr<ChMaterialSurface> m_mat; ///< contact material (shared among all shapes) 166 std::vector<BoxShape> m_boxes; ///< set of collision boxes 167 std::vector<SphereShape> m_spheres; ///< set of collision spheres 168 std::vector<CylinderShape> m_cylinders; ///< set of collision cylinders 169 std::vector<MeshShape> m_meshes; ///< set of collision meshes 170 std::string m_mesh_name; ///< visualization mesh name 171 chrono::ChVector<> m_offset; ///< offset for visualization mesh 172 chrono::ChColor m_color; ///< visualization asset color 173 174 friend class RoboSimian; 175 friend class RS_Limb; 176 }; 177 178 // ----------------------------------------------------------------------------- 179 // Robot chassis (torso) 180 // ----------------------------------------------------------------------------- 181 182 /// RoboSimian chassis (torso). 183 class CH_MODELS_API RS_Chassis : public RS_Part { 184 public: 185 RS_Chassis(const std::string& name, bool fixed, std::shared_ptr<ChMaterialSurface> mat, chrono::ChSystem* system); ~RS_Chassis()186 ~RS_Chassis() {} 187 188 /// Initialize the chassis at the specified (absolute) position. 189 void Initialize(const chrono::ChCoordsys<>& pos); 190 191 /// Enable/disable collision for the sled (Default: false). 192 void SetCollide(bool state); 193 194 private: 195 /// Translate the chassis by the specified value. 196 void Translate(const chrono::ChVector<>& shift); 197 198 bool m_collide; ///< true if collision enabled 199 200 friend class RoboSimian; 201 }; 202 203 // ----------------------------------------------------------------------------- 204 // Robot sled (fixed to chassis) 205 // ----------------------------------------------------------------------------- 206 207 /// RoboSimian sled (attached to chassis). 208 class CH_MODELS_API RS_Sled : public RS_Part { 209 public: 210 RS_Sled(const std::string& name, std::shared_ptr<ChMaterialSurface> mat, chrono::ChSystem* system); ~RS_Sled()211 ~RS_Sled() {} 212 213 /// Initialize the sled at the specified position (relative to the chassis). 214 void Initialize(std::shared_ptr<chrono::ChBodyAuxRef> chassis, ///< chassis body 215 const chrono::ChVector<>& xyz, ///< location (relative to chassis) 216 const chrono::ChVector<>& rpy ///< roll-pitch-yaw (relative to chassis) 217 ); 218 219 /// Enable/disable collision for the sled (default: true). 220 void SetCollide(bool state); 221 222 private: 223 /// Translate the sled by the specified value. 224 void Translate(const chrono::ChVector<>& shift); 225 226 bool m_collide; ///< true if collision enabled 227 228 friend class RoboSimian; 229 }; 230 231 // ----------------------------------------------------------------------------- 232 // Direct-drive robot wheels (not used in current model) 233 // ----------------------------------------------------------------------------- 234 235 /// RoboSimian direct-drive wheel. 236 /// Note that this part is not used in the current model. 237 class CH_MODELS_API RS_WheelDD : public RS_Part { 238 public: 239 RS_WheelDD(const std::string& name, int id, std::shared_ptr<ChMaterialSurface> mat, chrono::ChSystem* system); ~RS_WheelDD()240 ~RS_WheelDD() {} 241 242 /// Initialize the direct-drive wheel at the specified position (relative to the chassis). 243 void Initialize(std::shared_ptr<chrono::ChBodyAuxRef> chassis, ///< chassis body 244 const chrono::ChVector<>& xyz, ///< location (relative to chassis) 245 const chrono::ChVector<>& rpy ///< roll-pitch-yaw (relative to chassis) 246 ); 247 }; 248 249 // ----------------------------------------------------------------------------- 250 // Robot limb components 251 // ----------------------------------------------------------------------------- 252 253 /// RoboSimian link. 254 /// A robot link encapsulates information for a body in a robot limb. 255 class CH_MODELS_API Link { 256 public: Link(const std::string & mesh_name,const chrono::ChVector<> & offset,const chrono::ChColor & color,double mass,const chrono::ChVector<> & com,const chrono::ChVector<> & inertia_xx,const chrono::ChVector<> & inertia_xy,const std::vector<CylinderShape> & shapes)257 Link(const std::string& mesh_name, ///< name of associated mesh 258 const chrono::ChVector<>& offset, ///< mesh offset 259 const chrono::ChColor& color, ///< mesh color 260 double mass, ///< link mass 261 const chrono::ChVector<>& com, ///< location of COM 262 const chrono::ChVector<>& inertia_xx, ///< moments of inertia 263 const chrono::ChVector<>& inertia_xy, ///< products of inertia 264 const std::vector<CylinderShape>& shapes ///< list of collision shapes 265 ) 266 : m_mesh_name(mesh_name), 267 m_offset(offset), 268 m_color(color), 269 m_mass(mass), 270 m_com(com), 271 m_inertia_xx(inertia_xx), 272 m_inertia_xy(inertia_xy), 273 m_shapes(shapes) {} 274 275 private: 276 std::string m_mesh_name; 277 chrono::ChVector<> m_offset; 278 chrono::ChColor m_color; 279 double m_mass; 280 chrono::ChVector<> m_com; 281 chrono::ChVector<> m_inertia_xx; 282 chrono::ChVector<> m_inertia_xy; 283 std::vector<CylinderShape> m_shapes; 284 285 friend class RS_Limb; 286 }; 287 288 struct CH_MODELS_API LinkData { LinkDataLinkData289 LinkData(std::string myname, Link mylink, bool inc) : name(myname), link(mylink), include(inc) {} 290 ////LinkData(LinkData&&) {} 291 292 std::string name; 293 Link link; 294 bool include; 295 }; 296 297 struct CH_MODELS_API JointData { 298 std::string name; 299 std::string linkA; 300 std::string linkB; 301 bool fixed; 302 chrono::ChVector<> xyz; 303 chrono::ChVector<> rpy; 304 chrono::ChVector<> axis; 305 }; 306 307 /// RoboSimian limb. 308 /// A robot limb represents a multibody chain composed of robot links and joints. 309 class CH_MODELS_API RS_Limb { 310 public: 311 RS_Limb(const std::string& name, ///< limb name 312 LimbID id, ///< limb ID 313 const LinkData data[], ///< data for limb links 314 std::shared_ptr<ChMaterialSurface> wheel_mat, ///< contact material for the limb wheel 315 std::shared_ptr<ChMaterialSurface> link_mat, ///< contact material for the limb links 316 chrono::ChSystem* system ///< containing system 317 ); ~RS_Limb()318 ~RS_Limb() {} 319 320 /// Initialize the limb at the specified position (relative to the chassis). 321 void Initialize(std::shared_ptr<chrono::ChBodyAuxRef> chassis, ///< chassis body 322 const chrono::ChVector<>& xyz, ///< location (relative to chassis) 323 const chrono::ChVector<>& rpy, ///< roll-pitch-yaw (relative to chassis) 324 CollisionFamily::Enum collision_family, ///< collision family 325 ActuationMode wheel_mode ///< motor type for wheel actuation 326 ); 327 328 /// Set visualization type for all limb links. 329 void SetVisualizationType(VisualizationType vis); 330 331 /// Enable/disable collision on all links, except final wheel (default: false). 332 void SetCollideLinks(bool state); 333 334 /// Enable/disable collision for final wheel (default: true). 335 void SetCollideWheel(bool state); 336 337 /// Get the total mass of this limb. 338 double GetMass() const; 339 340 /// Get a handle to the wheel body. GetWheelBody()341 std::shared_ptr<chrono::ChBodyAuxRef> GetWheelBody() const { return m_wheel->GetBody(); } 342 343 /// Get location of the wheel body. GetWheelPos()344 const chrono::ChVector<>& GetWheelPos() const { return m_wheel->GetPos(); } 345 346 /// Get angular velocity of the wheel body (expressed in local coordinates). GetWheelAngVelocity()347 chrono::ChVector<> GetWheelAngVelocity() const { return m_wheel->GetBody()->GetWvel_loc(); } 348 349 /// Get wheel angle. GetWheelAngle()350 double GetWheelAngle() const { return m_wheel_motor->GetMotorRot(); } 351 352 /// Get wheel angular speed. GetWheelOmega()353 double GetWheelOmega() const { return m_wheel_motor->GetMotorRot_dt(); } 354 355 /// Get angle for specified motor. 356 /// Motors are named "joint1", "joint2", ... , "joint8", starting at the chassis. 357 double GetMotorAngle(const std::string& motor_name) const; 358 359 /// Get angular speed for specified motor. 360 /// Motors are named "joint1", "joint2", ... , "joint8", starting at the chassis. 361 double GetMotorOmega(const std::string& motor_name) const; 362 363 /// Get actuator reaction torque [Nm] for specified motor. 364 /// Motors are named "joint1", "joint2", ... , "joint8", starting at the chassis. 365 double GetMotorTorque(const std::string& motor_name) const; 366 367 /// Get angles for all 8 limb motors. 368 std::array<double, 8> GetMotorAngles(); 369 370 /// Get angular velocities for all 8 limb motors. 371 std::array<double, 8> GetMotorOmegas(); 372 373 /// Get actuator torques for all 8 limb motors. 374 std::array<double, 8> GetMotorTorques(); 375 376 /// Get current motor actuations. 377 void GetMotorActuations(std::array<double, 8>& angles, std::array<double, 8>& speeds); 378 379 /// Set activation for given motor at current time. 380 void Activate(const std::string& motor_name, double time, double val); 381 382 /// Set activations for all motors at current time. 383 void Activate(double time, const std::array<double, 8>& vals); 384 385 private: 386 /// Translate the limb bodies by the specified value. 387 void Translate(const chrono::ChVector<>& shift); 388 389 std::string m_name; 390 std::unordered_map<std::string, std::shared_ptr<RS_Part>> m_links; 391 std::unordered_map<std::string, std::shared_ptr<chrono::ChLink>> m_joints; 392 std::unordered_map<std::string, std::shared_ptr<chrono::ChLinkMotorRotation>> m_motors; 393 std::shared_ptr<RS_Part> m_wheel; 394 std::shared_ptr<chrono::ChLinkMotorRotation> m_wheel_motor; 395 396 bool m_collide_links; ///< collide flag for all links (except final wheel) 397 bool m_collide_wheel; ///< collide flag for the final wheel 398 399 friend class RoboSimian; 400 }; 401 402 // ----------------------------------------------------------------------------- 403 // Definition of the RoboSimian robot 404 // ----------------------------------------------------------------------------- 405 406 class ContactManager; 407 class ContactMaterial; 408 class RS_Driver; 409 410 /// RoboSimian robot model. 411 /// The robot model consists of a chassis (torso) with an attached sled and four limbs (legs). 412 class CH_MODELS_API RoboSimian { 413 public: 414 /// Construct a RoboSimian with an implicit Chrono system. 415 RoboSimian(chrono::ChContactMethod contact_method, ///< contact formulation (SMC or NSC) 416 bool has_sled = false, ///< true if robot has sled body attached to chassis 417 bool fixed = false ///< true if robot chassis fixed to ground 418 ); 419 420 /// Construct a RoboSimian within the specified Chrono system. 421 RoboSimian(chrono::ChSystem* system, ///< containing system 422 bool has_sled = false, ///< true if robot has sled body attached to chassis 423 bool fixed = false ///< true if robot chassis fixed to ground 424 ); 425 426 ~RoboSimian(); 427 428 /// Get the containing system. GetSystem()429 chrono::ChSystem* GetSystem() { return m_system; } 430 431 /// Set actuation type for wheel motors (default: SPEED). SetMotorActuationMode(ActuationMode mode)432 void SetMotorActuationMode(ActuationMode mode) { m_wheel_mode = mode; } 433 434 /// Set collision flags for the various subsystems. 435 /// By default, collision is enabled for the sled and wheels only. 436 /// The 'flags' argument can be any of the CollisionFlag enums, or a combination thereof (using bit-wise operators). 437 void SetCollide(int flags); 438 439 /// Set coefficients of friction for sled-terrain and wheel-terrain contacts. 440 /// Default values: 0.8. 441 void SetFrictionCoefficients(float sled_friction, float wheel_friction); 442 443 /// Attach a driver system. 444 void SetDriver(std::shared_ptr<RS_Driver> driver); 445 446 /// Set visualization type for chassis subsystem. 447 void SetVisualizationTypeChassis(VisualizationType vis); 448 /// Set visualization type for sled subsystem. 449 void SetVisualizationTypeSled(VisualizationType vis); 450 /// Set visualization type for all limb subsystems. 451 void SetVisualizationTypeLimbs(VisualizationType vis); 452 /// Set visualization type for thr specified limb subsystem. 453 void SetVisualizationTypeLimb(LimbID id, VisualizationType vis); 454 /// Set visualization type for all wheel subsystem. 455 void SetVisualizationTypeWheels(VisualizationType vis); 456 457 /// Set output directory. 458 void SetOutputDirectory(const std::string& outdir, const std::string& root = "results"); 459 460 /// Get the total mass of the robot. 461 double GetMass() const; 462 463 /// Get a handle to the robot's chassis subsystem. GetChassis()464 std::shared_ptr<RS_Chassis> GetChassis() const { return m_chassis; } 465 466 /// Get a handle to the robot's chassis body. GetChassisBody()467 std::shared_ptr<chrono::ChBodyAuxRef> GetChassisBody() const { return m_chassis->GetBody(); } 468 469 /// Get location of the chassis body. GetChassisPos()470 const chrono::ChVector<>& GetChassisPos() const { return m_chassis->GetPos(); } 471 472 /// Get orientation of the chassis body. GetChassisRot()473 const chrono::ChQuaternion<>& GetChassisRot() const { return m_chassis->GetRot(); } 474 475 /// Get a handle to the robot's sled subsystem. GetSled()476 std::shared_ptr<RS_Sled> GetSled() const { return m_sled; } 477 478 /// Get a handle to the robot's sled body. GetSledBody()479 std::shared_ptr<chrono::ChBodyAuxRef> GetSledBody() const { return m_sled->GetBody(); } 480 481 /// Get a handle to the robot's specified limb subsystem. GetLimb(LimbID id)482 std::shared_ptr<RS_Limb> GetLimb(LimbID id) const { return m_limbs[id]; } 483 484 /// Get a handle to the wheel body for the specified limb. GetWheelBody(LimbID id)485 std::shared_ptr<chrono::ChBodyAuxRef> GetWheelBody(LimbID id) const { return m_limbs[id]->GetWheelBody(); } 486 487 /// Get location of the wheel body for the specified limb. GetWheelPos(LimbID id)488 const chrono::ChVector<>& GetWheelPos(LimbID id) const { return m_limbs[id]->GetWheelPos(); } 489 490 /// Get angular velocity of the wheel body for the specified limb (expressed in local coordinates). GetWheelAngVelocity(LimbID id)491 chrono::ChVector<> GetWheelAngVelocity(LimbID id) const { return m_limbs[id]->GetWheelAngVelocity(); } 492 493 /// Get wheel angle for the specified limb. GetWheelAngle(LimbID id)494 double GetWheelAngle(LimbID id) const { return m_limbs[id]->GetWheelAngle(); } 495 496 /// Get wheel angular speed for the specified limb. GetWheelOmega(LimbID id)497 double GetWheelOmega(LimbID id) const { return m_limbs[id]->GetWheelOmega(); } 498 499 /// Get angles for all 8 limb motors. GetMotorAngles(LimbID id)500 std::array<double, 8> GetMotorAngles(LimbID id) { return m_limbs[id]->GetMotorAngles(); } 501 502 /// Get angular velocities for all 8 limb motors. GetMotorOmegas(LimbID id)503 std::array<double, 8> GetMotorOmegas(LimbID id) { return m_limbs[id]->GetMotorOmegas(); } 504 505 /// Get actuator torques for all 8 limb motors. GetMotorTorques(LimbID id)506 std::array<double, 8> GetMotorTorques(LimbID id) { return m_limbs[id]->GetMotorTorques(); } 507 508 /// Access the chassis contact material. GetChassisContactMaterial()509 std::shared_ptr<ChMaterialSurface> GetChassisContactMaterial() { return m_chassis_material; } 510 511 /// Access the sled contact material. GetSledContactMaterial()512 std::shared_ptr<ChMaterialSurface> GetSledContactMaterial() { return m_sled_material; } 513 514 /// Access the wheel contact material. Note that this material is shared by all wheels. GetWheelContactMaterial()515 std::shared_ptr<ChMaterialSurface> GetWheelContactMaterial() { return m_wheel_material; } 516 517 /// Access the wheelDD contact material. Note that this material is shared by all DD wheels. GetWheelDDContactMaterial()518 std::shared_ptr<ChMaterialSurface> GetWheelDDContactMaterial() { return m_wheelDD_material; } 519 520 /// Access the link contact material. Note that this material is shared by all non-wheel links. GetLinkContactMaterial()521 std::shared_ptr<ChMaterialSurface> GetLinkContactMaterial() { return m_link_material; } 522 523 /// Initialize the robot at the specified chassis position and orientation. 524 void Initialize(const chrono::ChCoordsys<>& pos); 525 526 /// Directly activate the specified motor on the specified limb. 527 void Activate(LimbID id, const std::string& motor_name, double time, double val); 528 529 /// Advance dynamics of underlying system. 530 /// If a driver system is specified, apply motor actuations at current time. 531 void DoStepDynamics(double step); 532 533 /// Translate all robot bodies by the specified value. 534 void Translate(const chrono::ChVector<>& shift); 535 536 /// Output current data. 537 void Output(); 538 539 /// Report current contacts for all robot parts. 540 void ReportContacts(); 541 542 private: 543 void Create(bool has_sled, bool fixed); 544 545 chrono::ChSystem* m_system; ///< pointer to the Chrono system 546 bool m_owns_system; ///< true if system created at construction 547 548 std::shared_ptr<RS_Chassis> m_chassis; ///< robot chassis 549 std::shared_ptr<RS_Sled> m_sled; ///< optional sled attached to chassis 550 std::vector<std::shared_ptr<RS_Limb>> m_limbs; ///< robot limbs 551 ////std::shared_ptr<RS_WheelDD> m_wheel_left; ///< left DD wheel 552 ////std::shared_ptr<RS_WheelDD> m_wheel_right; ///< right DD wheel 553 554 ActuationMode m_wheel_mode; ///< type of actuation for wheel motor 555 556 std::shared_ptr<ChMaterialSurface> m_chassis_material; ///< chassis contact material 557 std::shared_ptr<ChMaterialSurface> m_sled_material; ///< sled contact material 558 std::shared_ptr<ChMaterialSurface> m_wheel_material; ///< wheel contact material (shared across limbs) 559 std::shared_ptr<ChMaterialSurface> m_link_material; ///< link contact material (shared across limbs) 560 std::shared_ptr<ChMaterialSurface> m_wheelDD_material; ///< wheelDD contact material (shared across limbs) 561 562 float m_wheel_friction; ///< coefficient of friction wheel-terrain (used in material_override) 563 float m_sled_friction; ///< coefficient of friction sled-terrain (used in material_override) 564 565 std::shared_ptr<RS_Driver> m_driver; ///< robot driver system 566 ContactManager* m_contact_reporter; ///< contact reporting callback class 567 ContactMaterial* m_material_override; ///< contact material override callback class 568 569 std::string m_outdir; ///< path of output directory 570 std::string m_root; ///< prefix of output filenames 571 std::ofstream m_outf[4]; ///< output file streams (one per limb) 572 573 friend class ContactMaterial; 574 }; 575 576 // ----------------------------------------------------------------------------- 577 // RoboSimian driver classes 578 // ----------------------------------------------------------------------------- 579 580 typedef std::array<std::array<double, 8>, 4> Actuation; 581 582 /// Driver for the RoboSimian robot. 583 class CH_MODELS_API RS_Driver { 584 public: 585 /// Driving phases. 586 enum Phase { POSE, HOLD, START, CYCLE, STOP }; 587 588 RS_Driver(const std::string& filename_start, ///< name of file with joint actuations for start phase 589 const std::string& filename_cycle, ///< name of file with joint actuations for cycle phase 590 const std::string& filename_stop, ///< name of file with joint actuations for stop phase 591 bool repeat = false ///< true if cycle phase is looped 592 ); 593 594 ~RS_Driver(); 595 596 /// Specify time intervals to assume and then hold the initial pose. 597 void SetTimeOffsets(double time_pose, double time_hold); 598 599 /// Return the current limb motor actuations. GetActuation()600 Actuation GetActuation() { return m_actuations; } 601 602 /// Directly feed actuations to the motors. SetActuation(Actuation ext_act)603 void SetActuation(Actuation ext_act) { m_actuations = ext_act; } 604 605 /// Set the driving mode to accept external inputs. SetDrivingMode(bool drivemode)606 void SetDrivingMode(bool drivemode) { driven = drivemode; } 607 608 /// Set the motor type (setpoint/torque). UseTorqueMotors(bool use_tm)609 void UseTorqueMotors(bool use_tm) { torque_actuated = use_tm; } 610 611 /// Return the current phase GetCurrentPhase()612 std::string GetCurrentPhase() const { return m_phase_names[m_phase]; } 613 614 /// Class to be used as callback interface for user-defined actions at phase changes. 615 class CH_MODELS_API PhaseChangeCallback { 616 public: ~PhaseChangeCallback()617 virtual ~PhaseChangeCallback() {} 618 /// Function called on each phase change. 619 virtual void OnPhaseChange(RS_Driver::Phase old_phase, RS_Driver::Phase new_phase) = 0; 620 }; 621 622 /// Register a phase-change callback object. RegisterPhaseChangeCallback(PhaseChangeCallback * callback)623 void RegisterPhaseChangeCallback(PhaseChangeCallback* callback) { m_callback = callback; } 624 625 private: 626 void Update(double time); 627 void LoadDataLine(double& time, Actuation& activations); 628 629 std::ifstream m_ifs_start; ///< input file stream for start phase 630 std::ifstream m_ifs_cycle; ///< input file stream for cycle phase 631 std::ifstream m_ifs_stop; ///< input file stream for stop phase 632 std::ifstream* m_ifs; ///< active input file stream 633 double m_time_pose; ///< time interval to assume initial pose 634 double m_time_hold; ///< time interval to hold initial pose 635 double m_offset; ///< current time offset in input files 636 bool m_repeat; ///< repeat cycle 637 Phase m_phase; ///< current phase 638 double m_time_1; ///< time for cached actuations 639 double m_time_2; ///< time for cached actuations 640 Actuation m_actuations_1; ///< cached actuations (before) 641 Actuation m_actuations_2; ///< cached actuations (after) 642 Actuation m_actuations; ///< current actuations 643 PhaseChangeCallback* m_callback; ///< user callback for phase change 644 bool driven = false; ///< true if the driver is expecting external inputs instead of reading from a file 645 bool torque_actuated = false; ///< true if using torque actuation instead of setpoints 646 647 static const std::string m_phase_names[5]; ///< names of various driver phases 648 649 friend class RoboSimian; 650 }; 651 652 /// Robot driver callback to keep track of average speed and distance between phase changes. 653 class CH_MODELS_API RS_DriverCallback : public RS_Driver::PhaseChangeCallback { 654 public: RS_DriverCallback(RoboSimian * robot)655 RS_DriverCallback(RoboSimian* robot) : m_start_x(0), m_start_time(0), m_robot(robot) {} 656 virtual void OnPhaseChange(RS_Driver::Phase old_phase, RS_Driver::Phase new_phase) override; 657 658 /// Get distance traveled from last phase change. GetDistance()659 double GetDistance() const { return m_robot->GetChassisPos().x() - m_start_x; } 660 /// Get time elapsed since last phase change. GetDuration()661 double GetDuration() const { return m_robot->GetSystem()->GetChTime() - m_start_time; } 662 /// Get average robot speed since last phase change. GetAvgSpeed()663 double GetAvgSpeed() const { return GetDistance() / GetDuration(); } 664 665 double m_start_x; ///< location at start of current phase 666 double m_start_time; ///< time at start of current phase 667 668 private: 669 RoboSimian* m_robot; 670 }; 671 672 /// @} robot_models_robosimian 673 674 } // namespace robosimian 675 } // namespace chrono 676 #endif 677