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/ChLinkMarkers.h"
16
17 namespace chrono {
18
19 // Register into the object factory, to enable run-time dynamic creation and persistence
20 // CH_FACTORY_REGISTER(ChLinkMarkers) // NO! Abstract class!
21
ChLinkMarkers()22 ChLinkMarkers::ChLinkMarkers()
23 : marker1(NULL),
24 marker2(NULL),
25 markID1(0),
26 markID2(0),
27 relM(CSYSNORM),
28 relM_dt(CSYSNULL),
29 relM_dtdt(CSYSNULL),
30 relAngle(0),
31 relRotaxis(VNULL),
32 relWvel(VNULL),
33 relWacc(VNULL),
34 C_force(VNULL),
35 C_torque(VNULL) {}
36
ChLinkMarkers(const ChLinkMarkers & other)37 ChLinkMarkers::ChLinkMarkers(const ChLinkMarkers& other) : ChLink(other) {
38 marker1 = NULL;
39 marker2 = NULL;
40
41 markID1 = other.markID1;
42 markID2 = other.markID2;
43
44 relM = other.relM; // copy vars
45 relM_dt = other.relM_dt;
46 relM_dtdt = other.relM_dtdt;
47 relAngle = other.relAngle;
48 relRotaxis = other.relRotaxis;
49 relAxis = other.relAxis;
50 relWvel = other.relWvel;
51 relWacc = other.relWacc;
52 dist = other.dist;
53 dist_dt = other.dist_dt;
54
55 C_force = other.C_force;
56 C_torque = other.C_torque;
57 }
58
SetUpMarkers(ChMarker * mark1,ChMarker * mark2)59 void ChLinkMarkers::SetUpMarkers(ChMarker* mark1, ChMarker* mark2) {
60 // take care of the first link marker
61 marker1 = mark1;
62 if (mark1)
63 Body1 = (ChBodyFrame*)mark1->GetBody();
64 else
65 Body1 = NULL;
66
67 // take care of the second link marker
68 marker2 = mark2;
69 if (mark2)
70 Body2 = (ChBodyFrame*)mark2->GetBody();
71 else
72 Body2 = NULL;
73 }
74
ReferenceMarkers(ChMarker * mark1,ChMarker * mark2)75 bool ChLinkMarkers::ReferenceMarkers(ChMarker* mark1, ChMarker* mark2) {
76 this->SetUpMarkers(mark1, mark2);
77
78 if (mark1)
79 SetMarkID1(mark1->GetIdentifier());
80 else
81 SetMarkID1(0);
82 if (mark2)
83 SetMarkID2(mark2->GetIdentifier());
84 else
85 SetMarkID2(0);
86
87 return mark1 && mark2;
88 }
89
Initialize(std::shared_ptr<ChMarker> mmark1,std::shared_ptr<ChMarker> mmark2)90 void ChLinkMarkers::Initialize(std::shared_ptr<ChMarker> mmark1, std::shared_ptr<ChMarker> mmark2) {
91 ChMarker* mm1 = mmark1.get();
92 ChMarker* mm2 = mmark2.get();
93 assert(mm1 && mm2);
94 assert(mm1 != mm2);
95 assert(mm1->GetBody() && mm2->GetBody());
96 assert(mm1->GetBody()->GetSystem() == mm2->GetBody()->GetSystem());
97
98 ReferenceMarkers(mm1, mm2);
99 }
100
Initialize(std::shared_ptr<ChBody> mbody1,std::shared_ptr<ChBody> mbody2,const ChCoordsys<> & mpos)101 void ChLinkMarkers::Initialize(std::shared_ptr<ChBody> mbody1,
102 std::shared_ptr<ChBody> mbody2,
103 const ChCoordsys<>& mpos) {
104 return Initialize(mbody1, mbody2, false, mpos, mpos);
105 }
106
Initialize(std::shared_ptr<ChBody> mbody1,std::shared_ptr<ChBody> mbody2,bool pos_are_relative,const ChCoordsys<> & mpos1,const ChCoordsys<> & mpos2)107 void ChLinkMarkers::Initialize(std::shared_ptr<ChBody> mbody1,
108 std::shared_ptr<ChBody> mbody2,
109 bool pos_are_relative,
110 const ChCoordsys<>& mpos1,
111 const ChCoordsys<>& mpos2) {
112 assert(mbody1.get() != mbody2.get());
113 assert(mbody1->GetSystem() == mbody2->GetSystem());
114
115 // create markers to add to the two bodies
116 std::shared_ptr<ChMarker> mmark1(new ChMarker);
117 std::shared_ptr<ChMarker> mmark2(new ChMarker);
118 mbody1->AddMarker(mmark1);
119 mbody2->AddMarker(mmark2);
120
121 ChMarker* mm1 = mmark1.get();
122 ChMarker* mm2 = mmark2.get();
123 ReferenceMarkers(mm1, mm2);
124
125 if (pos_are_relative) {
126 mmark1->Impose_Rel_Coord(mpos1);
127 mmark2->Impose_Rel_Coord(mpos2);
128 } else {
129 mmark1->Impose_Abs_Coord(mpos1);
130 mmark2->Impose_Abs_Coord(mpos2);
131 }
132 }
133
134 // For all relative degrees of freedom of the two markers, compute relM, relM_dt,
135 // and relM_dtdt, as well as auxiliary data. Cache some intermediate quantities
136 // for possible reuse in UpdateState by some derived classes.
UpdateRelMarkerCoords()137 void ChLinkMarkers::UpdateRelMarkerCoords() {
138 PQw = Vsub(marker1->GetAbsCoord().pos, marker2->GetAbsCoord().pos);
139 PQw_dt = Vsub(marker1->GetAbsCoord_dt().pos, marker2->GetAbsCoord_dt().pos);
140 PQw_dtdt = Vsub(marker1->GetAbsCoord_dtdt().pos, marker2->GetAbsCoord_dtdt().pos);
141
142 dist = Vlength(PQw); // distance between origins, modulus
143 dist_dt = Vdot(Vnorm(PQw), PQw_dt); // speed between origins, modulus.
144
145 Quaternion qtemp1;
146
147 Quaternion temp1 = marker1->GetCoord_dt().rot;
148 Quaternion temp2 = marker2->GetCoord_dt().rot;
149
150 if (Qnotnull(temp1) || Qnotnull(temp2)) {
151 q_AD = // q'qqq + qqqq'
152 Qadd(Qcross(Qconjugate(marker2->GetCoord_dt().rot),
153 Qcross(Qconjugate(marker2->GetBody()->GetCoord().rot),
154 Qcross((marker1->GetBody()->GetCoord().rot), (marker1->GetCoord().rot)))),
155 Qcross(Qconjugate(marker2->GetCoord().rot),
156 Qcross(Qconjugate(marker2->GetBody()->GetCoord().rot),
157 Qcross((marker1->GetBody()->GetCoord().rot), (marker1->GetCoord_dt().rot)))));
158 } else
159 q_AD = QNULL;
160
161 q_BC = // qq'qq + qqq'q
162 Qadd(Qcross(Qconjugate(marker2->GetCoord().rot),
163 Qcross(Qconjugate(marker2->GetBody()->GetCoord_dt().rot),
164 Qcross((marker1->GetBody()->GetCoord().rot), (marker1->GetCoord().rot)))),
165 Qcross(Qconjugate(marker2->GetCoord().rot),
166 Qcross(Qconjugate(marker2->GetBody()->GetCoord().rot),
167 Qcross((marker1->GetBody()->GetCoord_dt().rot), (marker1->GetCoord().rot)))));
168
169 // q_8 = q''qqq + 2q'q'qq + 2q'qq'q + 2q'qqq'
170 // + 2qq'q'q + 2qq'qq' + 2qqq'q' + qqqq''
171 temp2 = marker2->GetCoord_dtdt().rot;
172 if (Qnotnull(temp2))
173 q_8 = Qcross(Qconjugate(marker2->GetCoord_dtdt().rot),
174 Qcross(Qconjugate(Body2->GetCoord().rot),
175 Qcross(Body1->GetCoord().rot,
176 marker1->GetCoord().rot))); // q_dtdt'm2 * q'o2 * q,o1 * q,m1
177 else
178 q_8 = QNULL;
179 temp1 = marker1->GetCoord_dtdt().rot;
180 if (Qnotnull(temp1)) {
181 qtemp1 = Qcross(Qconjugate(marker2->GetCoord().rot),
182 Qcross(Qconjugate(Body2->GetCoord().rot),
183 Qcross(Body1->GetCoord().rot,
184 marker1->GetCoord_dtdt().rot))); // q'm2 * q'o2 * q,o1 * q_dtdt,m1
185 q_8 = Qadd(q_8, qtemp1);
186 }
187 temp2 = marker2->GetCoord_dt().rot;
188 if (Qnotnull(temp2)) {
189 qtemp1 = Qcross(
190 Qconjugate(marker2->GetCoord_dt().rot),
191 Qcross(Qconjugate(Body2->GetCoord_dt().rot), Qcross(Body1->GetCoord().rot, marker1->GetCoord().rot)));
192 qtemp1 = Qscale(qtemp1, 2); // 2( q_dt'm2 * q_dt'o2 * q,o1 * q,m1)
193 q_8 = Qadd(q_8, qtemp1);
194 }
195 temp2 = marker2->GetCoord_dt().rot;
196 if (Qnotnull(temp2)) {
197 qtemp1 = Qcross(
198 Qconjugate(marker2->GetCoord_dt().rot),
199 Qcross(Qconjugate(Body2->GetCoord().rot), Qcross(Body1->GetCoord_dt().rot, marker1->GetCoord().rot)));
200 qtemp1 = Qscale(qtemp1, 2); // 2( q_dt'm2 * q'o2 * q_dt,o1 * q,m1)
201 q_8 = Qadd(q_8, qtemp1);
202 }
203 temp1 = marker1->GetCoord_dt().rot;
204 temp2 = marker2->GetCoord_dt().rot;
205 if (Qnotnull(temp2) && Qnotnull(temp1)) {
206 qtemp1 = Qcross(
207 Qconjugate(marker2->GetCoord_dt().rot),
208 Qcross(Qconjugate(Body2->GetCoord().rot), Qcross(Body1->GetCoord().rot, marker1->GetCoord_dt().rot)));
209 qtemp1 = Qscale(qtemp1, 2); // 2( q_dt'm2 * q'o2 * q,o1 * q_dt,m1)
210 q_8 = Qadd(q_8, qtemp1);
211 }
212
213 qtemp1 =
214 Qcross(Qconjugate(marker2->GetCoord().rot),
215 Qcross(Qconjugate(Body2->GetCoord_dt().rot), Qcross(Body1->GetCoord_dt().rot, marker1->GetCoord().rot)));
216 qtemp1 = Qscale(qtemp1, 2); // 2( q'm2 * q_dt'o2 * q_dt,o1 * q,m1)
217 q_8 = Qadd(q_8, qtemp1);
218 temp1 = marker1->GetCoord_dt().rot;
219 if (Qnotnull(temp1)) {
220 qtemp1 = Qcross(
221 Qconjugate(marker2->GetCoord().rot),
222 Qcross(Qconjugate(Body2->GetCoord_dt().rot), Qcross(Body1->GetCoord().rot, marker1->GetCoord_dt().rot)));
223 qtemp1 = Qscale(qtemp1, 2); // 2( q'm2 * q_dt'o2 * q,o1 * q_dt,m1)
224 q_8 = Qadd(q_8, qtemp1);
225 }
226 temp1 = marker1->GetCoord_dt().rot;
227 if (Qnotnull(temp1)) {
228 qtemp1 = Qcross(
229 Qconjugate(marker2->GetCoord().rot),
230 Qcross(Qconjugate(Body2->GetCoord().rot), Qcross(Body1->GetCoord_dt().rot, marker1->GetCoord_dt().rot)));
231 qtemp1 = Qscale(qtemp1, 2); // 2( q'm2 * q'o2 * q_dt,o1 * q_dt,m1)
232 q_8 = Qadd(q_8, qtemp1);
233 }
234
235 // q_4 = [Adtdt]'[A]'q + 2[Adt]'[Adt]'q
236 // + 2[Adt]'[A]'qdt + 2[A]'[Adt]'qdt
237 ChMatrix33<> m2_Rel_A_dt;
238 marker2->Compute_Adt(m2_Rel_A_dt);
239 ChMatrix33<> m2_Rel_A_dtdt;
240 marker2->Compute_Adtdt(m2_Rel_A_dtdt);
241
242 ChVector<> vtemp1;
243 ChVector<> vtemp2;
244
245 vtemp1 = Body2->GetA_dt().transpose() * PQw;
246 vtemp2 = m2_Rel_A_dt.transpose() * vtemp1;
247 q_4 = Vmul(vtemp2, 2); // 2[Aq_dt]'[Ao2_dt]'*Qpq,w
248
249 vtemp1 = Body2->GetA().transpose() * PQw_dt;
250 vtemp2 = m2_Rel_A_dt.transpose() * vtemp1;
251 vtemp2 = Vmul(vtemp2, 2); // 2[Aq_dt]'[Ao2]'*Qpq,w_dt
252 q_4 = Vadd(q_4, vtemp2);
253
254 vtemp1 = Body2->GetA_dt().transpose() * PQw_dt;
255 vtemp2 = marker2->GetA().transpose() * vtemp1;
256 vtemp2 = Vmul(vtemp2, 2); // 2[Aq]'[Ao2_dt]'*Qpq,w_dt
257 q_4 = Vadd(q_4, vtemp2);
258
259 vtemp1 = Body2->GetA().transpose() * PQw;
260 vtemp2 = m2_Rel_A_dtdt.transpose() * vtemp1;
261 q_4 = Vadd(q_4, vtemp2); // [Aq_dtdt]'[Ao2]'*Qpq,w
262
263 // ----------- RELATIVE MARKER COORDINATES
264
265 // relM.pos
266 relM.pos = marker2->GetA().transpose() * (Body2->GetA().transpose() * PQw);
267
268 // relM.rot
269 relM.rot = Qcross(Qconjugate(marker2->GetCoord().rot),
270 Qcross(Qconjugate(marker2->GetBody()->GetCoord().rot),
271 Qcross((marker1->GetBody()->GetCoord().rot), (marker1->GetCoord().rot))));
272
273 // relM_dt.pos
274 relM_dt.pos = m2_Rel_A_dt.transpose() * (Body2->GetA().transpose() * PQw) +
275 marker2->GetA().transpose() * (Body2->GetA_dt().transpose() * PQw) +
276 marker2->GetA().transpose() * (Body2->GetA().transpose() * PQw_dt);
277
278 // relM_dt.rot
279 relM_dt.rot = Qadd(q_AD, q_BC);
280
281 // relM_dtdt.pos
282 relM_dtdt.pos = marker2->GetA().transpose() * (Body2->GetA_dtdt().transpose() * PQw) +
283 marker2->GetA().transpose() * (Body2->GetA().transpose() * PQw_dtdt) + q_4;
284
285 // relM_dtdt.rot
286 qtemp1 = Qcross(Qconjugate(marker2->GetCoord().rot),
287 Qcross(Qconjugate(Body2->GetCoord_dtdt().rot),
288 Qcross(Body1->GetCoord().rot,
289 marker1->GetCoord().rot))); // ( q'm2 * q_dtdt'o2 * q,o1 * q,m1)
290 relM_dtdt.rot = Qadd(q_8, qtemp1);
291 qtemp1 = Qcross(Qconjugate(marker2->GetCoord().rot),
292 Qcross(Qconjugate(Body2->GetCoord().rot),
293 Qcross(Body1->GetCoord_dtdt().rot,
294 marker1->GetCoord().rot))); // ( q'm2 * q'o2 * q_dtdt,o1 * q,m1)
295 relM_dtdt.rot = Qadd(relM_dtdt.rot, qtemp1); // = q_8 + qq''qq + qqq''q
296
297 // ... and also "user-friendly" relative coordinates:
298
299 // relAngle and relAxis
300 Q_to_AngAxis(relM.rot, relAngle, relAxis);
301 // flip rel rotation axis if jerky sign
302 if (relAxis.z() < 0) {
303 relAxis = Vmul(relAxis, -1);
304 relAngle = -relAngle;
305 }
306 // rotation axis
307 relRotaxis = Vmul(relAxis, relAngle);
308 // relWvel
309 ChGwMatrix34<> relGw(relM.rot);
310 relWvel = relGw * relM_dt.rot;
311 // relWacc
312 relWacc = relGw * relM_dtdt.rot;
313 }
314
UpdateForces(double mytime)315 void ChLinkMarkers::UpdateForces(double mytime) {
316 // reset internal force accumulators
317 C_force = VNULL; // initialize int.forces accumulators
318 C_torque = VNULL;
319 }
320
Update(double time,bool update_assets)321 void ChLinkMarkers::Update(double time, bool update_assets) {
322 UpdateTime(time);
323 UpdateRelMarkerCoords();
324 UpdateForces(time);
325
326 // Update assets
327 ChPhysicsItem::Update(ChTime, update_assets);
328 }
329
330 //// STATE BOOKKEEPING FUNCTIONS
331
332 // Load residual R += c * F, starting at specified offset.
IntLoadResidual_F(const unsigned int off,ChVectorDynamic<> & R,const double c)333 void ChLinkMarkers::IntLoadResidual_F(const unsigned int off, ChVectorDynamic<>& R, const double c) {
334 if (!Body1 || !Body2)
335 return;
336
337 Vector mbody_force;
338 Vector mbody_torque;
339 if (Vnotnull(C_force)) {
340 Vector m_abs_force = Body2->GetA() * (marker2->GetA() * C_force);
341
342 if (Body2->Variables().IsActive()) {
343 Body2->To_abs_forcetorque(m_abs_force,
344 marker1->GetAbsCoord().pos, // absolute application point is always marker1
345 false, // from abs. space
346 mbody_force, mbody_torque); // resulting force-torque, both in abs coords
347 R.segment(Body2->Variables().GetOffset() + 0, 3) -= c * mbody_force.eigen();
348 R.segment(Body2->Variables().GetOffset() + 3, 3) -=
349 c * Body2->TransformDirectionParentToLocal(mbody_torque).eigen();
350 }
351
352 if (Body1->Variables().IsActive()) {
353 Body1->To_abs_forcetorque(m_abs_force,
354 marker1->GetAbsCoord().pos, // absolute application point is always marker1
355 false, // from abs. space
356 mbody_force, mbody_torque); // resulting force-torque, both in abs coords
357 R.segment(Body1->Variables().GetOffset() + 0, 3) += c * mbody_force.eigen();
358 R.segment(Body1->Variables().GetOffset() + 3, 3) +=
359 c * Body1->TransformDirectionParentToLocal(mbody_torque).eigen();
360 }
361 }
362 if (Vnotnull(C_torque)) {
363 Vector m_abs_torque = Body2->GetA() * (marker2->GetA() * C_torque);
364 // load torques in 'fb' vector accumulator of body variables (torques in local coords)
365 if (Body1->Variables().IsActive()) {
366 R.segment(Body1->Variables().GetOffset() + 3, 3) +=
367 c * Body1->TransformDirectionParentToLocal(m_abs_torque).eigen();
368 }
369 if (Body2->Variables().IsActive()) {
370 R.segment(Body2->Variables().GetOffset() + 3, 3) -=
371 c * Body2->TransformDirectionParentToLocal(m_abs_torque).eigen();
372 }
373 }
374 }
375
376 // SOLVER INTERFACE
377
ConstraintsFbLoadForces(double factor)378 void ChLinkMarkers::ConstraintsFbLoadForces(double factor) {
379 if (!Body1 || !Body2)
380 return;
381
382 if (Vnotnull(C_force)) {
383 Vector mbody_force;
384 Vector mbody_torque;
385 Vector m_abs_force = Body2->GetA() * (marker2->GetA() * C_force);
386
387 Body2->To_abs_forcetorque(m_abs_force,
388 marker1->GetAbsCoord().pos, // absolute application point is always marker1
389 false, // from abs. space
390 mbody_force, mbody_torque); // resulting force-torque, both in abs coords
391 Body2->Variables().Get_fb().segment(0, 3) -= factor * mbody_force.eigen();
392 Body2->Variables().Get_fb().segment(3, 3) -=
393 factor * Body2->TransformDirectionParentToLocal(mbody_torque).eigen();
394
395 Body1->To_abs_forcetorque(m_abs_force,
396 marker1->GetAbsCoord().pos, // absolute application point is always marker1
397 false, // from abs. space
398 mbody_force, mbody_torque); // resulting force-torque, both in abs coords
399 Body1->Variables().Get_fb().segment(0, 3) += factor * mbody_force.eigen();
400 Body1->Variables().Get_fb().segment(3, 3) +=
401 factor * Body1->TransformDirectionParentToLocal(mbody_torque).eigen();
402 }
403
404 if (Vnotnull(C_torque)) {
405 Vector m_abs_torque = Body2->GetA() * (marker2->GetA() * C_torque);
406 // load torques in 'fb' vector accumulator of body variables (torques in local coords)
407 Body1->Variables().Get_fb().segment(3, 3) +=
408 factor * Body1->TransformDirectionParentToLocal(m_abs_torque).eigen();
409 Body2->Variables().Get_fb().segment(3, 3) -=
410 factor * Body2->TransformDirectionParentToLocal(m_abs_torque).eigen();
411 }
412 }
413
ArchiveOUT(ChArchiveOut & marchive)414 void ChLinkMarkers::ArchiveOUT(ChArchiveOut& marchive) {
415 // version number
416 marchive.VersionWrite<ChLinkMarkers>();
417
418 // serialize parent class
419 ChLink::ArchiveOUT(marchive);
420
421 // serialize all member data:
422 marchive << CHNVP(markID1);
423 marchive << CHNVP(markID2);
424 }
425
426 /// Method to allow de serialization of transient data from archives.
ArchiveIN(ChArchiveIn & marchive)427 void ChLinkMarkers::ArchiveIN(ChArchiveIn& marchive) {
428 // version number
429 /*int version =*/ marchive.VersionRead<ChLinkMarkers>();
430
431 // deserialize parent class
432 ChLink::ArchiveIN(marchive);
433
434 // deserialize all member data:
435 marchive >> CHNVP(markID1);
436 marchive >> CHNVP(markID2);
437 }
438
439 } // end namespace chrono
440