1 // ============================================================================= 2 // PROJECT CHRONO - http://projectchrono.org 3 // 4 // Copyright (c) 2014 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: Alessandro Tasora, Radu Serban 13 // ============================================================================= 14 15 #ifndef CHLINKMATE_H 16 #define CHLINKMATE_H 17 18 #include "chrono/physics/ChLink.h" 19 #include "chrono/physics/ChLinkMask.h" 20 21 namespace chrono { 22 23 /// Base class for all 'simple' constraints between 24 /// two frames attached to two bodies. These constraints 25 /// can correspond to the typical 'mating' conditions that 26 /// are created in assemblies of 3D CAD tools (parallel 27 /// axis, or face-to-face, etc.). 28 /// Note that most of the ChLinkMate constraints can be 29 /// done also with the constraints inherited from ChLinkLock... 30 /// but in case of links of the ChLinkLock class they 31 /// reference two ChMarker objects, tht can also move, but 32 /// this is could be an unnecessary complication in most cases. 33 34 class ChApi ChLinkMate : public ChLink { 35 public: ChLinkMate()36 ChLinkMate() {} ChLinkMate(const ChLinkMate & other)37 ChLinkMate(const ChLinkMate& other) : ChLink(other) {} ~ChLinkMate()38 virtual ~ChLinkMate() {} 39 40 /// "Virtual" copy constructor (covariant return type). Clone()41 virtual ChLinkMate* Clone() const override { return new ChLinkMate(*this); } 42 43 /// Method to allow serialization of transient data to archives. 44 virtual void ArchiveOUT(ChArchiveOut& marchive) override; 45 46 /// Method to allow deserialization of transient data from archives. 47 virtual void ArchiveIN(ChArchiveIn& marchive) override; 48 }; 49 50 CH_CLASS_VERSION(ChLinkMate, 0) 51 52 // ----------------------------------------------------------------------------- 53 54 /// Generic mate constraint, where one can select which DOFs must be constrained 55 /// between two frames attached to the two bodies. 56 57 class ChApi ChLinkMateGeneric : public ChLinkMate { 58 public: 59 using ChConstraintVectorX = Eigen::Matrix<double, Eigen::Dynamic, 1, Eigen::ColMajor, 6, 1>; 60 61 ChLinkMateGeneric(bool mc_x = true, 62 bool mc_y = true, 63 bool mc_z = true, 64 bool mc_rx = true, 65 bool mc_ry = true, 66 bool mc_rz = true); 67 ChLinkMateGeneric(const ChLinkMateGeneric& other); 68 virtual ~ChLinkMateGeneric(); 69 70 /// "Virtual" copy constructor (covariant return type). Clone()71 virtual ChLinkMateGeneric* Clone() const override { return new ChLinkMateGeneric(*this); } 72 73 /// Get the link coordinate system, expressed relative to Body2 (the 'master' 74 /// body). This represents the 'main' reference of the link: reaction forces 75 /// are expressed in this coordinate system. 76 /// (It is the coordinate system of the contact plane relative to Body2) GetLinkRelativeCoords()77 virtual ChCoordsys<> GetLinkRelativeCoords() override { return frame2.GetCoord(); } 78 79 /// Get the master coordinate system for the assets (this will return the 80 /// absolute coordinate system of the 'master' marker2) 81 virtual ChFrame<> GetAssetsFrame(unsigned int nclone = 0) override { return frame2 >> *GetBody2(); } 82 83 /// Access the coordinate system considered attached to body1. 84 /// Its position is expressed in the coordinate system of body1. GetFrame1()85 ChFrame<>& GetFrame1() { return frame1; } 86 87 /// Access the coordinate system considered attached to body2. 88 /// Its position is expressed in the coordinate system of body2. GetFrame2()89 ChFrame<>& GetFrame2() { return frame2; } 90 IsConstrainedX()91 bool IsConstrainedX() const { return c_x; } IsConstrainedY()92 bool IsConstrainedY() const { return c_y; } IsConstrainedZ()93 bool IsConstrainedZ() const { return c_z; } IsConstrainedRx()94 bool IsConstrainedRx() const { return c_rx; } IsConstrainedRy()95 bool IsConstrainedRy() const { return c_ry; } IsConstrainedRz()96 bool IsConstrainedRz() const { return c_rz; } 97 98 /// Sets which movements (of frame 1 respect to frame 2) are constrained 99 void SetConstrainedCoords(bool mc_x, bool mc_y, bool mc_z, bool mc_rx, bool mc_ry, bool mc_rz); 100 101 /// Initialize the generic mate, given the two bodies to be connected, and the absolute position of 102 /// the mate (the two frames to connect on the bodies will be initially coincindent to that frame). 103 virtual void Initialize(std::shared_ptr<ChBodyFrame> mbody1, ///< first body to link 104 std::shared_ptr<ChBodyFrame> mbody2, ///< second body to link 105 ChFrame<> mabsframe ///< mate frame, in abs. coordinate 106 ); 107 108 /// Initialize the generic mate, given the two bodies to be connected, the positions of the two frames 109 /// to connect on the bodies (each expressed in body or abs. coordinates). 110 virtual void Initialize(std::shared_ptr<ChBodyFrame> mbody1, ///< first body to link 111 std::shared_ptr<ChBodyFrame> mbody2, ///< second body to link 112 bool pos_are_relative, ///< true: following pos. are relative to bodies 113 ChFrame<> mframe1, ///< slave frame 1 (rel. or abs.) 114 ChFrame<> mframe2 ///< master frame 2 (rel. or abs.) 115 ); 116 117 /// Initialization based on passing two vectors (point + dir) on the two bodies, which will represent the X axes of 118 /// the two frames (Y and Z will be built from the X vector via Gram Schmidt orthonormalization). 119 virtual void Initialize(std::shared_ptr<ChBodyFrame> mbody1, ///< first body to link 120 std::shared_ptr<ChBodyFrame> mbody2, ///< second body to link 121 bool pos_are_relative, ///< true: following pos. are relative to bodies 122 ChVector<> mpt1, ///< origin of slave frame 1 (rel. or abs.) 123 ChVector<> mpt2, ///< origin of master frame 2 (rel. or abs.) 124 ChVector<> mnorm1, ///< X axis of slave plane 1 (rel. or abs.) 125 ChVector<> mnorm2 ///< X axis of master plane 2 (rel. or abs.) 126 ); 127 128 // 129 // UPDATING FUNCTIONS 130 // 131 132 /// Override _all_ time, jacobian etc. updating. 133 virtual void Update(double mtime, bool update_assets = true) override; 134 135 /// If some constraint is redundant, return to normal state 136 virtual int RestoreRedundant() override; 137 138 /// User can use this to enable/disable all the constraint of 139 /// the link as desired. 140 virtual void SetDisabled(bool mdis) override; 141 142 /// Ex:3rd party software can set the 'broken' status via this method 143 virtual void SetBroken(bool mon) override; 144 GetDOC()145 virtual int GetDOC() override { return ndoc; } GetDOC_c()146 virtual int GetDOC_c() override { return ndoc_c; } GetDOC_d()147 virtual int GetDOC_d() override { return ndoc_d; } 148 149 // LINK VIOLATIONS 150 // Get the constraint violations, i.e. the residual of the constraint equations and their time derivatives (TODO) 151 152 /// Link violation (residuals of the link constraint equations). GetConstraintViolation()153 virtual ChVectorDynamic<> GetConstraintViolation() const override { return C; } 154 155 // 156 // STATE FUNCTIONS 157 // 158 159 // (override/implement interfaces for global state vectors, see ChPhysicsItem for comments.) 160 virtual void IntStateGatherReactions(const unsigned int off_L, ChVectorDynamic<>& L) override; 161 virtual void IntStateScatterReactions(const unsigned int off_L, const ChVectorDynamic<>& L) override; 162 virtual void IntLoadResidual_CqL(const unsigned int off_L, 163 ChVectorDynamic<>& R, 164 const ChVectorDynamic<>& L, 165 const double c) override; 166 virtual void IntLoadConstraint_C(const unsigned int off, 167 ChVectorDynamic<>& Qc, 168 const double c, 169 bool do_clamp, 170 double recovery_clamp) override; 171 virtual void IntLoadConstraint_Ct(const unsigned int off, ChVectorDynamic<>& Qc, const double c) override; 172 virtual void IntToDescriptor(const unsigned int off_v, 173 const ChStateDelta& v, 174 const ChVectorDynamic<>& R, 175 const unsigned int off_L, 176 const ChVectorDynamic<>& L, 177 const ChVectorDynamic<>& Qc) override; 178 virtual void IntFromDescriptor(const unsigned int off_v, 179 ChStateDelta& v, 180 const unsigned int off_L, 181 ChVectorDynamic<>& L) override; 182 183 // 184 // SOLVER INTERFACE 185 // 186 187 virtual void InjectConstraints(ChSystemDescriptor& mdescriptor) override; 188 virtual void ConstraintsBiReset() override; 189 virtual void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false) override; 190 virtual void ConstraintsBiLoad_Ct(double factor = 1) override; 191 virtual void ConstraintsLoadJacobians() override; 192 virtual void ConstraintsFetch_react(double factor = 1) override; 193 194 // 195 // SERIALIZATION 196 // 197 198 /// Method to allow serialization of transient data to archives. 199 virtual void ArchiveOUT(ChArchiveOut& marchive) override; 200 201 /// Method to allow deserialization of transient data from archives. 202 virtual void ArchiveIN(ChArchiveIn& marchive) override; 203 204 protected: 205 void SetupLinkMask(); 206 void ChangedLinkMask(); 207 208 ChFrame<> frame1; 209 ChFrame<> frame2; 210 211 bool c_x; 212 bool c_y; 213 bool c_z; 214 bool c_rx; 215 bool c_ry; 216 bool c_rz; 217 218 int ndoc; ///< number of DOC, degrees of constraint 219 int ndoc_c; ///< number of DOC, degrees of constraint (only bilaterals) 220 int ndoc_d; ///< number of DOC, degrees of constraint (only unilaterals) 221 222 ChLinkMask mask; 223 224 ChConstraintVectorX C; ///< residuals 225 }; 226 227 CH_CLASS_VERSION(ChLinkMateGeneric, 0) 228 229 // ----------------------------------------------------------------------------- 230 231 /// Mate constraint of plane-to-plane type. This correspond to the 232 /// typical planar face vs planar face mating used in 3D CAD assemblies. 233 /// The planes are defined by the Y and Z axes of the two frames. 234 235 class ChApi ChLinkMatePlane : public ChLinkMateGeneric { 236 protected: 237 bool flipped; 238 double separation; 239 240 public: ChLinkMatePlane()241 ChLinkMatePlane() : ChLinkMateGeneric(true, false, false, false, true, true), flipped(false), separation(0) {} 242 ChLinkMatePlane(const ChLinkMatePlane& other); ~ChLinkMatePlane()243 ~ChLinkMatePlane() {} 244 245 /// "Virtual" copy constructor (covariant return type). Clone()246 virtual ChLinkMatePlane* Clone() const override { return new ChLinkMatePlane(*this); } 247 248 /// Tell if the two normals must be opposed (flipped=false) or must have the same verse (flipped=true) 249 void SetFlipped(bool doflip); IsFlipped()250 bool IsFlipped() const { return flipped; } 251 252 /// Set the distance between the two planes, in normal direction SetSeparation(double msep)253 void SetSeparation(double msep) { separation = msep; } 254 /// Get the requested distance between the two planes, in normal direction GetSeparation()255 double GetSeparation() const { return separation; } 256 257 /// Specialized initialization for plane-plane mate, given the two bodies to be connected, two points on the two 258 /// faces, two normals on the faces (each expressed in body or abs. coordinates). 259 virtual void Initialize(std::shared_ptr<ChBodyFrame> mbody1, ///< first body to link 260 std::shared_ptr<ChBodyFrame> mbody2, ///< second body to link 261 bool pos_are_relative, ///< true: following pos. are relative to bodies 262 ChVector<> mpt1, ///< point on slave plane 1 (rel. or abs.) 263 ChVector<> mpt2, ///< point on master plane 2 (rel. or abs.) 264 ChVector<> mnorm1, ///< normal of slave plane 1 (rel. or abs.) 265 ChVector<> mnorm2 ///< normal of master plane 2 (rel. or abs.) 266 ) override; 267 268 /// Override _all_ time, jacobian etc. updating, inheriting parent but also adding the effect of separation 269 virtual void Update(double mtime, bool update_assets = true) override; 270 271 /// Method to allow serialization of transient data to archives. 272 virtual void ArchiveOUT(ChArchiveOut& marchive) override; 273 274 /// Method to allow deserialization of transient data from archives. 275 virtual void ArchiveIN(ChArchiveIn& marchive) override; 276 }; 277 278 CH_CLASS_VERSION(ChLinkMatePlane, 0) 279 280 // ----------------------------------------------------------------------------- 281 282 /// Mate constraint of coaxial type. This correspond to the 283 /// typical cylinder-vs-cylinder mating used in 3D CAD assemblies. 284 /// The two coaxial axes are the X axes of the two frames. 285 286 class ChApi ChLinkMateCoaxial : public ChLinkMateGeneric { 287 protected: 288 bool flipped; 289 290 public: ChLinkMateCoaxial()291 ChLinkMateCoaxial() : ChLinkMateGeneric(false, true, true, false, true, true), flipped(false) {} 292 ChLinkMateCoaxial(const ChLinkMateCoaxial& other); ~ChLinkMateCoaxial()293 virtual ~ChLinkMateCoaxial() {} 294 295 /// "Virtual" copy constructor (covariant return type). Clone()296 virtual ChLinkMateCoaxial* Clone() const override { return new ChLinkMateCoaxial(*this); } 297 298 /// Tell if the two axes must be opposed (flipped=false) or must have the same verse (flipped=true) 299 void SetFlipped(bool doflip); IsFlipped()300 bool IsFlipped() const { return flipped; } 301 302 /// Specialized initialization for coaxial mate, given the two bodies to be connected, two points, two directions 303 /// (each expressed in body or abs. coordinates). 304 virtual void Initialize(std::shared_ptr<ChBodyFrame> mbody1, ///< first body to link 305 std::shared_ptr<ChBodyFrame> mbody2, ///< second body to link 306 bool pos_are_relative, ///< true: following pos. are relative to bodies 307 ChVector<> mpt1, ///< point on slave axis 1 (rel. or abs.) 308 ChVector<> mpt2, ///< point on master axis 2 (rel. or abs.) 309 ChVector<> mdir1, ///< direction of slave axis 1 (rel. or abs.) 310 ChVector<> mdir2 ///< direction of master axis 2 (rel. or abs.) 311 ) override; 312 313 /// Method to allow serialization of transient data to archives. 314 virtual void ArchiveOUT(ChArchiveOut& marchive) override; 315 316 /// Method to allow deserialization of transient data from archives. 317 virtual void ArchiveIN(ChArchiveIn& marchive) override; 318 }; 319 320 CH_CLASS_VERSION(ChLinkMateCoaxial, 0) 321 322 // ----------------------------------------------------------------------------- 323 324 /// Mate constraint of spherical type. This correspond to the 325 /// typical point-on-point or spherical joint mating used in 3D CAD assemblies. 326 327 class ChApi ChLinkMateSpherical : public ChLinkMateGeneric { 328 public: ChLinkMateSpherical()329 ChLinkMateSpherical() : ChLinkMateGeneric(true, true, true, false, false, false) {} 330 ChLinkMateSpherical(const ChLinkMateSpherical& other); ~ChLinkMateSpherical()331 virtual ~ChLinkMateSpherical() {} 332 333 /// "Virtual" copy constructor (covariant return type). Clone()334 virtual ChLinkMateSpherical* Clone() const override { return new ChLinkMateSpherical(*this); } 335 336 using ChLinkMateGeneric::Initialize; 337 338 /// Specialized initialization for coincident mate, given the two bodies to be connected, and two points 339 /// (each expressed in body or abs. coordinates). 340 void Initialize(std::shared_ptr<ChBodyFrame> mbody1, ///< first body to link 341 std::shared_ptr<ChBodyFrame> mbody2, ///< second body to link 342 bool pos_are_relative, ///< true: following pos. are relative to bodies. 343 ChVector<> mpt1, ///< point slave 1 (rel. or abs.) 344 ChVector<> mpt2 ///< point master 2 (rel. or abs.) 345 ); 346 }; 347 348 CH_CLASS_VERSION(ChLinkMateSpherical, 0) 349 350 // ----------------------------------------------------------------------------- 351 352 /// Mate constraining distance of origin of frame B respect to X axis of frame A. 353 354 class ChApi ChLinkMateXdistance : public ChLinkMateGeneric { 355 protected: 356 double distance; 357 358 public: ChLinkMateXdistance()359 ChLinkMateXdistance() : ChLinkMateGeneric(true, false, false, false, false, false), distance(0) {} 360 ChLinkMateXdistance(const ChLinkMateXdistance& other); ~ChLinkMateXdistance()361 virtual ~ChLinkMateXdistance() {} 362 363 /// "Virtual" copy constructor (covariant return type). Clone()364 virtual ChLinkMateXdistance* Clone() const override { return new ChLinkMateXdistance(*this); } 365 366 /// Set the distance on X of frame 2 SetDistance(double msep)367 void SetDistance(double msep) { distance = msep; } 368 /// Get the requested distance on X of frame 2 GetDistance()369 double GetDistance() const { return distance; } 370 371 using ChLinkMateGeneric::Initialize; 372 373 /// Specialized initialization for X distance mate, given the two bodies to be connected, and two points 374 /// (each expressed in body or abs. coordinates). 375 void Initialize(std::shared_ptr<ChBodyFrame> mbody1, ///< first body to link 376 std::shared_ptr<ChBodyFrame> mbody2, ///< second body to link 377 bool pos_are_relative, ///< true: following pos. are relative to bodies 378 ChVector<> mpt1, ///< point slave 1 (rel. or abs.) 379 ChVector<> mpt2, ///< point master 2 (rel. or abs.) 380 ChVector<> mdir2 ///< direction of master axis 2 (rel. or abs.) 381 ); 382 383 /// Override _all_ time, jacobian etc. updating, inheriting parent but also adding the effect of separation 384 virtual void Update(double mtime, bool update_assets = true) override; 385 386 /// Method to allow serialization of transient data to archives. 387 virtual void ArchiveOUT(ChArchiveOut& marchive) override; 388 389 /// Method to allow deserialization of transient data from archives. 390 virtual void ArchiveIN(ChArchiveIn& marchive) override; 391 }; 392 393 CH_CLASS_VERSION(ChLinkMateXdistance, 0) 394 395 // ----------------------------------------------------------------------------- 396 397 /// Mate constraint of parallel type. This correspond to the 398 /// typical axis-is-parallel-to-axis (or edge to edge, etc.) mating 399 /// used in 3D CAD assemblies. The axes to be kept parallel are 400 /// the two X axes of the two frames. 401 402 class ChApi ChLinkMateParallel : public ChLinkMateGeneric { 403 protected: 404 bool flipped; 405 406 public: ChLinkMateParallel()407 ChLinkMateParallel() : ChLinkMateGeneric(false, false, false, false, true, true), flipped(false) {} 408 ChLinkMateParallel(const ChLinkMateParallel& other); ~ChLinkMateParallel()409 virtual ~ChLinkMateParallel() {} 410 411 /// "Virtual" copy constructor (covariant return type). Clone()412 virtual ChLinkMateParallel* Clone() const override { return new ChLinkMateParallel(*this); } 413 414 /// Tell if the two axes must be opposed (flipped=false) or must have the same verse (flipped=true) 415 void SetFlipped(bool doflip); IsFlipped()416 bool IsFlipped() const { return flipped; } 417 418 /// Specialized initialization for parallel mate, given the two bodies to be connected, two points and two 419 /// directions (each expressed in body or abs. coordinates). 420 virtual void Initialize(std::shared_ptr<ChBodyFrame> mbody1, ///< first body to link 421 std::shared_ptr<ChBodyFrame> mbody2, ///< second body to link 422 bool pos_are_relative, ///< true: following pos. are relative to bodies 423 ChVector<> mpt1, ///< point on slave axis 1 (rel. or abs.) 424 ChVector<> mpt2, ///< point on master axis 2 (rel. or abs.) 425 ChVector<> mdir1, ///< direction of slave axis 1 (rel. or abs.) 426 ChVector<> mdir2 ///< direction of master axis 2 (rel. or abs.) 427 ) override; 428 429 /// Method to allow serialization of transient data to archives. 430 virtual void ArchiveOUT(ChArchiveOut& marchive) override; 431 432 /// Method to allow deserialization of transient data from archives. 433 virtual void ArchiveIN(ChArchiveIn& marchive) override; 434 }; 435 436 CH_CLASS_VERSION(ChLinkMateParallel, 0) 437 438 // ----------------------------------------------------------------------------- 439 440 /// Mate constraint of orthogonal type. This correspond to the 441 /// typical axis-is-orthogonal-to-axis (or edge to edge, etc.) mating 442 /// used in 3D CAD assemblies. The the two X axes of the two frames 443 /// are aligned to the cross product of the two directions. 444 445 class ChApi ChLinkMateOrthogonal : public ChLinkMateGeneric { 446 protected: 447 ChVector<> reldir1; 448 ChVector<> reldir2; 449 450 public: ChLinkMateOrthogonal()451 ChLinkMateOrthogonal() 452 : ChLinkMateGeneric(false, false, false, true, false, false), reldir1(VECT_X), reldir2(VECT_Y) {} 453 ChLinkMateOrthogonal(const ChLinkMateOrthogonal& other); ~ChLinkMateOrthogonal()454 virtual ~ChLinkMateOrthogonal() {} 455 456 /// "Virtual" copy constructor (covariant return type). Clone()457 virtual ChLinkMateOrthogonal* Clone() const override { return new ChLinkMateOrthogonal(*this); } 458 459 /// Specialized initialization for orthogonal mate, given the two bodies to be connected, two points and two 460 /// directions (each expressed in body or abs. coordinates). 461 virtual void Initialize(std::shared_ptr<ChBodyFrame> mbody1, ///< first body to link 462 std::shared_ptr<ChBodyFrame> mbody2, ///< second body to link 463 bool pos_are_relative, ///< true: following pos. are relative to bodies 464 ChVector<> mpt1, ///< point on slave axis 1 (rel. or abs.) 465 ChVector<> mpt2, ///< point on master axis 2 (rel. or abs.) 466 ChVector<> mdir1, ///< direction of slave axis 1 (rel. or abs.) 467 ChVector<> mdir2 ///< direction of master axis 2 (rel. or abs.) 468 ) override; 469 470 /// Override _all_ time, jacobian etc. updating, inheriting parent but also adding the effect of separation 471 virtual void Update(double mtime, bool update_assets = true) override; 472 473 /// Method to allow serialization of transient data to archives. 474 virtual void ArchiveOUT(ChArchiveOut& marchive) override; 475 476 /// Method to allow deserialization of transient data from archives. 477 virtual void ArchiveIN(ChArchiveIn& marchive) override; 478 }; 479 480 CH_CLASS_VERSION(ChLinkMateOrthogonal, 0) 481 482 // ----------------------------------------------------------------------------- 483 484 /// Mate constraint that completely fix one frame's rotation and translation 485 /// respect to the other frame. 486 487 class ChApi ChLinkMateFix : public ChLinkMateGeneric { 488 public: ChLinkMateFix()489 ChLinkMateFix() : ChLinkMateGeneric(true, true, true, true, true, true) {} 490 ChLinkMateFix(const ChLinkMateFix& other); ~ChLinkMateFix()491 virtual ~ChLinkMateFix() {} 492 493 /// "Virtual" copy constructor (covariant return type). Clone()494 virtual ChLinkMateFix* Clone() const override { return new ChLinkMateFix(*this); } 495 496 using ChLinkMateGeneric::Initialize; 497 498 /// Specialized initialization for "fix" mate, given the two bodies to be connected, the positions of the two 499 /// auxiliary frames where the two bodies are connected are both automatically initialized as the current absolute 500 /// position of mbody1. 501 void Initialize(std::shared_ptr<ChBodyFrame> mbody1, ///< first body to link 502 std::shared_ptr<ChBodyFrame> mbody2 ///< second body to link 503 ); 504 }; 505 506 CH_CLASS_VERSION(ChLinkMateFix, 0) 507 508 } // end namespace chrono 509 510 #endif 511