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 CHLINKMARKERS_H 16 #define CHLINKMARKERS_H 17 18 #include "chrono/physics/ChBody.h" 19 #include "chrono/physics/ChLink.h" 20 #include "chrono/physics/ChMarker.h" 21 22 namespace chrono { 23 24 /// Class for links which connect two 'markers'. The markers are two ChMarker objects each belonging to the two linked 25 /// ChBody parts. Many specialized classes are based on this ChLinkMarkers class, for example the family of ChLinkLock 26 /// classes. ChLinkMarkers class allows an optional force vector and torque vector to be set between the two connected 27 /// markers. 28 class ChApi ChLinkMarkers : public ChLink { 29 protected: 30 // Protected constructors. 31 ChLinkMarkers(); 32 ChLinkMarkers(const ChLinkMarkers& other); 33 34 ChMarker* marker1; ///< slave coordsys 35 ChMarker* marker2; ///< master coordsys, =0 if liked to ground 36 37 int markID1; ///< unique identifier for markers 1 & 2, 38 int markID2; ///< when using plugin dynamic hierarchies 39 40 Coordsys relM; ///< relative marker position 2-1 41 Coordsys relM_dt; ///< relative marker speed 42 Coordsys relM_dtdt; ///< relative marker acceleration 43 44 double relAngle; ///< relative angle of rotation 45 ChVector<> relAxis; ///< relative axis of rotation 46 ChVector<> relRotaxis; ///< relative rotaion vector =angle*axis 47 ChVector<> relWvel; ///< relative angular speed 48 ChVector<> relWacc; ///< relative angular acceleration 49 double dist; ///< the distance between the two origins of markers, 50 double dist_dt; ///< the speed between the two origins of markers 51 52 ChVector<> C_force; ///< internal force applied by springs/dampers/actuators 53 ChVector<> C_torque; ///< internal torque applied by springs/dampers/actuators 54 55 // Cached intermediate variables. 56 // These are calculated in UpdateRelMarkerCoords and may be reused in UpdateState. 57 ChVector<> PQw; 58 ChVector<> PQw_dt; 59 ChVector<> PQw_dtdt; 60 ChQuaternion<> q_AD; 61 ChQuaternion<> q_BC; 62 ChQuaternion<> q_8; 63 ChVector<> q_4; 64 65 public: ~ChLinkMarkers()66 virtual ~ChLinkMarkers() {} 67 68 /// "Virtual" copy constructor (covariant return type). Clone()69 virtual ChLinkMarkers* Clone() const override { return new ChLinkMarkers(*this); } 70 71 /// Return the 1st referenced marker (the 'slave' marker, owned by 1st body) GetMarker1()72 ChMarker* GetMarker1() { return marker1; } 73 /// Return the 2nd referenced marker (the 'master' marker, owned by 2nd body) GetMarker2()74 ChMarker* GetMarker2() { return marker2; } 75 /// set the two markers associated with this link 76 virtual void SetUpMarkers(ChMarker* mark1, ChMarker* mark2); SetMarkID1(int mid)77 void SetMarkID1(int mid) { markID1 = mid; } SetMarkID2(int mid)78 void SetMarkID2(int mid) { markID2 = mid; } GetMarkID1()79 int GetMarkID1() { return markID1; } GetMarkID2()80 int GetMarkID2() { return markID2; } 81 82 /// Shortcut: set markers and marker IDs at once. 83 bool ReferenceMarkers(ChMarker* mark1, ChMarker* mark2); 84 85 /// Use this function after link creation, to initialize the link from 86 /// two markers to join. 87 /// Each marker must belong to a rigid body, and both rigid bodies 88 /// must belong to the same ChSystem. 89 /// The position of mark2 is used as link's position and main reference. 90 virtual void Initialize(std::shared_ptr<ChMarker> mark1, ///< first marker to join 91 std::shared_ptr<ChMarker> mark2 ///< second marker to join (master) 92 ); 93 94 /// Use this function after link creation, to initialize the link from 95 /// two joined rigid bodies. 96 /// Both rigid bodies must belong to the same ChSystem. 97 /// Two markers will be created and added to the rigid bodies (later, 98 /// you can use GetMarker1() and GetMarker2() to access them. 99 /// To specify the (absolute) position of link and markers, use 'mpos'. 100 virtual void Initialize(std::shared_ptr<ChBody> mbody1, ///< first body to join 101 std::shared_ptr<ChBody> mbody2, ///< second body to join 102 const ChCoordsys<>& mpos ///< the current absolute pos.& alignment. 103 ); 104 105 /// Use this function after link creation, to initialize the link from 106 /// two joined rigid bodies. 107 /// Both rigid bodies must belong to the same ChSystem. 108 /// Two markers will be created and added to the rigid bodies (later, 109 /// you can use GetMarker1() and GetMarker2() to access them. 110 /// To specify the (absolute) position of link and markers, use 'mpos'. 111 virtual void Initialize( 112 std::shared_ptr<ChBody> mbody1, ///< first body to join 113 std::shared_ptr<ChBody> mbody2, ///< second body to join 114 bool pos_are_relative, ///< if =true, following two positions are relative to bodies. If false, are absolute. 115 const ChCoordsys<>& mpos1, ///< the position & alignment of 1st marker (relative to body1 cords, or absolute) 116 const ChCoordsys<>& mpos2 ///< the position & alignment of 2nd marker (relative to body2 cords, or absolute) 117 ); 118 119 /// Get the link coordinate system, expressed relative to Body2 (the 'master' 120 /// body). This represents the 'main' reference of the link: reaction forces 121 /// and torques are expressed in this coordinate system. 122 /// (It is the coordinate system of the 'master' marker2 relative to Body2) GetLinkRelativeCoords()123 virtual ChCoordsys<> GetLinkRelativeCoords() override { return marker2->GetCoord(); } 124 125 /// Get the master coordinate system for the assets (this will return the 126 /// absolute coordinate system of the 'master' marker2) 127 virtual ChFrame<> GetAssetsFrame(unsigned int nclone = 0) override { return marker2->GetAbsFrame(); } 128 129 // 130 // UPDATING FUNCTIONS 131 // 132 133 /// Updates auxiliary quantities for all relative degrees of freedom of the two markers. 134 virtual void UpdateRelMarkerCoords(); 135 136 /// Updates auxiliary forces caused by springs/dampers/etc. which may 137 /// be connected between the two bodies of the link. 138 /// By default, it adds the forces which might have been added by the 139 /// user using Set_Scr_force() and Set_Scr_torque(). Note, these forces 140 /// are considered in the reference coordsystem of marker2 (the MAIN marker), 141 /// and their application point is the origin of marker1 (the SLAVE marker). 142 virtual void UpdateForces(double mytime); 143 144 /// Complete link update: UpdateTime -> UpdateRelMarkerCoords -> UpdateForces. 145 virtual void Update(double mytime, bool update_assets = true) override; 146 147 // 148 // STATE FUNCTIONS 149 // 150 151 /// Adds force to residual R, as R*= F*c 152 /// NOTE: here the off offset in R is NOT used because add F at the TWO offsets of the two connected bodies, 153 /// so it is assumed that offsets for Body1 and Body2 variables have been already set properly! 154 virtual void IntLoadResidual_F(const unsigned int off, ChVectorDynamic<>& R, const double c) override; 155 156 // 157 // SOLVER INTERFACE 158 // 159 160 /// Overrides the empty behaviour of the parent ChLink implementation, which 161 /// does not consider any user-imposed force between the two bodies. 162 /// It adds the current link-forces, if any, (caused by springs, etc.) to the 'fb' vectors 163 /// of the ChVariables referenced by encapsulated ChConstraints. 164 /// In details, it adds the effect caused by C_force and C_torque. 165 /// Both C_force and C_torque these forces are considered expressed in the 166 /// reference coordsystem of marker2 (the MAIN marker), 167 /// and their application point is the origin of marker1 (the SLAVE marker). 168 virtual void ConstraintsFbLoadForces(double factor = 1) override; 169 170 // 171 // LINK COORDINATES and other functions: 172 // 173 174 /// Relative position of marker 1 respect to marker 2. GetRelM()175 const Coordsys& GetRelM() const { return relM; } 176 /// Relative speed of marker 1 respect to marker 2. GetRelM_dt()177 const Coordsys& GetRelM_dt() const { return relM_dt; } 178 /// Relative acceleration of marker 1 respect to marker 2. GetRelM_dtdt()179 const Coordsys& GetRelM_dtdt() const { return relM_dtdt; } 180 /// Relative rotation angle of marker 1 respect to marker 2 (best with revolute joints..). GetRelAngle()181 double GetRelAngle() const { return relAngle; } 182 /// Relative finite rotation axis of marker 1 respect to marker 2. GetRelAxis()183 const ChVector<>& GetRelAxis() const { return relAxis; } GetRelRotaxis()184 const ChVector<>& GetRelRotaxis() const { return relRotaxis; } 185 /// Relative angular speed of marker 1 respect to marker 2. GetRelWvel()186 const ChVector<>& GetRelWvel() const { return relWvel; } 187 /// Relative angular acceleration of marker 1 respect to marker 2. GetRelWacc()188 const ChVector<>& GetRelWacc() const { return relWacc; } 189 /// Relative 'polar' distance of marker 1 respect to marker 2. GetDist()190 double GetDist() const { return dist; } 191 /// Relative speed of marker 1 respect to marker 2, along the polar distance vector. GetDist_dt()192 double GetDist_dt() const { return dist_dt; } 193 194 /// Get the total applied force accumulators (force, momentum) in link coords. 195 /// These forces might be affected by additional springs, dampers, etc. but they do not 196 /// include the reaction forces. GetC_force()197 const ChVector<>& GetC_force() const { return C_force; } GetC_torque()198 const ChVector<>& GetC_torque() const { return C_torque; } 199 200 // 201 // SERIALIZATION 202 // 203 204 /// Method to allow serialization of transient data to archives. 205 virtual void ArchiveOUT(ChArchiveOut& marchive) override; 206 207 /// Method to allow deserialization of transient data from archives. 208 virtual void ArchiveIN(ChArchiveIn& marchive) override; 209 }; 210 211 CH_CLASS_VERSION(ChLinkMarkers, 0) 212 213 } // end namespace chrono 214 215 #endif 216