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 #include "chrono/physics/ChLinkScrew.h"
16
17 namespace chrono {
18
19 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChLinkScrew)20 CH_FACTORY_REGISTER(ChLinkScrew)
21
22 ChLinkScrew::ChLinkScrew() {
23 Set_thread(0.05);
24
25 // Mask: initialize our LinkMaskLF (lock formulation mask) to X,Y,Z,Rx Ry,
26 // (note: the Z lock is not a standard LinkLock z-lock and will be handled as a custom screw constraint
27 // z = tau*alpha, later in the updating functions).
28 mask.SetLockMask(true, true, true, false, true, true, false);
29 BuildLink();
30 }
31
ChLinkScrew(const ChLinkScrew & other)32 ChLinkScrew::ChLinkScrew(const ChLinkScrew& other) : ChLinkLock(other) {
33 tau = other.tau;
34 }
35
UpdateState()36 void ChLinkScrew::UpdateState() {
37 // First, compute everything as it were a normal "revolute" joint, on z axis...
38 ChLinkLock::UpdateState();
39
40 // Then, MODIFY the Z part of equations, such that the Z = 0 becomes Z = tau * alpha
41 double scr_C, scr_C_dt, scr_C_dtdt;
42 double scr_Ct, scr_Qc;
43 double coeffa, coeffb, zangle, msign;
44 ChMatrixNM<double, 1, 7> scr_Cq1;
45 ChMatrixNM<double, 1, 7> scr_Cq2;
46 double Crz;
47
48 if (fabs(relM.rot.e0()) < 0.707) {
49 Crz = relM.rot.e0(); // cos(alpha/2)
50 msign = +1;
51 zangle = acos(Crz);
52 if (relM.rot.e3() < 0) {
53 zangle = -zangle; // a/2 = -acos(Crz);
54 msign = -1;
55 }
56 double mrelz = relM.pos.z();
57
58 scr_C = mrelz - tau * 2.0 * zangle;
59 // modulus correction..
60 scr_C = scr_C - Get_thread() * floor(scr_C / Get_thread());
61 double shiftedC = scr_C - Get_thread() * ceil(scr_C / Get_thread());
62 if (fabs(scr_C) > fabs(shiftedC))
63 scr_C = shiftedC;
64
65 coeffa = +2.0 * tau * msign * 1 / (sqrt(1 - pow(Crz, 2.0)));
66 coeffb = +2.0 * tau * msign * Crz / (pow((1 - pow(Crz, 2)), 3.0 / 2.0));
67
68 scr_C_dt = relM_dt.pos.z() + relM_dt.rot.e0() * coeffa;
69 scr_C_dtdt = relM_dtdt.pos.z() + relM_dt.rot.e0() * coeffb + relM_dtdt.rot.e0() * coeffa;
70 scr_Ct = Ct_temp.pos.z() + coeffa * Ct_temp.rot.e0();
71 scr_Qc = Qc_temp(2) + coeffa * Qc_temp(3) - relM_dt.rot.e0() * coeffb;
72 scr_Cq1.setZero();
73 scr_Cq2.setZero();
74 scr_Cq1.block(0, 3, 1, 4) = coeffa * Cq1_temp.block(3, 3, 1, 4);
75 scr_Cq2.block(0, 3, 1, 4) = coeffa * Cq2_temp.block(3, 3, 1, 4);
76 } else {
77 Crz = relM.rot.e3(); // Zz*sin(alpha/2)
78 msign = +1;
79 zangle = asin(Crz);
80 if (relM.rot.e0() < 0) {
81 zangle = CH_C_PI - zangle;
82 msign = -1;
83 }
84 double mrelz = relM.pos.z(); // fmod (relM.pos.z() , (tau * 2 * CH_C_PI));
85
86 scr_C = mrelz - tau * 2.0 * zangle;
87 // modulus correction..
88 scr_C = scr_C - Get_thread() * floor(scr_C / Get_thread());
89 double shiftedC = scr_C - Get_thread() * ceil(scr_C / Get_thread());
90 if (fabs(scr_C) > fabs(shiftedC))
91 scr_C = shiftedC;
92
93 coeffa = -2.0 * tau * msign * 1 / (sqrt(1 - pow(Crz, 2.0)));
94 coeffb = -2.0 * tau * msign * Crz / (pow((1 - pow(Crz, 2)), 3.0 / 2.0));
95
96 scr_C_dt = relM_dt.pos.z() + relM_dt.rot.e3() * coeffa;
97 scr_C_dtdt = relM_dtdt.pos.z() + relM_dt.rot.e3() * coeffb + relM_dtdt.rot.e3() * coeffa;
98 scr_Ct = Ct_temp.pos.z() + coeffa * Ct_temp.rot.e3();
99 scr_Qc = Qc_temp(2) + coeffa * Qc_temp(6) - relM_dt.rot.e3() * coeffb;
100 scr_Cq1.setZero();
101 scr_Cq2.setZero();
102 scr_Cq1.block(0, 3, 1, 4) = coeffa * Cq1_temp.block(6, 3, 1, 4);
103 scr_Cq2.block(0, 3, 1, 4) = coeffa * Cq2_temp.block(6, 3, 1, 4);
104 }
105
106 Cq1.block(2, 0, 1, 7) = Cq1_temp.block(2, 0, 1, 7) + scr_Cq1;
107 Cq2.block(2, 0, 1, 7) = Cq2_temp.block(2, 0, 1, 7) + scr_Cq2;
108
109 Qc(2) = scr_Qc;
110 C(2) = scr_C;
111 C_dt(2) = scr_C_dt;
112 C_dtdt(2) = scr_C_dtdt;
113 Ct(2) = scr_Ct;
114 }
115
ArchiveOUT(ChArchiveOut & marchive)116 void ChLinkScrew::ArchiveOUT(ChArchiveOut& marchive) {
117 // version number
118 marchive.VersionWrite<ChLinkScrew>();
119
120 // serialize parent class
121 ChLinkLock::ArchiveOUT(marchive);
122
123 // serialize all member data:
124 marchive << CHNVP(tau);
125 }
126
127 /// Method to allow de serialization of transient data from archives.
ArchiveIN(ChArchiveIn & marchive)128 void ChLinkScrew::ArchiveIN(ChArchiveIn& marchive) {
129 // version number
130 /*int version =*/ marchive.VersionRead<ChLinkScrew>();
131
132 // deserialize parent class
133 ChLinkLock::ArchiveIN(marchive);
134
135 // deserialize all member data:
136 marchive >> CHNVP(tau);
137 }
138
139 } // end namespace chrono
140