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/core/ChGlobal.h"
16 #include "chrono/physics/ChBody.h"
17 #include "chrono/physics/ChForce.h"
18
19 namespace chrono {
20
21 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChForce)22 CH_FACTORY_REGISTER(ChForce)
23
24 ChForce::ChForce()
25 : Body(nullptr),
26 mode(FORCE),
27 frame(BODY),
28 align(BODY_DIR),
29 vpoint(VNULL),
30 vrelpoint(VNULL),
31 restpos(VNULL),
32 mforce(0),
33 vdir(VECT_X),
34 vreldir(VECT_X),
35 force(VNULL),
36 relforce(VNULL) {
37 modula = chrono_types::make_shared<ChFunction_Const>(1);
38 move_x = chrono_types::make_shared<ChFunction_Const>(0);
39 move_y = chrono_types::make_shared<ChFunction_Const>(0);
40 move_z = chrono_types::make_shared<ChFunction_Const>(0);
41 f_x = chrono_types::make_shared<ChFunction_Const>(0);
42 f_y = chrono_types::make_shared<ChFunction_Const>(0);
43 f_z = chrono_types::make_shared<ChFunction_Const>(0);
44 }
45
ChForce(const ChForce & other)46 ChForce::ChForce(const ChForce& other) : ChObj(other) {
47 Body = other.Body;
48
49 mforce = other.mforce;
50 force = other.force;
51 relforce = other.relforce;
52 vdir = other.vdir;
53 vreldir = other.vreldir;
54 vpoint = other.vpoint;
55 vrelpoint = other.vrelpoint;
56 restpos = other.restpos;
57 align = other.align;
58 frame = other.frame;
59 mode = other.mode;
60
61 ChTime = other.ChTime;
62
63 Qf = other.Qf;
64
65 modula = std::shared_ptr<ChFunction>(other.modula->Clone());
66
67 move_x = std::shared_ptr<ChFunction>(other.move_x->Clone());
68 move_y = std::shared_ptr<ChFunction>(other.move_y->Clone());
69 move_z = std::shared_ptr<ChFunction>(other.move_z->Clone());
70
71 f_x = std::shared_ptr<ChFunction>(other.f_x->Clone());
72 f_y = std::shared_ptr<ChFunction>(other.f_y->Clone());
73 f_z = std::shared_ptr<ChFunction>(other.f_z->Clone());
74 }
75
76 // Impose absolute or relative positions, also setting the correct "rest position".
SetVpoint(ChVector<> mypoint)77 void ChForce::SetVpoint(ChVector<> mypoint) {
78 // abs pos
79 vpoint = mypoint;
80 // rel pos
81 vrelpoint = GetBody()->Point_World2Body(vpoint);
82
83 // computes initial rest position.
84 ChVector<> displace = VNULL;
85 if (move_x)
86 displace.x() = move_x->Get_y(ChTime);
87 if (move_y)
88 displace.y() = move_y->Get_y(ChTime);
89 if (move_z)
90 displace.z() = move_z->Get_y(ChTime);
91
92 switch (frame) {
93 case WORLD:
94 restpos = Vsub(vpoint, displace);
95 break;
96 case BODY:
97 restpos = Vsub(vrelpoint, displace);
98 break;
99 }
100 }
101
SetVrelpoint(ChVector<> myrelpoint)102 void ChForce::SetVrelpoint(ChVector<> myrelpoint) {
103 // rel pos
104 vrelpoint = myrelpoint;
105 // abs pos
106 vpoint = GetBody()->Point_Body2World(vrelpoint);
107
108 // computes initial rest position.
109 ChVector<> displace = VNULL;
110 if (move_x)
111 displace.x() = move_x->Get_y(ChTime);
112 if (move_y)
113 displace.y() = move_y->Get_y(ChTime);
114 if (move_z)
115 displace.z() = move_z->Get_y(ChTime);
116
117 switch (frame) {
118 case WORLD:
119 restpos = Vsub(vpoint, displace);
120 break;
121 case BODY:
122 restpos = Vsub(vrelpoint, displace);
123 break;
124 }
125 }
126
127 // Impose absolute force directions
SetDir(ChVector<> newf)128 void ChForce::SetDir(ChVector<> newf) {
129 vdir = Vnorm(newf);
130 vreldir = GetBody()->TransformDirectionParentToLocal(vdir);
131 UpdateState(); // update also F
132 }
133
134 // Impose relative force directions
SetRelDir(ChVector<> newf)135 void ChForce::SetRelDir(ChVector<> newf) {
136 vreldir = Vnorm(newf);
137 vdir = GetBody()->TransformDirectionLocalToParent(vreldir);
138 UpdateState(); // update also F
139 }
140
141 // Impose module
SetMforce(double newf)142 void ChForce::SetMforce(double newf) {
143 mforce = newf;
144 UpdateState(); // update also F
145 }
146
147 // Force as applied to body
GetBodyForceTorque(ChVector<> & body_force,ChVector<> & body_torque) const148 void ChForce::GetBodyForceTorque(ChVector<>& body_force, ChVector<>& body_torque) const {
149 switch (mode) {
150 case FORCE: {
151 body_force = force; // Fb = F.w
152 ChStarMatrix33<> Xpos(vrelpoint);
153 body_torque = -(Xpos.transpose() * relforce); // Mb = - [u]'[A]'F,w = - [u]'F,l
154 break;
155 }
156 case TORQUE:
157 body_force = VNULL; // Fb = 0;
158 body_torque = relforce; // Mb = [A]'F,w = F,l
159 break;
160
161 default:
162 break;
163 }
164 }
165
166 // Updating
167
UpdateTime(double mytime)168 void ChForce::UpdateTime(double mytime) {
169 ChTime = mytime;
170
171 //... put time-dependent stuff here..
172 }
173
UpdateState()174 void ChForce::UpdateState() {
175 ChBody* my_body;
176 double modforce;
177 ChVector<> vectforce;
178 ChVector<> vmotion;
179 ChVector<> xyzforce;
180
181 my_body = GetBody();
182
183 // ====== Update the position of point of application
184
185 vmotion = VNULL;
186 if (move_x)
187 vmotion.x() = move_x->Get_y(ChTime);
188 if (move_y)
189 vmotion.y() = move_y->Get_y(ChTime);
190 if (move_z)
191 vmotion.z() = move_z->Get_y(ChTime);
192
193 switch (frame) {
194 case WORLD:
195 vpoint = Vadd(restpos, vmotion); // Uw
196 vrelpoint = my_body->Point_World2Body(vpoint); // Uo1 = [A]'(Uw-Xo1)
197 break;
198 case BODY:
199 vrelpoint = Vadd(restpos, vmotion); // Uo1
200 vpoint = my_body->Point_Body2World(vrelpoint); // Uw = Xo1+[A]Uo1
201 break;
202 }
203
204 // ====== Update the fm force vector and add fv
205
206 modforce = mforce * modula->Get_y(ChTime);
207
208 vectforce = VNULL;
209 xyzforce = VNULL;
210 if (f_x)
211 xyzforce.x() = f_x->Get_y(ChTime);
212 if (f_y)
213 xyzforce.y() = f_y->Get_y(ChTime);
214 if (f_z)
215 xyzforce.z() = f_z->Get_y(ChTime);
216
217 switch (align) {
218 case WORLD_DIR:
219 vreldir = my_body->TransformDirectionParentToLocal(vdir);
220 vectforce = Vmul(vdir, modforce);
221 vectforce = Vadd(vectforce, xyzforce);
222 break;
223 case BODY_DIR:
224 vdir = my_body->TransformDirectionLocalToParent(vreldir);
225 vectforce = Vmul(vdir, modforce);
226 xyzforce = my_body->TransformDirectionLocalToParent(xyzforce);
227 vectforce = Vadd(vectforce, xyzforce);
228 break;
229 }
230
231 force = vectforce; // Fw
232 relforce = my_body->TransformDirectionParentToLocal(force); // Fo1 = [A]'Fw
233
234 // ====== Update the Qc lagrangian!
235
236 switch (mode) {
237 case FORCE: {
238 Qf(0) = force.x(); // pos.lagrangian Qfx
239 Qf(1) = force.y();
240 Qf(2) = force.z();
241
242 // Qfrot= (-[A][u][G])'f
243
244 ChStarMatrix33<> Xpos(vrelpoint);
245 ChVector<> VQtemp = Xpos.transpose() * relforce; // = [u]'[A]'F,w
246
247 ChGlMatrix34<> mGl(my_body->GetCoord().rot);
248 ChVectorN<double, 4> Qfrot = -mGl.transpose() * VQtemp.eigen(); // Q = - [Gl]'[u]'[A]'F,w
249
250 Qf.segment(3, 4) = Qfrot;
251
252 break;
253 }
254
255 case TORQUE:
256 Qf(0) = 0; // pos.lagrangian Qfx
257 Qf(1) = 0;
258 Qf(2) = 0;
259
260 // rot.lagangian
261 ChGlMatrix34<> mGl(my_body->GetCoord().rot);
262 ChVectorN<double, 4> Qfrot = mGl.transpose() * relforce.eigen();
263
264 Qf.segment(3, 4) = Qfrot;
265
266 break;
267 }
268 }
269
Update(double mytime)270 void ChForce::Update(double mytime) {
271 UpdateTime(mytime);
272 UpdateState();
273 }
274
275 // File I/O
276
ArchiveOUT(ChArchiveOut & marchive)277 void ChForce::ArchiveOUT(ChArchiveOut& marchive) {
278 // class version number
279 marchive.VersionWrite<ChForce>();
280
281 // serialize parent class too
282 ChObj::ArchiveOUT(marchive);
283
284 // stream out all member data
285
286 ForceType_mapper ftypemapper;
287 marchive << CHNVP(ftypemapper(mode), "force_type");
288 ReferenceFrame_mapper refmapper;
289 marchive << CHNVP(refmapper(frame), "reference_frame_type");
290 AlignmentFrame_mapper alignmapper;
291 marchive << CHNVP(alignmapper(align), "alignment_frame_type");
292
293 marchive << CHNVP(vrelpoint);
294 marchive << CHNVP(vpoint);
295 marchive << CHNVP(move_x);
296 marchive << CHNVP(move_y);
297 marchive << CHNVP(move_z);
298 marchive << CHNVP(restpos);
299 marchive << CHNVP(f_x);
300 marchive << CHNVP(f_y);
301 marchive << CHNVP(f_z);
302 marchive << CHNVP(mforce);
303 marchive << CHNVP(modula, "f_time");
304 marchive << CHNVP(vreldir);
305 marchive << CHNVP(vdir);
306 }
307
ArchiveIN(ChArchiveIn & marchive)308 void ChForce::ArchiveIN(ChArchiveIn& marchive) {
309 // class version number
310 /*int version =*/ marchive.VersionRead<ChForce>();
311
312 // deserialize parent class too
313 ChObj::ArchiveIN(marchive);
314
315 // stream in all member data
316
317 ForceType_mapper ftypemapper;
318 marchive >> CHNVP(ftypemapper(mode), "force_type");
319 ReferenceFrame_mapper refmapper;
320 marchive >> CHNVP(refmapper(frame), "reference_frame_type");
321 AlignmentFrame_mapper alignmapper;
322 marchive >> CHNVP(alignmapper(align), "alignment_frame_type");
323
324 marchive >> CHNVP(vrelpoint);
325 marchive >> CHNVP(vpoint);
326 marchive >> CHNVP(move_x);
327 marchive >> CHNVP(move_y);
328 marchive >> CHNVP(move_z);
329 marchive >> CHNVP(restpos);
330 marchive >> CHNVP(f_x);
331 marchive >> CHNVP(f_y);
332 marchive >> CHNVP(f_z);
333 marchive >> CHNVP(mforce);
334 marchive >> CHNVP(modula, "f_time");
335 marchive >> CHNVP(vreldir);
336 marchive >> CHNVP(vdir);
337 }
338
339 } // end namespace chrono