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 CHLINKDISTANCE_H 16 #define CHLINKDISTANCE_H 17 18 #include "chrono/physics/ChLink.h" 19 #include "chrono/solver/ChConstraintTwoBodies.h" 20 21 namespace chrono { 22 /// Class for enforcing a fixed polar distance 23 /// between two points on two ChBodyFrame objects. 24 /// The two points which are used to define the end points 25 /// of the distance are assumed not to move respect to the 26 /// two owner ChBody, as well as the amount of the distance 27 /// is assumed not to change during the simulation. If you 28 /// need to have a time-varying distance, or distance between 29 /// two points which move respect to the bodies, please use 30 /// the more advanced ChLinkLinActuator. 31 32 class ChApi ChLinkDistance : public ChLink { 33 public: 34 ChLinkDistance(); 35 ChLinkDistance(const ChLinkDistance& other); ~ChLinkDistance()36 ~ChLinkDistance() {} 37 38 /// "Virtual" copy constructor (covariant return type). Clone()39 virtual ChLinkDistance* Clone() const override { return new ChLinkDistance(*this); } 40 41 /// Initialize this constraint, given the two bodies to be connected, the 42 /// positions of the two anchor endpoints of the distance (each expressed 43 /// in body or abs. coordinates) and the imposed distance. 44 int Initialize( 45 std::shared_ptr<ChBodyFrame> mbody1, ///< first frame to link 46 std::shared_ptr<ChBodyFrame> mbody2, ///< second frame to link 47 bool pos_are_relative, ///< true: following pos. are relative to bodies 48 ChVector<> mpos1, ///< pos. of distance endpoint, for 1st body (rel. or abs., see flag above) 49 ChVector<> mpos2, ///< pos. of distance endpoint, for 2nd body (rel. or abs., see flag above) 50 bool auto_distance = true, ///< if true, initializes the imposed distance as the distance between mpos1 and mpos2 51 double mdistance = 0 ///< imposed distance (no need to define, if auto_distance=true.) 52 ); 53 54 /// Get the number of (bilateral) constraints introduced by this link. GetDOC_c()55 virtual int GetDOC_c() override { return 1; } 56 57 /// Get the link coordinate system, expressed relative to Body2 (the 'master' 58 /// body). This represents the 'main' reference of the link: reaction forces 59 /// are expressed in this coordinate system. 60 /// (It is the coordinate system of the contact plane relative to Body2) 61 virtual ChCoordsys<> GetLinkRelativeCoords() override; 62 63 /// Get the 1st anchor endpoint for the distance (expressed in Body1 coordinate system) GetEndPoint1Rel()64 ChVector<> GetEndPoint1Rel() const { return pos1; } 65 /// Set the 1st anchor endpoint for the distance (expressed in Body1 coordinate system) SetEndPoint1Rel(const ChVector<> & mset)66 void SetEndPoint1Rel(const ChVector<>& mset) { pos1 = mset; } 67 /// Get the 1st anchor endpoint for the distance (expressed in absolute coordinate system) GetEndPoint1Abs()68 ChVector<> GetEndPoint1Abs() const { return ((ChFrame<double>*)Body1)->TransformLocalToParent(pos1); } 69 /// Set the 1st anchor endpoint for the distance (expressed in absolute coordinate system) SetEndPoint1Abs(ChVector<> & mset)70 void SetEndPoint1Abs(ChVector<>& mset) { pos1 = ((ChFrame<double>*)Body1)->TransformParentToLocal(mset); } 71 72 /// Get the 2nd anchor endpoint for the distance (expressed in Body2 coordinate system) GetEndPoint2Rel()73 ChVector<> GetEndPoint2Rel() const { return pos2; } 74 /// Set the 2nd anchor endpoint for the distance (expressed in Body2 coordinate system) SetEndPoint2Rel(const ChVector<> & mset)75 void SetEndPoint2Rel(const ChVector<>& mset) { pos2 = mset; } 76 /// Get the 1st anchor endpoint for the distance (expressed in absolute coordinate system) GetEndPoint2Abs()77 ChVector<> GetEndPoint2Abs() const { return ((ChFrame<double>*)Body2)->TransformLocalToParent(pos2); } 78 /// Set the 1st anchor endpoint for the distance (expressed in absolute coordinate system) SetEndPoint2Abs(ChVector<> & mset)79 void SetEndPoint2Abs(ChVector<>& mset) { pos2 = ((ChFrame<double>*)Body2)->TransformParentToLocal(mset); } 80 81 /// Set the imposed distance SetImposedDistance(const double mset)82 void SetImposedDistance(const double mset) { distance = mset; } 83 84 /// Get the imposed distance GetImposedDistance()85 double GetImposedDistance() const { return distance; }; 86 87 /// Get the distance currently existing between the two endpoints GetCurrentDistance()88 double GetCurrentDistance() const { return curr_dist; } 89 90 /// Get the constraint violation GetConstraintViolation()91 virtual ChVectorDynamic<> GetConstraintViolation() const override { return C; } 92 93 /// Override _all_ time, jacobian etc. updating. 94 /// In detail, it computes jacobians, violations, etc. and stores 95 /// results in inner structures. 96 virtual void Update(double mtime, bool update_assets = true) override; 97 98 // 99 // STATE FUNCTIONS 100 // 101 102 virtual void IntStateGatherReactions(const unsigned int off_L, ChVectorDynamic<>& L) override; 103 virtual void IntStateScatterReactions(const unsigned int off_L, const ChVectorDynamic<>& L) override; 104 virtual void IntLoadResidual_CqL(const unsigned int off_L, 105 ChVectorDynamic<>& R, 106 const ChVectorDynamic<>& L, 107 const double c) override; 108 virtual void IntLoadConstraint_C(const unsigned int off, 109 ChVectorDynamic<>& Qc, 110 const double c, 111 bool do_clamp, 112 double recovery_clamp) override; 113 virtual void IntToDescriptor(const unsigned int off_v, 114 const ChStateDelta& v, 115 const ChVectorDynamic<>& R, 116 const unsigned int off_L, 117 const ChVectorDynamic<>& L, 118 const ChVectorDynamic<>& Qc) override; 119 virtual void IntFromDescriptor(const unsigned int off_v, 120 ChStateDelta& v, 121 const unsigned int off_L, 122 ChVectorDynamic<>& L) override; 123 124 // 125 // SOLVER INTERFACE 126 // 127 128 virtual void InjectConstraints(ChSystemDescriptor& mdescriptor) override; 129 virtual void ConstraintsBiReset() override; 130 virtual void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false) override; 131 virtual void ConstraintsLoadJacobians() override; 132 virtual void ConstraintsFetch_react(double factor = 1) override; 133 134 // 135 // SERIALIZATION 136 // 137 138 /// Method to allow serialization of transient data to archives. 139 virtual void ArchiveOUT(ChArchiveOut& marchive) override; 140 141 /// Method to allow deserialization of transient data from archives. 142 virtual void ArchiveIN(ChArchiveIn& marchive) override; 143 144 private: 145 double distance; ///< imposed distance 146 double curr_dist; ///< current distance 147 ChVector<> pos1; ///< first endpoint, in body rel. coords 148 ChVector<> pos2; ///< second endpoint, in body rel. coords 149 ChConstraintTwoBodies Cx; ///< the constraint object 150 ChVectorN<double, 1> C; ///< constraint violation 151 }; 152 153 CH_CLASS_VERSION(ChLinkDistance,0) 154 155 156 } // end namespace chrono 157 158 #endif 159