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, Arman Pazouki
13 // =============================================================================
14
15 //// TODO
16 //// Serialization/deserialization of unique_ptr members is currently commented
17 //// out until support for unique_ptr is implemented in ChArchive.
18
19 #include "chrono/physics/ChSystem.h"
20 #include "chrono/physics/ChLinkLock.h"
21
22 namespace chrono {
23
24 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChLinkLock)25 CH_FACTORY_REGISTER(ChLinkLock)
26
27 ChLinkLock::ChLinkLock() : type(LinkType::FREE), ndoc(0), ndoc_c(0), ndoc_d(0), d_restlength(0) {
28 // Need to zero out the bottom-right 4x3 block
29 Cq1_temp.setZero();
30 Cq2_temp.setZero();
31
32 // Note: the joint is not completely built at this time.
33 // Requires definition of the mask (based on concrete joint type)
34 }
35
ChLinkLock(const ChLinkLock & other)36 ChLinkLock::ChLinkLock(const ChLinkLock& other) : ChLinkMarkers(other) {
37 mask = other.mask;
38
39 force_D.reset(other.force_D->Clone());
40 force_R.reset(other.force_R->Clone());
41 force_X.reset(other.force_X->Clone());
42 force_Y.reset(other.force_Y->Clone());
43 force_Z.reset(other.force_Z->Clone());
44 force_Rx.reset(other.force_Rx->Clone());
45 force_Ry.reset(other.force_Ry->Clone());
46 force_Rz.reset(other.force_Rz->Clone());
47
48 d_restlength = other.d_restlength;
49
50 type = other.type;
51
52 limit_X.reset(other.limit_X->Clone());
53 limit_Y.reset(other.limit_Y->Clone());
54 limit_Z.reset(other.limit_Z->Clone());
55 limit_Rx.reset(other.limit_Rx->Clone());
56 limit_Ry.reset(other.limit_Ry->Clone());
57 limit_Rz.reset(other.limit_Rz->Clone());
58 limit_Rp.reset(other.limit_Rp->Clone());
59 limit_D.reset(other.limit_D->Clone());
60
61 Ct_temp = other.Ct_temp;
62
63 BuildLinkType(other.type);
64 }
65
~ChLinkLock()66 ChLinkLock::~ChLinkLock() {}
67
68 //// Note: ability to explicitly provide joint forces was removed.
69 //// If ever needed, these functions can be re-enabled. In that case,
70 //// a typical user call would be:
71 //// auto my_force = chrono_types::make_unique<ChLinkForce>();
72 //// my_joint->SetForce_X(std::move(my_force));
73
74 /*
75 void ChLinkLock::SetForce_D(std::unique_ptr<ChLinkForce>&& force) {
76 force_D = std::move(force);
77 }
78 void ChLinkLock::SetForce_R(std::unique_ptr<ChLinkForce>&& force) {
79 force_R = std::move(force);
80 }
81 void ChLinkLock::SetForce_X(std::unique_ptr<ChLinkForce>&& force) {
82 force_X = std::move(force);
83 }
84 void ChLinkLock::SetForce_Y(std::unique_ptr<ChLinkForce>&& force) {
85 force_Y = std::move(force);
86 }
87 void ChLinkLock::SetForce_Z(std::unique_ptr<ChLinkForce>&& force) {
88 force_Z = std::move(force);
89 }
90 void ChLinkLock::SetForce_Rx(std::unique_ptr<ChLinkForce>&& force) {
91 force_Rx = std::move(force);
92 }
93 void ChLinkLock::SetForce_Ry(std::unique_ptr<ChLinkForce>&& force) {
94 force_Ry = std::move(force);
95 }
96 void ChLinkLock::SetForce_Rz(std::unique_ptr<ChLinkForce>&& force) {
97 force_Rz = std::move(force);
98 }
99 */
100
GetForce_D()101 ChLinkForce& ChLinkLock::GetForce_D() {
102 if (!force_D)
103 force_D = chrono_types::make_unique<ChLinkForce>();
104 return *force_D;
105 }
GetForce_R()106 ChLinkForce& ChLinkLock::GetForce_R() {
107 if (!force_R)
108 force_R = chrono_types::make_unique<ChLinkForce>();
109 return *force_R;
110 }
GetForce_X()111 ChLinkForce& ChLinkLock::GetForce_X() {
112 if (!force_X)
113 force_X = chrono_types::make_unique<ChLinkForce>();
114 return *force_X;
115 }
GetForce_Y()116 ChLinkForce& ChLinkLock::GetForce_Y() {
117 if (!force_Y)
118 force_Y = chrono_types::make_unique<ChLinkForce>();
119 return *force_Y;
120 }
GetForce_Z()121 ChLinkForce& ChLinkLock::GetForce_Z() {
122 if (!force_Z)
123 force_Z = chrono_types::make_unique<ChLinkForce>();
124 return *force_Z;
125 }
GetForce_Rx()126 ChLinkForce& ChLinkLock::GetForce_Rx() {
127 if (!force_Rx)
128 force_Rx = chrono_types::make_unique<ChLinkForce>();
129 return *force_Rx;
130 }
GetForce_Ry()131 ChLinkForce& ChLinkLock::GetForce_Ry() {
132 if (!force_Ry)
133 force_Ry = chrono_types::make_unique<ChLinkForce>();
134 return *force_Ry;
135 }
GetForce_Rz()136 ChLinkForce& ChLinkLock::GetForce_Rz() {
137 if (!force_Rz)
138 force_Rz = chrono_types::make_unique<ChLinkForce>();
139 return *force_Rz;
140 }
141
142 //// Note: ability to explicitly provide limits was removed.
143 //// If ever needed, these functions can be re-enabled. In that case,
144 //// a typical user call would be:
145 //// auto my_limit = chrono_types::make_unique<ChLinkLimit>();
146 //// my_joint->SetLimit_X(std::move(my_force));
147
148 /*
149 void ChLinkLock::SetLimit_X(std::unique_ptr<ChLinkLimit>&& limit) {
150 limit_X = std::move(limit);
151 }
152 void ChLinkLock::SetLimit_Y(std::unique_ptr<ChLinkLimit>&& limit) {
153 limit_Y = std::move(limit);
154 }
155 void ChLinkLock::SetLimit_Z(std::unique_ptr<ChLinkLimit>&& limit) {
156 limit_Z = std::move(limit);
157 }
158 void ChLinkLock::SetLimit_Rx(std::unique_ptr<ChLinkLimit>&& limit) {
159 limit_Rx = std::move(limit);
160 }
161 void ChLinkLock::SetLimit_Ry(std::unique_ptr<ChLinkLimit>&& limit) {
162 limit_Ry = std::move(limit);
163 }
164 void ChLinkLock::SetLimit_Rz(std::unique_ptr<ChLinkLimit>&& limit) {
165 limit_Rz = std::move(limit);
166 }
167 void ChLinkLock::SetLimit_Rp(std::unique_ptr<ChLinkLimit>&& limit) {
168 limit_Rp = std::move(limit);
169 }
170 void ChLinkLock::SetLimit_D(std::unique_ptr<ChLinkLimit>&& limit) {
171 limit_D = std::move(limit);
172 }
173 */
174
GetLimit_X()175 ChLinkLimit& ChLinkLock::GetLimit_X() {
176 if (!limit_X)
177 limit_X = chrono_types::make_unique<ChLinkLimit>();
178 return *limit_X;
179 }
GetLimit_Y()180 ChLinkLimit& ChLinkLock::GetLimit_Y() {
181 if (!limit_Y)
182 limit_Y = chrono_types::make_unique<ChLinkLimit>();
183 return *limit_Y;
184 }
GetLimit_Z()185 ChLinkLimit& ChLinkLock::GetLimit_Z() {
186 if (!limit_Z)
187 limit_Z = chrono_types::make_unique<ChLinkLimit>();
188 return *limit_Z;
189 }
GetLimit_Rx()190 ChLinkLimit& ChLinkLock::GetLimit_Rx() {
191 if (!limit_Rx)
192 limit_Rx = chrono_types::make_unique<ChLinkLimit>();
193 return *limit_Rx;
194 }
GetLimit_Ry()195 ChLinkLimit& ChLinkLock::GetLimit_Ry() {
196 if (!limit_Ry)
197 limit_Ry = chrono_types::make_unique<ChLinkLimit>();
198 return *limit_Ry;
199 }
GetLimit_Rz()200 ChLinkLimit& ChLinkLock::GetLimit_Rz() {
201 if (!limit_Rz)
202 limit_Rz = chrono_types::make_unique<ChLinkLimit>();
203 return *limit_Rz;
204 }
GetLimit_Rp()205 ChLinkLimit& ChLinkLock::GetLimit_Rp() {
206 if (!limit_Rp)
207 limit_Rp = chrono_types::make_unique<ChLinkLimit>();
208 return *limit_Rp;
209 }
GetLimit_D()210 ChLinkLimit& ChLinkLock::GetLimit_D() {
211 if (!limit_D)
212 limit_D = chrono_types::make_unique<ChLinkLimit>();
213 return *limit_D;
214 }
215
SetDisabled(bool mdis)216 void ChLinkLock::SetDisabled(bool mdis) {
217 ChLinkMarkers::SetDisabled(mdis);
218
219 if (mask.SetAllDisabled(mdis) > 0)
220 BuildLink();
221 }
222
SetBroken(bool mbro)223 void ChLinkLock::SetBroken(bool mbro) {
224 ChLinkMarkers::SetBroken(mbro);
225
226 if (mask.SetAllBroken(mbro) > 0)
227 BuildLink();
228 }
229
RestoreRedundant()230 int ChLinkLock::RestoreRedundant() {
231 int mchanges = mask.RestoreRedundant();
232 if (mchanges)
233 BuildLink();
234
235 return mchanges;
236 }
237
SetUpMarkers(ChMarker * mark1,ChMarker * mark2)238 void ChLinkLock::SetUpMarkers(ChMarker* mark1, ChMarker* mark2) {
239 ChLinkMarkers::SetUpMarkers(mark1, mark2);
240 assert(this->Body1 && this->Body2);
241
242 mask.SetTwoBodiesVariables(&Body1->Variables(), &Body2->Variables());
243
244 // We must call BuildLink here again, because only now are the constraints properly activated
245 // (and hence the correct NDOC available).
246 BuildLink();
247 }
248
BuildLink()249 void ChLinkLock::BuildLink() {
250 // set ndoc by counting non-dofs
251 ndoc = mask.GetMaskDoc();
252 ndoc_c = mask.GetMaskDoc_c();
253 ndoc_d = mask.GetMaskDoc_d();
254
255 // create matrices
256 C.resize(ndoc);
257 C_dt.resize(ndoc);
258 C_dtdt.resize(ndoc);
259 react.resize(ndoc);
260 Qc.resize(ndoc);
261 Ct.resize(ndoc);
262 Cq1.resize(ndoc, BODY_QDOF);
263 Cq2.resize(ndoc, BODY_QDOF);
264 Cqw1.resize(ndoc, BODY_DOF);
265 Cqw2.resize(ndoc, BODY_DOF);
266
267 // Zero out vectors of constraint violations
268 C.setZero();
269 C_dt.setZero();
270 C_dtdt.setZero();
271 react.setZero();
272
273 // Need to zero out the first 3 entries in rows corrsponding to rotation constraints
274 Cq1.setZero();
275 Cq2.setZero();
276 }
277
BuildLink(bool x,bool y,bool z,bool e0,bool e1,bool e2,bool e3)278 void ChLinkLock::BuildLink(bool x, bool y, bool z, bool e0, bool e1, bool e2, bool e3) {
279 mask.SetLockMask(x, y, z, e0, e1, e2, e3);
280 BuildLink();
281 }
282
BuildLinkType(LinkType link_type)283 void ChLinkLock::BuildLinkType(LinkType link_type) {
284 type = link_type;
285
286 // SetLockMask() sets the constraints for the link coordinates: (X,Y,Z, E0,E1,E2,E3)
287 switch (type) {
288 case LinkType::FREE:
289 BuildLink(false, false, false, false, false, false, false);
290 break;
291 case LinkType::LOCK:
292 // this should never happen
293 BuildLink(true, true, true, false, true, true, true);
294 break;
295 case LinkType::SPHERICAL:
296 BuildLink(true, true, true, false, false, false, false);
297 break;
298 case LinkType::POINTPLANE:
299 BuildLink(false, false, true, false, false, false, false);
300 break;
301 case LinkType::POINTLINE:
302 BuildLink(false, true, true, false, false, false, false);
303 break;
304 case LinkType::REVOLUTE:
305 BuildLink(true, true, true, false, true, true, false);
306 break;
307 case LinkType::CYLINDRICAL:
308 BuildLink(true, true, false, false, true, true, false);
309 break;
310 case LinkType::PRISMATIC:
311 BuildLink(true, true, false, false, true, true, true);
312 break;
313 case LinkType::PLANEPLANE:
314 BuildLink(false, false, true, false, true, true, false);
315 break;
316 case LinkType::OLDHAM:
317 BuildLink(false, false, true, false, true, true, true);
318 break;
319 case LinkType::ALIGN:
320 BuildLink(false, false, false, false, true, true, true);
321 break;
322 case LinkType::PARALLEL:
323 BuildLink(false, false, false, false, true, true, false);
324 break;
325 case LinkType::PERPEND:
326 BuildLink(false, false, false, false, true, false, true);
327 break;
328 case LinkType::REVOLUTEPRISMATIC:
329 BuildLink(false, true, true, false, true, true, false);
330 break;
331 default:
332 BuildLink(false, false, false, false, false, false, false);
333 break;
334 }
335 }
336
ChangeLinkType(LinkType new_link_type)337 void ChLinkLock::ChangeLinkType(LinkType new_link_type) {
338 BuildLinkType(new_link_type);
339
340 limit_X.reset(nullptr);
341 limit_Y.reset(nullptr);
342 limit_Z.reset(nullptr);
343 limit_Rx.reset(nullptr);
344 limit_Ry.reset(nullptr);
345 limit_Rz.reset(nullptr);
346 limit_D.reset(nullptr);
347 limit_Rp.reset(nullptr);
348 }
349
350 // setup the functions when user changes them.
351
352 // -----------------------------------------------------------------------------
353 // UPDATING PROCEDURES
354
355 // Complete update.
Update(double time,bool update_assets)356 void ChLinkLock::Update(double time, bool update_assets) {
357 UpdateTime(time);
358 UpdateRelMarkerCoords();
359 UpdateState();
360 UpdateCqw();
361 UpdateForces(time);
362
363 // Update assets
364 ChPhysicsItem::Update(ChTime, update_assets);
365 }
366
367 // Updates Cq1_temp, Cq2_temp, Qc_temp, etc., i.e. all LOCK-FORMULATION temp.matrices
UpdateState()368 void ChLinkLock::UpdateState() {
369 // ----------- SOME PRECALCULATED VARIABLES, to optimize speed
370
371 ChStarMatrix33<> P1star(marker1->GetCoord().pos); // [P] star matrix of rel pos of mark1
372 ChStarMatrix33<> Q2star(marker2->GetCoord().pos); // [Q] star matrix of rel pos of mark2
373
374 ChGlMatrix34<> body1Gl(Body1->GetCoord().rot);
375 ChGlMatrix34<> body2Gl(Body2->GetCoord().rot);
376
377 // COMPUTE THE Cq Ct Qc matrices (temporary, for complete lock constraint)
378
379 ChMatrix33<> m2_Rel_A_dt;
380 marker2->Compute_Adt(m2_Rel_A_dt);
381 ChMatrix33<> m2_Rel_A_dtdt;
382 marker2->Compute_Adtdt(m2_Rel_A_dtdt);
383
384 // ----------- PARTIAL DERIVATIVE Ct OF CONSTRAINT
385 Ct_temp.pos =
386 m2_Rel_A_dt.transpose() * (Body2->GetA().transpose() * PQw) +
387 marker2->GetA().transpose() *
388 (Body2->GetA().transpose() * (Body1->GetA() * marker1->GetCoord_dt().pos) - marker2->GetCoord_dt().pos);
389
390 Ct_temp.rot = q_AD;
391
392 //------------ COMPLETE JACOBIANS Cq1_temp AND Cq2_temp AND Qc_temp VECTOR.
393 // [Cq_temp]= [[CqxT] [CqxR]] {Qc_temp} ={[Qcx]}
394 // [[ 0 ] [CqrR]] {[Qcr]}
395
396 // JACOBIANS Cq1_temp, Cq2_temp:
397
398 ChMatrix33<> CqxT = marker2->GetA().transpose() * Body2->GetA().transpose(); // [CqxT]=[Aq]'[Ao2]'
399 ChStarMatrix33<> tmpStar(Body2->GetA().transpose() * PQw);
400
401 Cq1_temp.topLeftCorner<3, 3>() = CqxT; // *- -- Cq1_temp(1-3) = [Aqo2]
402 Cq2_temp.topLeftCorner<3, 3>() = -CqxT; // -- *- Cq2_temp(1-3) = -[Aqo2]
403 Cq1_temp.topRightCorner<3, 4>() = -CqxT * Body1->GetA() * P1star * body1Gl; // -* -- Cq1_temp(4-7)
404 Cq2_temp.topRightCorner<3, 4>() = CqxT * Body2->GetA() * Q2star * body2Gl + //
405 marker2->GetA().transpose() * tmpStar * body2Gl; // -- -* Cq2_temp(4-7)
406
407 {
408 ChStarMatrix44<> stempQ1(Qcross(Qconjugate(marker2->GetCoord().rot), Qconjugate(Body2->GetCoord().rot)));
409 ChStarMatrix44<> stempQ2(marker1->GetCoord().rot);
410 stempQ2.semiTranspose();
411 Cq1_temp.bottomRightCorner<4, 4>() = stempQ1 * stempQ2; // =* == Cq1_temp(col 4-7, row 4-7) ... CqrR
412 }
413
414 {
415 ChStarMatrix44<> stempQ1(Qconjugate(marker2->GetCoord().rot));
416 ChStarMatrix44<> stempQ2(Qcross(Body1->GetCoord().rot, marker1->GetCoord().rot));
417 stempQ2.semiTranspose();
418 stempQ2.semiNegate();
419 Cq2_temp.bottomRightCorner<4, 4>() = stempQ1 * stempQ2; // == =* Cq2_temp(col 4-7, row 4-7) ... CqrR
420 }
421
422 //--------- COMPLETE Qc VECTOR
423 ChVector<> Qcx;
424 ChVector<> vtemp1;
425 ChVector<> vtemp2;
426
427 vtemp1 = Vcross(Body1->GetWvel_loc(), Vcross(Body1->GetWvel_loc(), marker1->GetCoord().pos));
428 vtemp1 = Vadd(vtemp1, marker1->GetCoord_dtdt().pos);
429 vtemp1 = Vadd(vtemp1, Vmul(Vcross(Body1->GetWvel_loc(), marker1->GetCoord_dt().pos), 2));
430
431 vtemp2 = Vcross(Body2->GetWvel_loc(), Vcross(Body2->GetWvel_loc(), marker2->GetCoord().pos));
432 vtemp2 = Vadd(vtemp2, marker2->GetCoord_dtdt().pos);
433 vtemp2 = Vadd(vtemp2, Vmul(Vcross(Body2->GetWvel_loc(), marker2->GetCoord_dt().pos), 2));
434
435 Qcx = CqxT * (Body1->GetA() * vtemp1 - Body2->GetA() * vtemp2);
436
437 ChStarMatrix33<> mtemp1(Body2->GetWvel_loc());
438 ChMatrix33<> mtemp3 = Body2->GetA() * mtemp1 * mtemp1;
439 vtemp2 = marker2->GetA().transpose() * (mtemp3.transpose() * PQw); // [Aq]'[[A2][w2][w2]]'*Qpq,w
440 Qcx = Vadd(Qcx, vtemp2);
441 Qcx = Vadd(Qcx, q_4); // [Adtdt]'[A]'q + 2[Adt]'[Adt]'q + 2[Adt]'[A]'qdt + 2[A]'[Adt]'qdt
442
443 Qc_temp.segment(0, 3) = Qcx.eigen(); // * Qc_temp, for all translational coords
444 Qc_temp.segment(3, 4) = q_8.eigen(); // * Qc_temp, for all rotational coords (Qcr = q_8)
445
446 // *** NOTE! The final Qc must change sign, to be used in
447 // lagrangian equation: [Cq]*q_dtdt = Qc
448 // because until now we have computed it as [Cq]*q_dtdt + "Qc" = 0
449 Qc_temp *= -1;
450
451 // ---------------------
452 // Updates Cq1, Cq2, Qc,
453 // C, C_dt, C_dtdt, Ct.
454 // ---------------------
455 int index = 0;
456
457 if (mask.Constr_X().IsActive()) {
458 Cq1.block<1, 7>(index, 0) = Cq1_temp.block<1, 7>(0, 0);
459 Cq2.block<1, 7>(index, 0) = Cq2_temp.block<1, 7>(0, 0);
460
461 Qc(index) = Qc_temp(0);
462
463 C(index) = relM.pos.x();
464 C_dt(index) = relM_dt.pos.x();
465 C_dtdt(index) = relM_dtdt.pos.x();
466
467 Ct(index) = Ct_temp.pos.x();
468
469 index++;
470 }
471
472 if (mask.Constr_Y().IsActive()) {
473 Cq1.block<1, 7>(index, 0) = Cq1_temp.block<1, 7>(1, 0);
474 Cq2.block<1, 7>(index, 0) = Cq2_temp.block<1, 7>(1, 0);
475
476 Qc(index) = Qc_temp(1);
477
478 C(index) = relM.pos.y();
479 C_dt(index) = relM_dt.pos.y();
480 C_dtdt(index) = relM_dtdt.pos.y();
481
482 Ct(index) = Ct_temp.pos.y();
483
484 index++;
485 }
486
487 if (mask.Constr_Z().IsActive()) {
488 Cq1.block<1, 7>(index, 0) = Cq1_temp.block<1, 7>(2, 0);
489 Cq2.block<1, 7>(index, 0) = Cq2_temp.block<1, 7>(2, 0);
490
491 Qc(index) = Qc_temp(2);
492
493 C(index) = relM.pos.z();
494 C_dt(index) = relM_dt.pos.z();
495 C_dtdt(index) = relM_dtdt.pos.z();
496
497 Ct(index) = Ct_temp.pos.z();
498
499 index++;
500 }
501
502 if (mask.Constr_E0().IsActive()) {
503 Cq1.block<1, 4>(index, 3) = Cq1_temp.block<1, 4>(3, 3);
504 Cq2.block<1, 4>(index, 3) = Cq2_temp.block<1, 4>(3, 3);
505
506 Qc(index) = Qc_temp(3);
507
508 C(index) = relM.rot.e0();
509 C_dt(index) = relM_dt.rot.e0();
510 C_dtdt(index) = relM_dtdt.rot.e0();
511
512 Ct(index) = Ct_temp.rot.e0();
513
514 index++;
515 }
516
517 if (mask.Constr_E1().IsActive()) {
518 Cq1.block<1, 4>(index, 3) = Cq1_temp.block<1, 4>(4, 3);
519 Cq2.block<1, 4>(index, 3) = Cq2_temp.block<1, 4>(4, 3);
520
521 Qc(index) = Qc_temp(4);
522
523 C(index) = relM.rot.e1();
524 C_dt(index) = relM_dt.rot.e1();
525 C_dtdt(index) = relM_dtdt.rot.e1();
526
527 Ct(index) = Ct_temp.rot.e1();
528
529 index++;
530 }
531
532 if (mask.Constr_E2().IsActive()) {
533 Cq1.block<1, 4>(index, 3) = Cq1_temp.block<1, 4>(5, 3);
534 Cq2.block<1, 4>(index, 3) = Cq2_temp.block<1, 4>(5, 3);
535
536 Qc(index) = Qc_temp(5);
537
538 C(index) = relM.rot.e2();
539 C_dt(index) = relM_dt.rot.e2();
540 C_dtdt(index) = relM_dtdt.rot.e2();
541
542 Ct(index) = Ct_temp.rot.e2();
543
544 index++;
545 }
546
547 if (mask.Constr_E3().IsActive()) {
548 Cq1.block<1, 4>(index, 3) = Cq1_temp.block<1, 4>(6, 3);
549 Cq2.block<1, 4>(index, 3) = Cq2_temp.block<1, 4>(6, 3);
550
551 Qc(index) = Qc_temp(6);
552
553 C(index) = relM.rot.e3();
554 C_dt(index) = relM_dt.rot.e3();
555 C_dtdt(index) = relM_dtdt.rot.e3();
556
557 Ct(index) = Ct_temp.rot.e3();
558
559 index++;
560 }
561 }
562
Transform_Cq_to_Cqw(const ChLinkLock::ChConstraintMatrixX7 & mCq,ChLinkLock::ChConstraintMatrixX6 & mCqw,ChBodyFrame * mbody)563 static void Transform_Cq_to_Cqw(const ChLinkLock::ChConstraintMatrixX7& mCq,
564 ChLinkLock::ChConstraintMatrixX6& mCqw,
565 ChBodyFrame* mbody) {
566 // translational part - not changed
567 mCqw.block(0, 0, mCq.rows(), 3) = mCq.block(0, 0, mCq.rows(), 3);
568
569 // rotational part [Cq_w] = [Cq_q]*[Gl]'*1/4
570 ChGlMatrix34<> mGl(mbody->GetCoord().rot);
571 for (int colres = 0; colres < 3; colres++) {
572 for (int row = 0; row < mCq.rows(); row++) {
573 double sum = 0;
574 for (int col = 0; col < 4; col++) {
575 sum += mCq(row, col + 3) * mGl(colres, col);
576 }
577 mCqw(row, colres + 3) = sum * 0.25;
578 }
579 }
580 //// RADU: explicit loop slightly more performant than Eigen expressions
581 ////mCqw.block(0, 3, mCq.rows(), 3) = 0.25 * mCq.block(0, 3, mCq.rows(), 4) * mGl.transpose();
582 }
583
UpdateCqw()584 void ChLinkLock::UpdateCqw() {
585 Transform_Cq_to_Cqw(Cq1, Cqw1, Body1);
586 Transform_Cq_to_Cqw(Cq2, Cqw2, Body2);
587 }
588
589 // Override UpdateForces to include possible contributions from joint limits.
UpdateForces(double mytime)590 void ChLinkLock::UpdateForces(double mytime) {
591 ChLinkMarkers::UpdateForces(mytime);
592
593 ChVector<> m_force = VNULL;
594 ChVector<> m_torque = VNULL;
595
596 // COMPUTE THE FORCES IN THE LINK, FOR EXAMPLE
597 // CAUSED BY SPRINGS
598 // NOTE!!!!! C_force and C_torque are considered in the reference coordsystem
599 // of marker2 (the MAIN marker), and their application point is considered the
600 // origin of marker1 (the SLAVE marker)
601
602 // 1)========== the generic spring-damper
603
604 if (force_D && force_D->IsActive()) {
605 double dfor;
606 dfor = force_D->GetForce((dist - d_restlength), dist_dt, ChTime);
607 m_force = Vmul(Vnorm(relM.pos), dfor);
608
609 C_force = Vadd(C_force, m_force);
610 }
611
612 // 2)========== the generic torsional spring / torsional damper
613
614 if (force_R && force_R->IsActive()) {
615 double tor;
616 // 1) the tors. spring
617 tor = force_R->GetForce(relAngle, 0, ChTime);
618 m_torque = Vmul(relAxis, tor);
619 C_torque = Vadd(C_torque, m_torque);
620 // 2) the tors. damper
621 double angle_dt = Vlength(relWvel);
622 tor = force_R->GetForce(0, angle_dt, ChTime);
623 m_torque = Vmul(Vnorm(relWvel), tor);
624 C_torque = Vadd(C_torque, m_torque);
625 }
626
627 // 3)========== the XYZ forces
628
629 m_force = VNULL;
630
631 if (force_X && force_X->IsActive()) {
632 m_force.x() = force_X->GetForce(relM.pos.x(), relM_dt.pos.x(), ChTime);
633 }
634
635 if (force_Y && force_Y->IsActive()) {
636 m_force.y() = force_Y->GetForce(relM.pos.y(), relM_dt.pos.y(), ChTime);
637 }
638
639 if (force_Z && force_Z->IsActive()) {
640 m_force.z() = force_Z->GetForce(relM.pos.z(), relM_dt.pos.z(), ChTime);
641 }
642
643 C_force = Vadd(C_force, m_force);
644
645 // 4)========== the RxRyRz forces (torques)
646
647 m_torque = VNULL;
648
649 if (force_Rx && force_Rx->IsActive()) {
650 m_torque.x() = force_Rx->GetForce(relRotaxis.x(), relWvel.x(), ChTime);
651 }
652
653 if (force_Ry && force_Ry->IsActive()) {
654 m_torque.y() = force_Ry->GetForce(relRotaxis.y(), relWvel.y(), ChTime);
655 }
656
657 if (force_Rz && force_Rz->IsActive()) {
658 m_torque.z() = force_Rz->GetForce(relRotaxis.z(), relWvel.z(), ChTime);
659 }
660
661 C_torque = Vadd(C_torque, m_torque);
662
663 // ========== the link-limits "cushion forces"
664
665 m_force = VNULL;
666 m_torque = VNULL;
667
668 if (limit_X && limit_X->IsActive()) {
669 m_force.x() = limit_X->GetForce(relM.pos.x(), relM_dt.pos.x());
670 }
671 if (limit_Y && limit_Y->IsActive()) {
672 m_force.y() = limit_Y->GetForce(relM.pos.y(), relM_dt.pos.y());
673 }
674 if (limit_Z && limit_Z->IsActive()) {
675 m_force.z() = limit_Z->GetForce(relM.pos.z(), relM_dt.pos.z());
676 }
677
678 if (limit_D && limit_D->IsActive()) {
679 m_force = Vadd(m_force, Vmul(Vnorm(relM.pos), limit_D->GetForce(dist, dist_dt)));
680 }
681
682 if (limit_Rx && limit_Rx->IsActive()) {
683 m_torque.x() = limit_Rx->GetForce(relRotaxis.x(), relWvel.x());
684 }
685 if (limit_Ry && limit_Ry->IsActive()) {
686 m_torque.y() = limit_Ry->GetForce(relRotaxis.y(), relWvel.y());
687 }
688 if (limit_Rz && limit_Rz->IsActive()) {
689 m_torque.z() = limit_Rz->GetForce(relRotaxis.z(), relWvel.z());
690 }
691 if (limit_Rp && limit_Rp->IsActive()) {
692 ChVector<> arm_xaxis = VaxisXfromQuat(relM.rot); // the X axis of the marker1, respect to m2.
693 double zenith = VangleYZplaneNorm(arm_xaxis); // the angle of m1 Xaxis about normal to YZ plane
694 double polar = VangleRX(arm_xaxis); // the polar angle of m1 Xaxis spinning about m2 Xaxis
695
696 ChVector<> projected_arm(0, arm_xaxis.y(), arm_xaxis.z());
697 ChVector<> torq_axis;
698 torq_axis = Vcross(VECT_X, projected_arm);
699 torq_axis = Vnorm(torq_axis); // the axis of torque, laying on YZ plane.
700
701 double zenithspeed = Vdot(torq_axis, relWvel); // the speed of zenith rotation toward cone.
702
703 m_torque = Vadd(m_torque, Vmul(torq_axis, limit_Rp->GetPolarForce(zenith, zenithspeed, polar)));
704 }
705
706 C_force = Vadd(C_force, m_force); // +++
707 C_torque = Vadd(C_torque, m_torque); // +++
708
709 // ========== other forces??
710 }
711
712 // Count also unilateral constraints from joint limits (if any)
GetDOC_d()713 int ChLinkLock::GetDOC_d() {
714 int mdocd = ndoc_d;
715
716 if (limit_X && limit_X->IsActive()) {
717 if (limit_X->constr_lower.IsActive())
718 ++mdocd;
719 if (limit_X->constr_upper.IsActive())
720 ++mdocd;
721 }
722 if (limit_Y && limit_Y->IsActive()) {
723 if (limit_Y->constr_lower.IsActive())
724 ++mdocd;
725 if (limit_Y->constr_upper.IsActive())
726 ++mdocd;
727 }
728 if (limit_Z && limit_Z->IsActive()) {
729 if (limit_Z->constr_lower.IsActive())
730 ++mdocd;
731 if (limit_Z->constr_upper.IsActive())
732 ++mdocd;
733 }
734 if (limit_Rx && limit_Rx->IsActive()) {
735 if (limit_Rx->constr_lower.IsActive())
736 ++mdocd;
737 if (limit_Rx->constr_upper.IsActive())
738 ++mdocd;
739 }
740 if (limit_Ry && limit_Ry->IsActive()) {
741 if (limit_Ry->constr_lower.IsActive())
742 ++mdocd;
743 if (limit_Ry->constr_upper.IsActive())
744 ++mdocd;
745 }
746 if (limit_Rz && limit_Rz->IsActive()) {
747 if (limit_Rz->constr_lower.IsActive())
748 ++mdocd;
749 if (limit_Rz->constr_upper.IsActive())
750 ++mdocd;
751 }
752
753 return mdocd;
754 }
755
IntStateScatterReactions(const unsigned int off_L,const ChVectorDynamic<> & L)756 void ChLinkLock::IntStateScatterReactions(const unsigned int off_L, const ChVectorDynamic<>& L) {
757 react_force = VNULL;
758 react_torque = VNULL;
759
760 react = L.segment(off_L, react.size());
761
762 // From react vector to the 'intuitive' react_force and react_torque
763 const ChQuaternion<>& q2 = Body2->GetRot();
764 const ChQuaternion<>& q1p = marker1->GetAbsCoord().rot;
765 const ChQuaternion<>& qs = marker2->GetCoord().rot;
766 const ChMatrix33<>& Cs = marker2->GetA();
767
768 ChMatrix44<> Chi__q1p_barT; //[Chi] * [transpose(bar(q1p))]
769 Chi__q1p_barT(0, 0) = q1p.e0();
770 Chi__q1p_barT(0, 1) = q1p.e1();
771 Chi__q1p_barT(0, 2) = q1p.e2();
772 Chi__q1p_barT(0, 3) = q1p.e3();
773 Chi__q1p_barT(1, 0) = q1p.e1();
774 Chi__q1p_barT(1, 1) = -q1p.e0();
775 Chi__q1p_barT(1, 2) = q1p.e3();
776 Chi__q1p_barT(1, 3) = -q1p.e2();
777 Chi__q1p_barT(2, 0) = q1p.e2();
778 Chi__q1p_barT(2, 1) = -q1p.e3();
779 Chi__q1p_barT(2, 2) = -q1p.e0();
780 Chi__q1p_barT(2, 3) = q1p.e1();
781 Chi__q1p_barT(3, 0) = q1p.e3();
782 Chi__q1p_barT(3, 1) = q1p.e2();
783 Chi__q1p_barT(3, 2) = -q1p.e1();
784 Chi__q1p_barT(3, 3) = -q1p.e0();
785
786 ChMatrix44<> qs_tilde;
787 qs_tilde(0, 0) = qs.e0();
788 qs_tilde(0, 1) = -qs.e1();
789 qs_tilde(0, 2) = -qs.e2();
790 qs_tilde(0, 3) = -qs.e3();
791 qs_tilde(1, 0) = qs.e1();
792 qs_tilde(1, 1) = qs.e0();
793 qs_tilde(1, 2) = -qs.e3();
794 qs_tilde(1, 3) = qs.e2();
795 qs_tilde(2, 0) = qs.e2();
796 qs_tilde(2, 1) = qs.e3();
797 qs_tilde(2, 2) = qs.e0();
798 qs_tilde(2, 3) = -qs.e1();
799 qs_tilde(3, 0) = qs.e3();
800 qs_tilde(3, 1) = -qs.e2();
801 qs_tilde(3, 2) = qs.e1();
802 qs_tilde(3, 3) = qs.e0();
803
804 // Ts = 0.5*CsT*G(q2)*Chi*(q1 qp)_barT*qs~*KT*lambda
805 ChGlMatrix34<> Gl_q2(q2);
806 ChMatrix34<> Ts = 0.25 * Cs.transpose() * Gl_q2 * Chi__q1p_barT * qs_tilde;
807
808 // Translational constraint reaction force = -lambda_translational
809 // Translational constraint reaction torque = -d~''(t)*lambda_translational
810 // No reaction force from the rotational constraints
811
812 int local_off = 0;
813
814 if (mask.Constr_X().IsActive()) {
815 react_force.x() = -react(local_off);
816 react_torque.y() = -relM.pos.z() * react(local_off);
817 react_torque.z() = relM.pos.y() * react(local_off);
818 local_off++;
819 }
820 if (mask.Constr_Y().IsActive()) {
821 react_force.y() = -react(local_off);
822 react_torque.x() = relM.pos.z() * react(local_off);
823 react_torque.z() += -relM.pos.x() * react(local_off);
824 local_off++;
825 }
826 if (mask.Constr_Z().IsActive()) {
827 react_force.z() = -react(local_off);
828 react_torque.x() += -relM.pos.y() * react(local_off);
829 react_torque.y() += relM.pos.x() * react(local_off);
830 local_off++;
831 }
832
833 if (mask.Constr_E1().IsActive()) {
834 react_torque.x() += Ts(0, 1) * (react(local_off));
835 react_torque.y() += Ts(1, 1) * (react(local_off));
836 react_torque.z() += Ts(2, 1) * (react(local_off));
837 local_off++;
838 }
839 if (mask.Constr_E2().IsActive()) {
840 react_torque.x() += Ts(0, 2) * (react(local_off));
841 react_torque.y() += Ts(1, 2) * (react(local_off));
842 react_torque.z() += Ts(2, 2) * (react(local_off));
843 local_off++;
844 }
845 if (mask.Constr_E3().IsActive()) {
846 react_torque.x() += Ts(0, 3) * (react(local_off));
847 react_torque.y() += Ts(1, 3) * (react(local_off));
848 react_torque.z() += Ts(2, 3) * (react(local_off));
849 local_off++;
850 }
851
852 // ***TO DO***?: TRASFORMATION FROM delta COORDS TO LINK COORDS, if
853 // non-default delta
854 // if delta rotation?
855
856 // add also the contribution from link limits to the react_force and
857 // react_torque.
858 if (limit_X && limit_X->IsActive()) {
859 if (limit_X->constr_lower.IsActive()) {
860 react_force.x() -= L(off_L + local_off);
861 local_off++;
862 }
863 if (limit_X->constr_upper.IsActive()) {
864 react_force.x() += L(off_L + local_off);
865 local_off++;
866 }
867 }
868 if (limit_Y && limit_Y->IsActive()) {
869 if (limit_Y->constr_lower.IsActive()) {
870 react_force.y() -= L(off_L + local_off);
871 local_off++;
872 }
873 if (limit_Y->constr_upper.IsActive()) {
874 react_force.y() += L(off_L + local_off);
875 local_off++;
876 }
877 }
878 if (limit_Z && limit_Z->IsActive()) {
879 if (limit_Z->constr_lower.IsActive()) {
880 react_force.z() -= L(off_L + local_off);
881 local_off++;
882 }
883 if (limit_Z->constr_upper.IsActive()) {
884 react_force.z() += L(off_L + local_off);
885 local_off++;
886 }
887 }
888 if (limit_Rx && limit_Rx->IsActive()) {
889 if (limit_Rx->constr_lower.IsActive()) {
890 react_torque.x() -= 0.5 * L(off_L + local_off);
891 local_off++;
892 }
893 if (limit_Rx->constr_upper.IsActive()) {
894 react_torque.x() += 0.5 * L(off_L + local_off);
895 local_off++;
896 }
897 }
898 if (limit_Ry && limit_Ry->IsActive()) {
899 if (limit_Ry->constr_lower.IsActive()) {
900 react_torque.y() -= 0.5 * L(off_L + local_off);
901 local_off++;
902 }
903 if (limit_Ry->constr_upper.IsActive()) {
904 react_torque.y() += 0.5 * L(off_L + local_off);
905 local_off++;
906 }
907 }
908 if (limit_Rz && limit_Rz->IsActive()) {
909 if (limit_Rz->constr_lower.IsActive()) {
910 react_torque.z() -= 0.5 * L(off_L + local_off);
911 local_off++;
912 }
913 if (limit_Rz->constr_upper.IsActive()) {
914 react_torque.z() += 0.5 * L(off_L + local_off);
915 local_off++;
916 }
917 }
918
919 // the internal forces add their contribute to the reactions
920 // NOT NEEDED?, since C_force and react_force must stay separated???
921 // react_force = Vadd(react_force, C_force);
922 // react_torque = Vadd(react_torque, C_torque);
923 }
924
IntStateGatherReactions(const unsigned int off_L,ChVectorDynamic<> & L)925 void ChLinkLock::IntStateGatherReactions(const unsigned int off_L, ChVectorDynamic<>& L) {
926 L.segment(off_L, react.size()) = react;
927
928 ////int local_off = this->GetDOC_c();
929
930 // gather also the contribution from link limits
931 // TODO not yet implemented
932 }
933
IntLoadResidual_CqL(const unsigned int off_L,ChVectorDynamic<> & R,const ChVectorDynamic<> & L,const double c)934 void ChLinkLock::IntLoadResidual_CqL(const unsigned int off_L, // offset in L multipliers
935 ChVectorDynamic<>& R, // result: the R residual, R += c*Cq'*L
936 const ChVectorDynamic<>& L, // the L vector
937 const double c) // a scaling factor
938 {
939 int cnt = 0;
940 for (int i = 0; i < mask.nconstr; i++) {
941 if (mask.Constr_N(i).IsActive()) {
942 mask.Constr_N(i).MultiplyTandAdd(R, L(off_L + cnt) * c);
943 cnt++;
944 }
945 }
946
947 int local_offset = this->GetDOC_c();
948
949 if (limit_X && limit_X->IsActive()) {
950 if (limit_X->constr_lower.IsActive()) {
951 limit_X->constr_lower.MultiplyTandAdd(R, L(off_L + local_offset) * c);
952 ++local_offset;
953 }
954 if (limit_X->constr_upper.IsActive()) {
955 limit_X->constr_upper.MultiplyTandAdd(R, L(off_L + local_offset) * c);
956 ++local_offset;
957 }
958 }
959 if (limit_Y && limit_Y->IsActive()) {
960 if (limit_Y->constr_lower.IsActive()) {
961 limit_Y->constr_lower.MultiplyTandAdd(R, L(off_L + local_offset) * c);
962 ++local_offset;
963 }
964 if (limit_Y->constr_upper.IsActive()) {
965 limit_Y->constr_upper.MultiplyTandAdd(R, L(off_L + local_offset) * c);
966 ++local_offset;
967 }
968 }
969 if (limit_Z && limit_Z->IsActive()) {
970 if (limit_Z->constr_lower.IsActive()) {
971 limit_Z->constr_lower.MultiplyTandAdd(R, L(off_L + local_offset) * c);
972 ++local_offset;
973 }
974 if (limit_Z->constr_upper.IsActive()) {
975 limit_Z->constr_upper.MultiplyTandAdd(R, L(off_L + local_offset) * c);
976 ++local_offset;
977 }
978 }
979 if (limit_Rx && limit_Rx->IsActive()) {
980 if (limit_Rx->constr_lower.IsActive()) {
981 limit_Rx->constr_lower.MultiplyTandAdd(R, L(off_L + local_offset) * c);
982 ++local_offset;
983 }
984 if (limit_Rx->constr_upper.IsActive()) {
985 limit_Rx->constr_upper.MultiplyTandAdd(R, L(off_L + local_offset) * c);
986 ++local_offset;
987 }
988 }
989 if (limit_Ry && limit_Ry->IsActive()) {
990 if (limit_Ry->constr_lower.IsActive()) {
991 limit_Ry->constr_lower.MultiplyTandAdd(R, L(off_L + local_offset) * c);
992 ++local_offset;
993 }
994 if (limit_Ry->constr_upper.IsActive()) {
995 limit_Ry->constr_upper.MultiplyTandAdd(R, L(off_L + local_offset) * c);
996 ++local_offset;
997 }
998 }
999 if (limit_Rz && limit_Rz->IsActive()) {
1000 if (limit_Rz->constr_lower.IsActive()) {
1001 limit_Rz->constr_lower.MultiplyTandAdd(R, L(off_L + local_offset) * c);
1002 ++local_offset;
1003 }
1004 if (limit_Rz->constr_upper.IsActive()) {
1005 limit_Rz->constr_upper.MultiplyTandAdd(R, L(off_L + local_offset) * c);
1006 ++local_offset;
1007 }
1008 }
1009 }
1010
IntLoadConstraint_C(const unsigned int off_L,ChVectorDynamic<> & Qc,const double c,bool do_clamp,double recovery_clamp)1011 void ChLinkLock::IntLoadConstraint_C(const unsigned int off_L, // offset in Qc residual
1012 ChVectorDynamic<>& Qc, // result: the Qc residual, Qc += c*C
1013 const double c, // a scaling factor
1014 bool do_clamp, // apply clamping to c*C?
1015 double recovery_clamp) // value for min/max clamping of c*C
1016 {
1017 int cnt = 0;
1018 for (int i = 0; i < mask.nconstr; i++) {
1019 if (mask.Constr_N(i).IsActive()) {
1020 if (do_clamp) {
1021 if (mask.Constr_N(i).IsUnilateral())
1022 Qc(off_L + cnt) += ChMax(c * C(cnt), -recovery_clamp);
1023 else
1024 Qc(off_L + cnt) += ChMin(ChMax(c * C(cnt), -recovery_clamp), recovery_clamp);
1025 } else
1026 Qc(off_L + cnt) += c * C(cnt);
1027 cnt++;
1028 }
1029 }
1030
1031 if (!do_clamp)
1032 recovery_clamp = 1e24;
1033
1034 int local_offset = this->GetDOC_c();
1035
1036 if (limit_X && limit_X->IsActive()) {
1037 if (limit_X->constr_lower.IsActive()) {
1038 Qc(off_L + local_offset) += ChMax(c * (-limit_X->GetMin() + relM.pos.x()), -recovery_clamp);
1039 ++local_offset;
1040 }
1041 if (limit_X->constr_upper.IsActive()) {
1042 Qc(off_L + local_offset) += ChMax(c * (limit_X->GetMax() - relM.pos.x()), -recovery_clamp);
1043 ++local_offset;
1044 }
1045 }
1046 if (limit_Y && limit_Y->IsActive()) {
1047 if (limit_Y->constr_lower.IsActive()) {
1048 Qc(off_L + local_offset) += ChMax(c * (-limit_Y->GetMin() + relM.pos.y()), -recovery_clamp);
1049 ++local_offset;
1050 }
1051 if (limit_Y->constr_upper.IsActive()) {
1052 Qc(off_L + local_offset) += ChMax(c * (limit_Y->GetMax() - relM.pos.y()), -recovery_clamp);
1053 ++local_offset;
1054 }
1055 }
1056 if (limit_Z && limit_Z->IsActive()) {
1057 if (limit_Z->constr_lower.IsActive()) {
1058 Qc(off_L + local_offset) += ChMax(c * (-limit_Z->GetMin() + relM.pos.z()), -recovery_clamp);
1059 ++local_offset;
1060 }
1061 if (limit_Z->constr_upper.IsActive()) {
1062 Qc(off_L + local_offset) += ChMax(c * (limit_Z->GetMax() - relM.pos.z()), -recovery_clamp);
1063 ++local_offset;
1064 }
1065 }
1066 if (limit_Rx && limit_Rx->IsActive()) {
1067 if (limit_Rx->constr_lower.IsActive()) {
1068 Qc(off_L + local_offset) += ChMax(c * (-sin(0.5 * limit_Rx->GetMin()) + relM.rot.e1()), -recovery_clamp);
1069 ++local_offset;
1070 }
1071 if (limit_Rx->constr_upper.IsActive()) {
1072 Qc(off_L + local_offset) += ChMax(c * (sin(0.5 * limit_Rx->GetMax()) - relM.rot.e1()), -recovery_clamp);
1073 ++local_offset;
1074 }
1075 }
1076 if (limit_Ry && limit_Ry->IsActive()) {
1077 if (limit_Ry->constr_lower.IsActive()) {
1078 Qc(off_L + local_offset) += ChMax(c * (-sin(0.5 * limit_Ry->GetMin()) + relM.rot.e2()), -recovery_clamp);
1079 ++local_offset;
1080 }
1081 if (limit_Ry->constr_upper.IsActive()) {
1082 Qc(off_L + local_offset) += ChMax(c * (sin(0.5 * limit_Ry->GetMax()) - relM.rot.e2()), -recovery_clamp);
1083 ++local_offset;
1084 }
1085 }
1086 if (limit_Rz && limit_Rz->IsActive()) {
1087 if (limit_Rz->constr_lower.IsActive()) {
1088 Qc(off_L + local_offset) += ChMax(c * (-sin(0.5 * limit_Rz->GetMin()) + relM.rot.e3()), -recovery_clamp);
1089 ++local_offset;
1090 }
1091 if (limit_Rz->constr_upper.IsActive()) {
1092 Qc(off_L + local_offset) += ChMax(c * (sin(0.5 * limit_Rz->GetMax()) - relM.rot.e3()), -recovery_clamp);
1093 ++local_offset;
1094 }
1095 }
1096 }
1097
IntLoadConstraint_Ct(const unsigned int off_L,ChVectorDynamic<> & Qc,const double c)1098 void ChLinkLock::IntLoadConstraint_Ct(const unsigned int off_L, // offset in Qc residual
1099 ChVectorDynamic<>& Qc, // result: the Qc residual, Qc += c*Ct
1100 const double c) // a scaling factor
1101 {
1102 int cnt = 0;
1103 for (int i = 0; i < mask.nconstr; i++) {
1104 if (mask.Constr_N(i).IsActive()) {
1105 Qc(off_L + cnt) += c * Ct(cnt);
1106 cnt++;
1107 }
1108 }
1109
1110 // nothing to do for ChLinkLimit
1111 }
1112
IntToDescriptor(const unsigned int off_v,const ChStateDelta & v,const ChVectorDynamic<> & R,const unsigned int off_L,const ChVectorDynamic<> & L,const ChVectorDynamic<> & Qc)1113 void ChLinkLock::IntToDescriptor(const unsigned int off_v,
1114 const ChStateDelta& v,
1115 const ChVectorDynamic<>& R,
1116 const unsigned int off_L,
1117 const ChVectorDynamic<>& L,
1118 const ChVectorDynamic<>& Qc) {
1119 int cnt = 0;
1120 for (int i = 0; i < mask.nconstr; i++) {
1121 if (mask.Constr_N(i).IsActive()) {
1122 mask.Constr_N(i).Set_l_i(L(off_L + cnt));
1123 mask.Constr_N(i).Set_b_i(Qc(off_L + cnt));
1124 cnt++;
1125 }
1126 }
1127
1128 int local_offset = this->GetDOC_c();
1129
1130 if (limit_X && limit_X->IsActive()) {
1131 if (limit_X->constr_lower.IsActive()) {
1132 limit_X->constr_lower.Set_l_i(L(off_L + local_offset));
1133 limit_X->constr_lower.Set_b_i(Qc(off_L + local_offset));
1134 ++local_offset;
1135 }
1136 if (limit_X->constr_upper.IsActive()) {
1137 limit_X->constr_upper.Set_l_i(L(off_L + local_offset));
1138 limit_X->constr_upper.Set_b_i(Qc(off_L + local_offset));
1139 ++local_offset;
1140 }
1141 }
1142 if (limit_Y && limit_Y->IsActive()) {
1143 if (limit_Y->constr_lower.IsActive()) {
1144 limit_Y->constr_lower.Set_l_i(L(off_L + local_offset));
1145 limit_Y->constr_lower.Set_b_i(Qc(off_L + local_offset));
1146 ++local_offset;
1147 }
1148 if (limit_Y->constr_upper.IsActive()) {
1149 limit_Y->constr_upper.Set_l_i(L(off_L + local_offset));
1150 limit_Y->constr_upper.Set_b_i(Qc(off_L + local_offset));
1151 ++local_offset;
1152 }
1153 }
1154 if (limit_Z && limit_Z->IsActive()) {
1155 if (limit_Z->constr_lower.IsActive()) {
1156 limit_Z->constr_lower.Set_l_i(L(off_L + local_offset));
1157 limit_Z->constr_lower.Set_b_i(Qc(off_L + local_offset));
1158 ++local_offset;
1159 }
1160 if (limit_Z->constr_upper.IsActive()) {
1161 limit_Z->constr_upper.Set_l_i(L(off_L + local_offset));
1162 limit_Z->constr_upper.Set_b_i(Qc(off_L + local_offset));
1163 ++local_offset;
1164 }
1165 }
1166 if (limit_Rx && limit_Rx->IsActive()) {
1167 if (limit_Rx->constr_lower.IsActive()) {
1168 limit_Rx->constr_lower.Set_l_i(L(off_L + local_offset));
1169 limit_Rx->constr_lower.Set_b_i(Qc(off_L + local_offset));
1170 ++local_offset;
1171 }
1172 if (limit_Rx->constr_upper.IsActive()) {
1173 limit_Rx->constr_upper.Set_l_i(L(off_L + local_offset));
1174 limit_Rx->constr_upper.Set_b_i(Qc(off_L + local_offset));
1175 ++local_offset;
1176 }
1177 }
1178 if (limit_Ry && limit_Ry->IsActive()) {
1179 if (limit_Ry->constr_lower.IsActive()) {
1180 limit_Ry->constr_lower.Set_l_i(L(off_L + local_offset));
1181 limit_Ry->constr_lower.Set_b_i(Qc(off_L + local_offset));
1182 ++local_offset;
1183 }
1184 if (limit_Ry->constr_upper.IsActive()) {
1185 limit_Ry->constr_upper.Set_l_i(L(off_L + local_offset));
1186 limit_Ry->constr_upper.Set_b_i(Qc(off_L + local_offset));
1187 ++local_offset;
1188 }
1189 }
1190 if (limit_Rz && limit_Rz->IsActive()) {
1191 if (limit_Rz->constr_lower.IsActive()) {
1192 limit_Rz->constr_lower.Set_l_i(L(off_L + local_offset));
1193 limit_Rz->constr_lower.Set_b_i(Qc(off_L + local_offset));
1194 ++local_offset;
1195 }
1196 if (limit_Rz->constr_upper.IsActive()) {
1197 limit_Rz->constr_upper.Set_l_i(L(off_L + local_offset));
1198 limit_Rz->constr_upper.Set_b_i(Qc(off_L + local_offset));
1199 ++local_offset;
1200 }
1201 }
1202 }
1203
IntFromDescriptor(const unsigned int off_v,ChStateDelta & v,const unsigned int off_L,ChVectorDynamic<> & L)1204 void ChLinkLock::IntFromDescriptor(const unsigned int off_v,
1205 ChStateDelta& v,
1206 const unsigned int off_L,
1207 ChVectorDynamic<>& L) {
1208 int cnt = 0;
1209 for (int i = 0; i < mask.nconstr; i++) {
1210 if (mask.Constr_N(i).IsActive()) {
1211 L(off_L + cnt) = mask.Constr_N(i).Get_l_i();
1212 cnt++;
1213 }
1214 }
1215
1216 int local_offset = this->GetDOC_c();
1217
1218 if (limit_X && limit_X->IsActive()) {
1219 if (limit_X->constr_lower.IsActive()) {
1220 L(off_L + local_offset) = limit_X->constr_lower.Get_l_i();
1221 ++local_offset;
1222 }
1223 if (limit_X->constr_upper.IsActive()) {
1224 L(off_L + local_offset) = limit_X->constr_upper.Get_l_i();
1225 ++local_offset;
1226 }
1227 }
1228 if (limit_Y && limit_Y->IsActive()) {
1229 if (limit_Y->constr_lower.IsActive()) {
1230 L(off_L + local_offset) = limit_Y->constr_lower.Get_l_i();
1231 ++local_offset;
1232 }
1233 if (limit_Y->constr_upper.IsActive()) {
1234 L(off_L + local_offset) = limit_Y->constr_upper.Get_l_i();
1235 ++local_offset;
1236 }
1237 }
1238 if (limit_Z && limit_Z->IsActive()) {
1239 if (limit_Z->constr_lower.IsActive()) {
1240 L(off_L + local_offset) = limit_Z->constr_lower.Get_l_i();
1241 ++local_offset;
1242 }
1243 if (limit_Z->constr_upper.IsActive()) {
1244 L(off_L + local_offset) = limit_Z->constr_upper.Get_l_i();
1245 ++local_offset;
1246 }
1247 }
1248 if (limit_Rx && limit_Rx->IsActive()) {
1249 if (limit_Rx->constr_lower.IsActive()) {
1250 L(off_L + local_offset) = limit_Rx->constr_lower.Get_l_i();
1251 ++local_offset;
1252 }
1253 if (limit_Rx->constr_upper.IsActive()) {
1254 L(off_L + local_offset) = limit_Rx->constr_upper.Get_l_i();
1255 ++local_offset;
1256 }
1257 }
1258 if (limit_Ry && limit_Ry->IsActive()) {
1259 if (limit_Ry->constr_lower.IsActive()) {
1260 L(off_L + local_offset) = limit_Ry->constr_lower.Get_l_i();
1261 ++local_offset;
1262 }
1263 if (limit_Ry->constr_upper.IsActive()) {
1264 L(off_L + local_offset) = limit_Ry->constr_upper.Get_l_i();
1265 ++local_offset;
1266 }
1267 }
1268 if (limit_Rz && limit_Rz->IsActive()) {
1269 if (limit_Rz->constr_lower.IsActive()) {
1270 L(off_L + local_offset) = limit_Rz->constr_lower.Get_l_i();
1271 ++local_offset;
1272 }
1273 if (limit_Rz->constr_upper.IsActive()) {
1274 L(off_L + local_offset) = limit_Rz->constr_upper.Get_l_i();
1275 ++local_offset;
1276 }
1277 }
1278 }
1279
InjectConstraints(ChSystemDescriptor & mdescriptor)1280 void ChLinkLock::InjectConstraints(ChSystemDescriptor& mdescriptor) {
1281 if (!this->IsActive())
1282 return;
1283
1284 for (int i = 0; i < mask.nconstr; i++) {
1285 if (mask.Constr_N(i).IsActive())
1286 mdescriptor.InsertConstraint(&mask.Constr_N(i));
1287 }
1288
1289 if (limit_X && limit_X->IsActive()) {
1290 if (limit_X->constr_lower.IsActive()) {
1291 limit_X->constr_lower.SetVariables(&Body1->Variables(), &Body2->Variables());
1292 mdescriptor.InsertConstraint(&limit_X->constr_lower);
1293 }
1294 if (limit_X->constr_upper.IsActive()) {
1295 limit_X->constr_upper.SetVariables(&Body1->Variables(), &Body2->Variables());
1296 mdescriptor.InsertConstraint(&limit_X->constr_upper);
1297 }
1298 }
1299 if (limit_Y && limit_Y->IsActive()) {
1300 if (limit_Y->constr_lower.IsActive()) {
1301 limit_Y->constr_lower.SetVariables(&Body1->Variables(), &Body2->Variables());
1302 mdescriptor.InsertConstraint(&limit_Y->constr_lower);
1303 }
1304 if (limit_Y->constr_upper.IsActive()) {
1305 limit_Y->constr_upper.SetVariables(&Body1->Variables(), &Body2->Variables());
1306 mdescriptor.InsertConstraint(&limit_Y->constr_upper);
1307 }
1308 }
1309 if (limit_Z && limit_Z->IsActive()) {
1310 if (limit_Z->constr_lower.IsActive()) {
1311 limit_Z->constr_lower.SetVariables(&Body1->Variables(), &Body2->Variables());
1312 mdescriptor.InsertConstraint(&limit_Z->constr_lower);
1313 }
1314 if (limit_Z->constr_upper.IsActive()) {
1315 limit_Z->constr_upper.SetVariables(&Body1->Variables(), &Body2->Variables());
1316 mdescriptor.InsertConstraint(&limit_Z->constr_upper);
1317 }
1318 }
1319 if (limit_Rx && limit_Rx->IsActive()) {
1320 if (limit_Rx->constr_lower.IsActive()) {
1321 limit_Rx->constr_lower.SetVariables(&Body1->Variables(), &Body2->Variables());
1322 mdescriptor.InsertConstraint(&limit_Rx->constr_lower);
1323 }
1324 if (limit_Rx->constr_upper.IsActive()) {
1325 limit_Rx->constr_upper.SetVariables(&Body1->Variables(), &Body2->Variables());
1326 mdescriptor.InsertConstraint(&limit_Rx->constr_upper);
1327 }
1328 }
1329 if (limit_Ry && limit_Ry->IsActive()) {
1330 if (limit_Ry->constr_lower.IsActive()) {
1331 limit_Ry->constr_lower.SetVariables(&Body1->Variables(), &Body2->Variables());
1332 mdescriptor.InsertConstraint(&limit_Ry->constr_lower);
1333 }
1334 if (limit_Ry->constr_upper.IsActive()) {
1335 limit_Ry->constr_upper.SetVariables(&Body1->Variables(), &Body2->Variables());
1336 mdescriptor.InsertConstraint(&limit_Ry->constr_upper);
1337 }
1338 }
1339 if (limit_Rz && limit_Rz->IsActive()) {
1340 if (limit_Rz->constr_lower.IsActive()) {
1341 limit_Rz->constr_lower.SetVariables(&Body1->Variables(), &Body2->Variables());
1342 mdescriptor.InsertConstraint(&limit_Rz->constr_lower);
1343 }
1344 if (limit_Rz->constr_upper.IsActive()) {
1345 limit_Rz->constr_upper.SetVariables(&Body1->Variables(), &Body2->Variables());
1346 mdescriptor.InsertConstraint(&limit_Rz->constr_upper);
1347 }
1348 }
1349 }
1350
ConstraintsBiReset()1351 void ChLinkLock::ConstraintsBiReset() {
1352 for (int i = 0; i < mask.nconstr; i++) {
1353 mask.Constr_N(i).Set_b_i(0.);
1354 }
1355
1356 if (limit_X && limit_X->IsActive()) {
1357 if (limit_X->constr_lower.IsActive()) {
1358 limit_X->constr_lower.Set_b_i(0.);
1359 }
1360 if (limit_X->constr_upper.IsActive()) {
1361 limit_X->constr_upper.Set_b_i(0.);
1362 }
1363 }
1364 if (limit_Y && limit_Y->IsActive()) {
1365 if (limit_Y->constr_lower.IsActive()) {
1366 limit_Y->constr_lower.Set_b_i(0.);
1367 }
1368 if (limit_Y->constr_upper.IsActive()) {
1369 limit_Y->constr_upper.Set_b_i(0.);
1370 }
1371 }
1372 if (limit_Z && limit_Z->IsActive()) {
1373 if (limit_Z->constr_lower.IsActive()) {
1374 limit_Z->constr_lower.Set_b_i(0.);
1375 }
1376 if (limit_Z->constr_upper.IsActive()) {
1377 limit_Z->constr_upper.Set_b_i(0.);
1378 }
1379 }
1380 if (limit_Rx && limit_Rx->IsActive()) {
1381 if (limit_Rx->constr_lower.IsActive()) {
1382 limit_Rx->constr_lower.Set_b_i(0.);
1383 }
1384 if (limit_Rx->constr_upper.IsActive()) {
1385 limit_Rx->constr_upper.Set_b_i(0.);
1386 }
1387 }
1388 if (limit_Ry && limit_Ry->IsActive()) {
1389 if (limit_Ry->constr_lower.IsActive()) {
1390 limit_Ry->constr_lower.Set_b_i(0.);
1391 }
1392 if (limit_Ry->constr_upper.IsActive()) {
1393 limit_Ry->constr_upper.Set_b_i(0.);
1394 }
1395 }
1396 if (limit_Rz && limit_Rz->IsActive()) {
1397 if (limit_Rz->constr_lower.IsActive()) {
1398 limit_Rz->constr_lower.Set_b_i(0.);
1399 }
1400 if (limit_Rz->constr_upper.IsActive()) {
1401 limit_Rz->constr_upper.Set_b_i(0.);
1402 }
1403 }
1404 }
1405
ConstraintsBiLoad_C(double factor,double recovery_clamp,bool do_clamp)1406 void ChLinkLock::ConstraintsBiLoad_C(double factor, double recovery_clamp, bool do_clamp) {
1407 int cnt = 0;
1408 for (int i = 0; i < mask.nconstr; i++) {
1409 if (mask.Constr_N(i).IsActive()) {
1410 if (do_clamp) {
1411 if (mask.Constr_N(i).IsUnilateral())
1412 mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() + ChMax(factor * C(cnt), -recovery_clamp));
1413 else
1414 mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() +
1415 ChMin(ChMax(factor * C(cnt), -recovery_clamp), recovery_clamp));
1416 } else
1417 mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() + factor * C(cnt));
1418
1419 cnt++;
1420 }
1421 }
1422
1423 if (limit_X && limit_X->IsActive()) {
1424 if (limit_X->constr_lower.IsActive()) {
1425 if (!do_clamp) {
1426 limit_X->constr_lower.Set_b_i(limit_X->constr_lower.Get_b_i() +
1427 factor * (-limit_X->GetMin() + relM.pos.x()));
1428 } else {
1429 limit_X->constr_lower.Set_b_i(limit_X->constr_lower.Get_b_i() +
1430 ChMax(factor * (-limit_X->GetMin() + relM.pos.x()), -recovery_clamp));
1431 }
1432 }
1433 if (limit_X->constr_upper.IsActive()) {
1434 if (!do_clamp) {
1435 limit_X->constr_upper.Set_b_i(limit_X->constr_upper.Get_b_i() +
1436 factor * (limit_X->GetMax() - relM.pos.x()));
1437 } else {
1438 limit_X->constr_upper.Set_b_i(limit_X->constr_upper.Get_b_i() +
1439 ChMax(factor * (limit_X->GetMax() - relM.pos.x()), -recovery_clamp));
1440 }
1441 }
1442 }
1443 if (limit_Y && limit_Y->IsActive()) {
1444 if (limit_Y->constr_lower.IsActive()) {
1445 if (!do_clamp) {
1446 limit_Y->constr_lower.Set_b_i(limit_Y->constr_lower.Get_b_i() +
1447 factor * (-limit_Y->GetMin() + relM.pos.y()));
1448 } else {
1449 limit_Y->constr_lower.Set_b_i(limit_Y->constr_lower.Get_b_i() +
1450 ChMax(factor * (-limit_Y->GetMin() + relM.pos.y()), -recovery_clamp));
1451 }
1452 }
1453 if (limit_Y->constr_upper.IsActive()) {
1454 if (!do_clamp) {
1455 limit_Y->constr_upper.Set_b_i(limit_Y->constr_upper.Get_b_i() +
1456 factor * (limit_Y->GetMax() - relM.pos.y()));
1457 } else {
1458 limit_Y->constr_upper.Set_b_i(limit_Y->constr_upper.Get_b_i() +
1459 ChMax(factor * (limit_Y->GetMax() - relM.pos.y()), -recovery_clamp));
1460 }
1461 }
1462 }
1463 if (limit_Z && limit_Z->IsActive()) {
1464 if (limit_Z->constr_lower.IsActive()) {
1465 if (!do_clamp) {
1466 limit_Z->constr_lower.Set_b_i(limit_Z->constr_lower.Get_b_i() +
1467 factor * (-limit_Z->GetMin() + relM.pos.z()));
1468 } else {
1469 limit_Z->constr_lower.Set_b_i(limit_Z->constr_lower.Get_b_i() +
1470 ChMax(factor * (-limit_Z->GetMin() + relM.pos.z()), -recovery_clamp));
1471 }
1472 }
1473 if (limit_Z->constr_upper.IsActive()) {
1474 if (!do_clamp) {
1475 limit_Z->constr_upper.Set_b_i(limit_Z->constr_upper.Get_b_i() +
1476 factor * (limit_Z->GetMax() - relM.pos.z()));
1477 } else {
1478 limit_Z->constr_upper.Set_b_i(limit_Z->constr_upper.Get_b_i() +
1479 ChMax(factor * (limit_Z->GetMax() - relM.pos.z()), -recovery_clamp));
1480 }
1481 }
1482 }
1483 if (limit_Rx && limit_Rx->IsActive()) {
1484 if (limit_Rx->constr_lower.IsActive()) {
1485 if (!do_clamp) {
1486 limit_Rx->constr_lower.Set_b_i(limit_Rx->constr_lower.Get_b_i() +
1487 factor * (-sin(0.5 * limit_Rx->GetMin()) + relM.rot.e1()));
1488 } else {
1489 limit_Rx->constr_lower.Set_b_i(
1490 limit_Rx->constr_lower.Get_b_i() +
1491 ChMax(factor * (-sin(0.5 * limit_Rx->GetMin()) + relM.rot.e1()), -recovery_clamp));
1492 }
1493 }
1494 if (limit_Rx->constr_upper.IsActive()) {
1495 if (!do_clamp) {
1496 limit_Rx->constr_upper.Set_b_i(limit_Rx->constr_upper.Get_b_i() +
1497 factor * (sin(0.5 * limit_Rx->GetMax()) - relM.rot.e1()));
1498 } else {
1499 limit_Rx->constr_upper.Set_b_i(
1500 limit_Rx->constr_upper.Get_b_i() +
1501 ChMax(factor * (sin(0.5 * limit_Rx->GetMax()) - relM.rot.e1()), -recovery_clamp));
1502 }
1503 }
1504 }
1505 if (limit_Ry && limit_Ry->IsActive()) {
1506 if (limit_Ry->constr_lower.IsActive()) {
1507 if (!do_clamp) {
1508 limit_Ry->constr_lower.Set_b_i(limit_Ry->constr_lower.Get_b_i() +
1509 factor * (-sin(0.5 * limit_Ry->GetMin()) + relM.rot.e2()));
1510 } else {
1511 limit_Ry->constr_lower.Set_b_i(
1512 limit_Ry->constr_lower.Get_b_i() +
1513 ChMax(factor * (-sin(0.5 * limit_Ry->GetMin()) + relM.rot.e2()), -recovery_clamp));
1514 }
1515 }
1516 if (limit_Ry->constr_upper.IsActive()) {
1517 if (!do_clamp) {
1518 limit_Ry->constr_upper.Set_b_i(limit_Ry->constr_upper.Get_b_i() +
1519 factor * (sin(0.5 * limit_Ry->GetMax()) - relM.rot.e2()));
1520 } else {
1521 limit_Ry->constr_upper.Set_b_i(
1522 limit_Ry->constr_upper.Get_b_i() +
1523 ChMax(factor * (sin(0.5 * limit_Ry->GetMax()) - relM.rot.e2()), -recovery_clamp));
1524 }
1525 }
1526 }
1527 if (limit_Rz && limit_Rz->IsActive()) {
1528 if (limit_Rz->constr_lower.IsActive()) {
1529 if (!do_clamp) {
1530 limit_Rz->constr_lower.Set_b_i(limit_Rz->constr_lower.Get_b_i() +
1531 factor * (-sin(0.5 * limit_Rz->GetMin()) + relM.rot.e3()));
1532 } else {
1533 limit_Rz->constr_lower.Set_b_i(
1534 limit_Rz->constr_lower.Get_b_i() +
1535 ChMax(factor * (-sin(0.5 * limit_Rz->GetMin()) + relM.rot.e3()), -recovery_clamp));
1536 }
1537 }
1538 if (limit_Rz->constr_upper.IsActive()) {
1539 if (!do_clamp) {
1540 limit_Rz->constr_upper.Set_b_i(limit_Rz->constr_upper.Get_b_i() +
1541 factor * (sin(0.5 * limit_Rz->GetMax()) - relM.rot.e3()));
1542 } else {
1543 limit_Rz->constr_upper.Set_b_i(
1544 limit_Rz->constr_upper.Get_b_i() +
1545 ChMax(factor * (sin(0.5 * limit_Rz->GetMax()) - relM.rot.e3()), -recovery_clamp));
1546 }
1547 }
1548 }
1549 }
1550
ConstraintsBiLoad_Ct(double factor)1551 void ChLinkLock::ConstraintsBiLoad_Ct(double factor) {
1552 int cnt = 0;
1553 for (int i = 0; i < mask.nconstr; i++) {
1554 if (mask.Constr_N(i).IsActive()) {
1555 mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() + factor * Ct(cnt));
1556 cnt++;
1557 }
1558 }
1559 }
1560
ConstraintsBiLoad_Qc(double factor)1561 void ChLinkLock::ConstraintsBiLoad_Qc(double factor) {
1562 int cnt = 0;
1563 for (int i = 0; i < mask.nconstr; i++) {
1564 if (mask.Constr_N(i).IsActive()) {
1565 mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() + factor * Qc(cnt));
1566 cnt++;
1567 }
1568 }
1569 }
1570
Transform_Cq_to_Cqw_row(const ChMatrixNM<double,7,BODY_QDOF> & mCq,int qrow,ChMatrixRef mCqw,int qwrow,const ChGlMatrix34<> & Gl)1571 void Transform_Cq_to_Cqw_row(const ChMatrixNM<double, 7, BODY_QDOF>& mCq, int qrow, ChMatrixRef mCqw, int qwrow, const ChGlMatrix34<>& Gl) {
1572 // translational part - not changed
1573 mCqw.block<1, 3>(qwrow, 0) = mCq.block<1, 3>(qrow, 0);
1574
1575 // rotational part [Cq_w] = [Cq_q]*[Gl]'*1/4
1576 for (int colres = 0; colres < 3; colres++) {
1577 double sum = 0;
1578 for (int col = 0; col < 4; col++) {
1579 sum += mCq(qrow, col + 3) * Gl(colres, col);
1580 }
1581 mCqw(qwrow, colres + 3) = sum * 0.25;
1582 }
1583 //// RADU: explicit loop slightly more performant than Eigen expressions
1584 ////mCqw.block<1, 3>(qwrow, 3) = 0.25 * mCq.block<1, 4>(qrow, 3) * Gl.transpose();
1585 }
1586
ConstraintsLoadJacobians()1587 void ChLinkLock::ConstraintsLoadJacobians() {
1588 if (ndoc == 0)
1589 return;
1590
1591 int cnt = 0;
1592 for (int i = 0; i < mask.nconstr; i++) {
1593 if (mask.Constr_N(i).IsActive()) {
1594 mask.Constr_N(i).Get_Cq_a().block(0, 0, 1, Cqw1.cols()) = Cqw1.block(cnt, 0, 1, Cqw1.cols());
1595 mask.Constr_N(i).Get_Cq_b().block(0, 0, 1, Cqw2.cols()) = Cqw2.block(cnt, 0, 1, Cqw2.cols());
1596 cnt++;
1597
1598 // sets also the CFM term
1599 // mask->Constr_N(i).Set_cfm_i(this->attractor);
1600 }
1601 }
1602
1603 ChGlMatrix34<> Gl1(Body1->GetCoord().rot);
1604 ChGlMatrix34<> Gl2(Body2->GetCoord().rot);
1605
1606 if (limit_X && limit_X->IsActive()) {
1607 if (limit_X->constr_lower.IsActive()) {
1608 limit_X->constr_lower.SetVariables(&Body1->Variables(), &Body2->Variables());
1609 Transform_Cq_to_Cqw_row(Cq1_temp, 0, limit_X->constr_lower.Get_Cq_a(), 0, Gl1);
1610 Transform_Cq_to_Cqw_row(Cq2_temp, 0, limit_X->constr_lower.Get_Cq_b(), 0, Gl2);
1611 }
1612 if (limit_X->constr_upper.IsActive()) {
1613 limit_X->constr_upper.SetVariables(&Body1->Variables(), &Body2->Variables());
1614 Transform_Cq_to_Cqw_row(Cq1_temp, 0, limit_X->constr_upper.Get_Cq_a(), 0, Gl1);
1615 Transform_Cq_to_Cqw_row(Cq2_temp, 0, limit_X->constr_upper.Get_Cq_b(), 0, Gl2);
1616 limit_X->constr_upper.Get_Cq_a() *= -1;
1617 limit_X->constr_upper.Get_Cq_b() *= -1;
1618 }
1619 }
1620 if (limit_Y && limit_Y->IsActive()) {
1621 if (limit_Y->constr_lower.IsActive()) {
1622 limit_Y->constr_lower.SetVariables(&Body1->Variables(), &Body2->Variables());
1623 Transform_Cq_to_Cqw_row(Cq1_temp, 1, limit_Y->constr_lower.Get_Cq_a(), 0, Gl1);
1624 Transform_Cq_to_Cqw_row(Cq2_temp, 1, limit_Y->constr_lower.Get_Cq_b(), 0, Gl2);
1625 }
1626 if (limit_Y->constr_upper.IsActive()) {
1627 limit_Y->constr_upper.SetVariables(&Body1->Variables(), &Body2->Variables());
1628 Transform_Cq_to_Cqw_row(Cq1_temp, 1, limit_Y->constr_upper.Get_Cq_a(), 0, Gl1);
1629 Transform_Cq_to_Cqw_row(Cq2_temp, 1, limit_Y->constr_upper.Get_Cq_b(), 0, Gl2);
1630 limit_Y->constr_upper.Get_Cq_a() *= -1;
1631 limit_Y->constr_upper.Get_Cq_b() *= -1;
1632 }
1633 }
1634 if (limit_Z && limit_Z->IsActive()) {
1635 if (limit_Z->constr_lower.IsActive()) {
1636 limit_Z->constr_lower.SetVariables(&Body1->Variables(), &Body2->Variables());
1637 Transform_Cq_to_Cqw_row(Cq1_temp, 2, limit_Z->constr_lower.Get_Cq_a(), 0, Gl1);
1638 Transform_Cq_to_Cqw_row(Cq2_temp, 2, limit_Z->constr_lower.Get_Cq_b(), 0, Gl2);
1639 }
1640 if (limit_Z->constr_upper.IsActive()) {
1641 limit_Z->constr_upper.SetVariables(&Body1->Variables(), &Body2->Variables());
1642 Transform_Cq_to_Cqw_row(Cq1_temp, 2, limit_Z->constr_upper.Get_Cq_a(), 0, Gl1);
1643 Transform_Cq_to_Cqw_row(Cq2_temp, 2, limit_Z->constr_upper.Get_Cq_b(), 0, Gl2);
1644 limit_Z->constr_upper.Get_Cq_a() *= -1;
1645 limit_Z->constr_upper.Get_Cq_b() *= -1;
1646 }
1647 }
1648 if (limit_Rx && limit_Rx->IsActive()) {
1649 if (limit_Rx->constr_lower.IsActive()) {
1650 limit_Rx->constr_lower.SetVariables(&Body1->Variables(), &Body2->Variables());
1651 Transform_Cq_to_Cqw_row(Cq1_temp, 4, limit_Rx->constr_lower.Get_Cq_a(), 0, Gl1);
1652 Transform_Cq_to_Cqw_row(Cq2_temp, 4, limit_Rx->constr_lower.Get_Cq_b(), 0, Gl2);
1653 }
1654 if (limit_Rx->constr_upper.IsActive()) {
1655 limit_Rx->constr_upper.SetVariables(&Body1->Variables(), &Body2->Variables());
1656 Transform_Cq_to_Cqw_row(Cq1_temp, 4, limit_Rx->constr_upper.Get_Cq_a(), 0, Gl1);
1657 Transform_Cq_to_Cqw_row(Cq2_temp, 4, limit_Rx->constr_upper.Get_Cq_b(), 0, Gl2);
1658 limit_Rx->constr_upper.Get_Cq_a() *= -1;
1659 limit_Rx->constr_upper.Get_Cq_b() *= -1;
1660 }
1661 }
1662 if (limit_Ry && limit_Ry->IsActive()) {
1663 if (limit_Ry->constr_lower.IsActive()) {
1664 limit_Ry->constr_lower.SetVariables(&Body1->Variables(), &Body2->Variables());
1665 Transform_Cq_to_Cqw_row(Cq1_temp, 5, limit_Ry->constr_lower.Get_Cq_a(), 0, Gl1);
1666 Transform_Cq_to_Cqw_row(Cq2_temp, 5, limit_Ry->constr_lower.Get_Cq_b(), 0, Gl2);
1667 }
1668 if (limit_Ry->constr_upper.IsActive()) {
1669 limit_Ry->constr_upper.SetVariables(&Body1->Variables(), &Body2->Variables());
1670 Transform_Cq_to_Cqw_row(Cq1_temp, 5, limit_Ry->constr_upper.Get_Cq_a(), 0, Gl1);
1671 Transform_Cq_to_Cqw_row(Cq2_temp, 5, limit_Ry->constr_upper.Get_Cq_b(), 0, Gl2);
1672 limit_Ry->constr_upper.Get_Cq_a() *= -1;
1673 limit_Ry->constr_upper.Get_Cq_b() *= -1;
1674 }
1675 }
1676 if (limit_Rz && limit_Rz->IsActive()) {
1677 if (limit_Rz->constr_lower.IsActive()) {
1678 limit_Rz->constr_lower.SetVariables(&Body1->Variables(), &Body2->Variables());
1679 Transform_Cq_to_Cqw_row(Cq1_temp, 6, limit_Rz->constr_lower.Get_Cq_a(), 0, Gl1);
1680 Transform_Cq_to_Cqw_row(Cq2_temp, 6, limit_Rz->constr_lower.Get_Cq_b(), 0, Gl2);
1681 }
1682 if (limit_Rz->constr_upper.IsActive()) {
1683 limit_Rz->constr_upper.SetVariables(&Body1->Variables(), &Body2->Variables());
1684 Transform_Cq_to_Cqw_row(Cq1_temp, 6, limit_Rz->constr_upper.Get_Cq_a(), 0, Gl1);
1685 Transform_Cq_to_Cqw_row(Cq2_temp, 6, limit_Rz->constr_upper.Get_Cq_b(), 0, Gl2);
1686 limit_Rz->constr_upper.Get_Cq_a() *= -1;
1687 limit_Rz->constr_upper.Get_Cq_b() *= -1;
1688 }
1689 }
1690 }
1691
ConstraintsFetch_react(double factor)1692 void ChLinkLock::ConstraintsFetch_react(double factor) {
1693 react_force = VNULL;
1694 react_torque = VNULL;
1695
1696 // From constraints to react vector:
1697 int cnt = 0;
1698 for (int i = 0; i < mask.nconstr; i++) {
1699 if (mask.Constr_N(i).IsActive()) {
1700 react(cnt) = mask.Constr_N(i).Get_l_i() * factor;
1701 cnt++;
1702 }
1703 }
1704
1705 // From react vector to the 'intuitive' react_force and react_torque
1706 const ChQuaternion<>& q2 = Body2->GetRot();
1707 const ChQuaternion<>& q1p = marker1->GetAbsCoord().rot;
1708 const ChQuaternion<>& qs = marker2->GetCoord().rot;
1709 const ChMatrix33<>& Cs = marker2->GetA();
1710
1711 ChMatrix44<> Chi__q1p_barT; //[Chi] * [transpose(bar(q1p))]
1712 Chi__q1p_barT(0, 0) = q1p.e0();
1713 Chi__q1p_barT(0, 1) = q1p.e1();
1714 Chi__q1p_barT(0, 2) = q1p.e2();
1715 Chi__q1p_barT(0, 3) = q1p.e3();
1716 Chi__q1p_barT(1, 0) = q1p.e1();
1717 Chi__q1p_barT(1, 1) = -q1p.e0();
1718 Chi__q1p_barT(1, 2) = q1p.e3();
1719 Chi__q1p_barT(1, 3) = -q1p.e2();
1720 Chi__q1p_barT(2, 0) = q1p.e2();
1721 Chi__q1p_barT(2, 1) = -q1p.e3();
1722 Chi__q1p_barT(2, 2) = -q1p.e0();
1723 Chi__q1p_barT(2, 3) = q1p.e1();
1724 Chi__q1p_barT(3, 0) = q1p.e3();
1725 Chi__q1p_barT(3, 1) = q1p.e2();
1726 Chi__q1p_barT(3, 2) = -q1p.e1();
1727 Chi__q1p_barT(3, 3) = -q1p.e0();
1728
1729 ChMatrix44<> qs_tilde;
1730 qs_tilde(0, 0) = qs.e0();
1731 qs_tilde(0, 1) = -qs.e1();
1732 qs_tilde(0, 2) = -qs.e2();
1733 qs_tilde(0, 3) = -qs.e3();
1734 qs_tilde(1, 0) = qs.e1();
1735 qs_tilde(1, 1) = qs.e0();
1736 qs_tilde(1, 2) = -qs.e3();
1737 qs_tilde(1, 3) = qs.e2();
1738 qs_tilde(2, 0) = qs.e2();
1739 qs_tilde(2, 1) = qs.e3();
1740 qs_tilde(2, 2) = qs.e0();
1741 qs_tilde(2, 3) = -qs.e1();
1742 qs_tilde(3, 0) = qs.e3();
1743 qs_tilde(3, 1) = -qs.e2();
1744 qs_tilde(3, 2) = qs.e1();
1745 qs_tilde(3, 3) = qs.e0();
1746
1747 // Ts = 0.5*CsT*G(q2)*Chi*(q1 qp)_barT*qs~*KT*lambda
1748 ChGlMatrix34<> Gl_q2(q2);
1749 ChMatrix34<> Ts = 0.25 * Cs.transpose() * Gl_q2 * Chi__q1p_barT * qs_tilde;
1750
1751 // Translational constraint reaction force = -lambda_translational
1752 // Translational constraint reaction torque = -d~''(t)*lambda_translational
1753 // No reaction force from the rotational constraints
1754
1755 int n_constraint = 0;
1756
1757 if (mask.Constr_X().IsActive()) {
1758 react_force.x() = -react(n_constraint);
1759 react_torque.y() = -relM.pos.z() * react(n_constraint);
1760 react_torque.z() = relM.pos.y() * react(n_constraint);
1761 n_constraint++;
1762 }
1763 if (mask.Constr_Y().IsActive()) {
1764 react_force.y() = -react(n_constraint);
1765 react_torque.x() = relM.pos.z() * react(n_constraint);
1766 react_torque.z() += -relM.pos.x() * react(n_constraint);
1767 n_constraint++;
1768 }
1769 if (mask.Constr_Z().IsActive()) {
1770 react_force.z() = -react(n_constraint);
1771 react_torque.x() += -relM.pos.y() * react(n_constraint);
1772 react_torque.y() += relM.pos.x() * react(n_constraint);
1773 n_constraint++;
1774 }
1775
1776 if (mask.Constr_E1().IsActive()) {
1777 react_torque.x() += Ts(0, 1) * (react(n_constraint));
1778 react_torque.y() += Ts(1, 1) * (react(n_constraint));
1779 react_torque.z() += Ts(2, 1) * (react(n_constraint));
1780 n_constraint++;
1781 }
1782 if (mask.Constr_E2().IsActive()) {
1783 react_torque.x() += Ts(0, 2) * (react(n_constraint));
1784 react_torque.y() += Ts(1, 2) * (react(n_constraint));
1785 react_torque.z() += Ts(2, 2) * (react(n_constraint));
1786 n_constraint++;
1787 }
1788 if (mask.Constr_E3().IsActive()) {
1789 react_torque.x() += Ts(0, 3) * (react(n_constraint));
1790 react_torque.y() += Ts(1, 3) * (react(n_constraint));
1791 react_torque.z() += Ts(2, 3) * (react(n_constraint));
1792 n_constraint++;
1793 }
1794
1795 // ***TO DO***?: TRANSFORMATION FROM delta COORDS TO LINK COORDS, if
1796 // non-default delta
1797 // if delta rotation?
1798
1799 // add also the contribution from link limits to the react_force and
1800 // react_torque.
1801 if (limit_X && limit_X->IsActive()) {
1802 if (limit_X->constr_lower.IsActive()) {
1803 react_force.x() -= factor * limit_X->constr_lower.Get_l_i();
1804 }
1805 if (limit_X->constr_upper.IsActive()) {
1806 react_force.x() += factor * limit_X->constr_upper.Get_l_i();
1807 }
1808 }
1809 if (limit_Y && limit_Y->IsActive()) {
1810 if (limit_Y->constr_lower.IsActive()) {
1811 react_force.y() -= factor * limit_Y->constr_lower.Get_l_i();
1812 }
1813 if (limit_Y->constr_upper.IsActive()) {
1814 react_force.y() += factor * limit_Y->constr_upper.Get_l_i();
1815 }
1816 }
1817 if (limit_Z && limit_Z->IsActive()) {
1818 if (limit_Z->constr_lower.IsActive()) {
1819 react_force.z() -= factor * limit_Z->constr_lower.Get_l_i();
1820 }
1821 if (limit_Z->constr_upper.IsActive()) {
1822 react_force.z() += factor * limit_Z->constr_upper.Get_l_i();
1823 }
1824 }
1825 if (limit_Rx && limit_Rx->IsActive()) {
1826 if (limit_Rx->constr_lower.IsActive()) {
1827 react_torque.x() -= 0.5 * factor * limit_Rx->constr_lower.Get_l_i();
1828 }
1829 if (limit_Rx->constr_upper.IsActive()) {
1830 react_torque.x() += 0.5 * factor * limit_Rx->constr_upper.Get_l_i();
1831 }
1832 }
1833 if (limit_Ry && limit_Ry->IsActive()) {
1834 if (limit_Ry->constr_lower.IsActive()) {
1835 react_torque.y() -= 0.5 * factor * limit_Ry->constr_lower.Get_l_i();
1836 }
1837 if (limit_Ry->constr_upper.IsActive()) {
1838 react_torque.y() += 0.5 * factor * limit_Ry->constr_upper.Get_l_i();
1839 }
1840 }
1841 if (limit_Rz && limit_Rz->IsActive()) {
1842 if (limit_Rz->constr_lower.IsActive()) {
1843 react_torque.z() -= 0.5 * factor * limit_Rz->constr_lower.Get_l_i();
1844 }
1845 if (limit_Rz->constr_upper.IsActive()) {
1846 react_torque.z() += 0.5 * factor * limit_Rz->constr_upper.Get_l_i();
1847 }
1848 }
1849
1850 // the internal forces add their contribute to the reactions
1851 // NOT NEEDED?, since C_force and react_force must stay separated???
1852 // react_force = Vadd(react_force, C_force);
1853 // react_torque = Vadd(react_torque, C_torque);
1854 }
1855
1856 // SERIALIZATION
1857
1858 // To avoid putting the following mapper macro inside the class definition,
1859 // enclose macros in local 'my_enum_mappers_types'.
1860 class my_enum_mappers_types : public ChLinkLock {
1861 public:
1862 CH_ENUM_MAPPER_BEGIN(LinkType);
1863 CH_ENUM_VAL(LinkType::LOCK);
1864 CH_ENUM_VAL(LinkType::SPHERICAL);
1865 CH_ENUM_VAL(LinkType::POINTPLANE);
1866 CH_ENUM_VAL(LinkType::POINTLINE);
1867 CH_ENUM_VAL(LinkType::CYLINDRICAL);
1868 CH_ENUM_VAL(LinkType::PRISMATIC);
1869 CH_ENUM_VAL(LinkType::PLANEPLANE);
1870 CH_ENUM_VAL(LinkType::OLDHAM);
1871 CH_ENUM_VAL(LinkType::REVOLUTE);
1872 CH_ENUM_VAL(LinkType::FREE);
1873 CH_ENUM_VAL(LinkType::ALIGN);
1874 CH_ENUM_VAL(LinkType::PARALLEL);
1875 CH_ENUM_VAL(LinkType::PERPEND);
1876 CH_ENUM_VAL(LinkType::TRAJECTORY);
1877 CH_ENUM_VAL(LinkType::CLEARANCE);
1878 CH_ENUM_VAL(LinkType::REVOLUTEPRISMATIC);
1879 CH_ENUM_MAPPER_END(LinkType);
1880 };
1881
ArchiveOUT(ChArchiveOut & marchive)1882 void ChLinkLock::ArchiveOUT(ChArchiveOut& marchive) {
1883 // version number
1884 marchive.VersionWrite<ChLinkLock>();
1885
1886 // serialize parent class
1887 ChLinkMarkers::ArchiveOUT(marchive);
1888
1889 // serialize all member data
1890 my_enum_mappers_types::LinkType_mapper typemapper;
1891 marchive << CHNVP(typemapper(type), "link_type");
1892
1893 ////marchive << CHNVP(mask); //// TODO: needed?
1894
1895 marchive << CHNVP(d_restlength);
1896
1897 marchive << CHNVP(force_D.get());
1898
1899 ////marchive << CHNVP(force_D);
1900 ////marchive << CHNVP(force_R);
1901 ////marchive << CHNVP(force_X);
1902 ////marchive << CHNVP(force_Y);
1903 ////marchive << CHNVP(force_Z);
1904 ////marchive << CHNVP(force_Rx);
1905 ////marchive << CHNVP(force_Ry);
1906 ////marchive << CHNVP(force_Rz);
1907
1908 ////marchive << CHNVP(limit_X);
1909 ////marchive << CHNVP(limit_Y);
1910 ////marchive << CHNVP(limit_Z);
1911 ////marchive << CHNVP(limit_Rx);
1912 ////marchive << CHNVP(limit_Ry);
1913 ////marchive << CHNVP(limit_Rz);
1914 ////marchive << CHNVP(limit_Rp);
1915 ////marchive << CHNVP(limit_D);
1916 }
1917
ArchiveIN(ChArchiveIn & marchive)1918 void ChLinkLock::ArchiveIN(ChArchiveIn& marchive) {
1919 // version number
1920 /*int version =*/ marchive.VersionRead<ChLinkLock>();
1921
1922 // deserialize parent class
1923 ChLinkMarkers::ArchiveIN(marchive);
1924
1925 // deserialize all member data
1926 my_enum_mappers_types::LinkType_mapper typemapper;
1927 LinkType link_type;
1928 marchive >> CHNVP(typemapper(link_type), "link_type");
1929 ChangeLinkType(link_type);
1930
1931 ////if (mask) delete (mask); marchive >> CHNVP(mask); //// TODO: needed?
1932
1933 marchive >> CHNVP(d_restlength);
1934
1935 {
1936 ChLinkForce* force_D_ptr;
1937 marchive >> CHNVP(force_D_ptr);
1938 force_D.reset(force_D_ptr);
1939 }
1940 ////marchive >> CHNVP(force_D);
1941 ////marchive >> CHNVP(force_R);
1942 ////marchive >> CHNVP(force_X);
1943 ////marchive >> CHNVP(force_Y);
1944 ////marchive >> CHNVP(force_Z);
1945 ////marchive >> CHNVP(force_Rx);
1946 ////marchive >> CHNVP(force_Ry);
1947 ////marchive >> CHNVP(force_Rz);
1948
1949 ////marchive >> CHNVP(limit_X);
1950 ////marchive >> CHNVP(limit_Y);
1951 ////marchive >> CHNVP(limit_Z);
1952 ////marchive >> CHNVP(limit_Rx);
1953 ////marchive >> CHNVP(limit_Ry);
1954 ////marchive >> CHNVP(limit_Rz);
1955 ////marchive >> CHNVP(limit_Rp);
1956 ////marchive >> CHNVP(limit_D);
1957 }
1958
1959 // =======================================================================================
1960
Lock(bool lock)1961 void ChLinkLockRevolute::Lock(bool lock) {
1962 BuildLink(true, true, true, false, true, true, lock);
1963 if (system) {
1964 system->ForceUpdate();
1965 }
1966 }
1967
Lock(bool lock)1968 void ChLinkLockSpherical::Lock(bool lock) {
1969 BuildLink(true, true, true, false, lock, lock, lock);
1970 if (system) {
1971 system->ForceUpdate();
1972 }
1973 }
1974
Lock(bool lock)1975 void ChLinkLockCylindrical::Lock(bool lock) {
1976 BuildLink(true, true, lock, false, true, true, lock);
1977 if (system) {
1978 system->ForceUpdate();
1979 }
1980 }
1981
Lock(bool lock)1982 void ChLinkLockPrismatic::Lock(bool lock) {
1983 BuildLink(true, true, lock, false, true, true, true);
1984 if (system) {
1985 system->ForceUpdate();
1986 }
1987 }
1988
Lock(bool lock)1989 void ChLinkLockPointPlane::Lock(bool lock) {
1990 BuildLink(lock, lock, true, false, lock, lock, lock);
1991 if (system) {
1992 system->ForceUpdate();
1993 }
1994 }
1995
Lock(bool lock)1996 void ChLinkLockPointLine::Lock(bool lock) {
1997 BuildLink(lock, true, true, false, lock, lock, lock);
1998 if (system) {
1999 system->ForceUpdate();
2000 }
2001 }
2002
Lock(bool lock)2003 void ChLinkLockPlanePlane::Lock(bool lock) {
2004 BuildLink(lock, lock, true, false, true, true, lock);
2005 if (system) {
2006 system->ForceUpdate();
2007 }
2008 }
2009
Lock(bool lock)2010 void ChLinkLockOldham::Lock(bool lock) {
2011 BuildLink(lock, lock, true, false, true, true, true);
2012 if (system) {
2013 system->ForceUpdate();
2014 }
2015 }
2016
Lock(bool lock)2017 void ChLinkLockFree::Lock(bool lock) {
2018 BuildLink(lock, lock, lock, false, lock, lock, lock);
2019 if (system) {
2020 system->ForceUpdate();
2021 }
2022 }
2023
Lock(bool lock)2024 void ChLinkLockAlign::Lock(bool lock) {
2025 BuildLink(lock, lock, lock, false, true, true, true);
2026 if (system) {
2027 system->ForceUpdate();
2028 }
2029 }
2030
Lock(bool lock)2031 void ChLinkLockParallel::Lock(bool lock) {
2032 BuildLink(lock, lock, lock, false, true, true, lock);
2033 if (system) {
2034 system->ForceUpdate();
2035 }
2036 }
2037
Lock(bool lock)2038 void ChLinkLockPerpend::Lock(bool lock) {
2039 BuildLink(lock, lock, lock, false, true, lock, true);
2040 if (system) {
2041 system->ForceUpdate();
2042 }
2043 }
2044
Lock(bool lock)2045 void ChLinkLockRevolutePrismatic::Lock(bool lock) {
2046 BuildLink(lock, true, true, false, true, true, lock);
2047 if (system) {
2048 system->ForceUpdate();
2049 }
2050 }
2051
2052 // =======================================================================================
2053 // ChLinkLockLock implementation
2054 // =======================================================================================
2055
2056 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChLinkLockLock)2057 CH_FACTORY_REGISTER(ChLinkLockLock)
2058
2059 ChLinkLockLock::ChLinkLockLock()
2060 : motion_axis(VECT_Z),
2061 angleset(AngleSet::ANGLE_AXIS),
2062 relC(CSYSNORM),
2063 relC_dt(CSYSNULL),
2064 relC_dtdt(CSYSNULL),
2065 deltaC(CSYSNORM),
2066 deltaC_dt(CSYSNULL),
2067 deltaC_dtdt(CSYSNULL) {
2068 type = LinkType::LOCK;
2069 BuildLink(true, true, true, false, true, true, true);
2070
2071 motion_X = chrono_types::make_shared<ChFunction_Const>(0); // default: no motion
2072 motion_Y = chrono_types::make_shared<ChFunction_Const>(0);
2073 motion_Z = chrono_types::make_shared<ChFunction_Const>(0);
2074 motion_ang = chrono_types::make_shared<ChFunction_Const>(0);
2075 motion_ang2 = chrono_types::make_shared<ChFunction_Const>(0);
2076 motion_ang3 = chrono_types::make_shared<ChFunction_Const>(0);
2077 }
2078
ChLinkLockLock(const ChLinkLockLock & other)2079 ChLinkLockLock::ChLinkLockLock(const ChLinkLockLock& other) : ChLinkLock(other) {
2080 type = LinkType::LOCK;
2081 BuildLink(true, true, true, false, true, true, true);
2082
2083 motion_X = std::shared_ptr<ChFunction>(other.motion_X->Clone());
2084 motion_Y = std::shared_ptr<ChFunction>(other.motion_Y->Clone());
2085 motion_Z = std::shared_ptr<ChFunction>(other.motion_Z->Clone());
2086 motion_ang = std::shared_ptr<ChFunction>(other.motion_ang->Clone());
2087 motion_ang2 = std::shared_ptr<ChFunction>(other.motion_ang2->Clone());
2088 motion_ang3 = std::shared_ptr<ChFunction>(other.motion_ang3->Clone());
2089
2090 motion_axis = other.motion_axis;
2091 angleset = other.angleset;
2092
2093 deltaC = other.deltaC;
2094 deltaC_dt = other.deltaC_dt;
2095 deltaC_dtdt = other.deltaC_dtdt;
2096 relC = other.relC;
2097 relC_dt = other.relC_dt;
2098 relC_dtdt = other.relC_dtdt;
2099 }
2100
SetMotion_X(std::shared_ptr<ChFunction> m_funct)2101 void ChLinkLockLock::SetMotion_X(std::shared_ptr<ChFunction> m_funct) {
2102 motion_X = m_funct;
2103 }
2104
SetMotion_Y(std::shared_ptr<ChFunction> m_funct)2105 void ChLinkLockLock::SetMotion_Y(std::shared_ptr<ChFunction> m_funct) {
2106 motion_Y = m_funct;
2107 }
2108
SetMotion_Z(std::shared_ptr<ChFunction> m_funct)2109 void ChLinkLockLock::SetMotion_Z(std::shared_ptr<ChFunction> m_funct) {
2110 motion_Z = m_funct;
2111 }
2112
SetMotion_ang(std::shared_ptr<ChFunction> m_funct)2113 void ChLinkLockLock::SetMotion_ang(std::shared_ptr<ChFunction> m_funct) {
2114 motion_ang = m_funct;
2115 }
2116
SetMotion_ang2(std::shared_ptr<ChFunction> m_funct)2117 void ChLinkLockLock::SetMotion_ang2(std::shared_ptr<ChFunction> m_funct) {
2118 motion_ang2 = m_funct;
2119 }
2120
SetMotion_ang3(std::shared_ptr<ChFunction> m_funct)2121 void ChLinkLockLock::SetMotion_ang3(std::shared_ptr<ChFunction> m_funct) {
2122 motion_ang3 = m_funct;
2123 }
2124
SetMotion_axis(Vector m_axis)2125 void ChLinkLockLock::SetMotion_axis(Vector m_axis) {
2126 motion_axis = m_axis;
2127 }
2128
2129 // Sequence of calls for full update:
2130 // UpdateTime(time);
2131 // UpdateRelMarkerCoords();
2132 // UpdateState();
2133 // UpdateCqw();
2134 // UpdateForces(time);
2135 // Override UpdateTime to include possible contributions from imposed motion.
UpdateTime(double time)2136 void ChLinkLockLock::UpdateTime(double time) {
2137 ChLinkLock::UpdateTime(time);
2138
2139 double ang, ang_dt, ang_dtdt;
2140
2141 // If some limit is provided, the delta values may have been changed by limits themselves,
2142 // so no further modifications by motion laws.
2143 if ((limit_X && limit_X->IsActive()) || (limit_Y && limit_Y->IsActive()) ||
2144 (limit_Z && limit_Z->IsActive()) || (limit_Rx && limit_Rx->IsActive()) ||
2145 (limit_Ry && limit_Ry->IsActive()) || (limit_Rz && limit_Rz->IsActive()))
2146 return;
2147
2148 // Update motion position/speed/acceleration by motion laws
2149 // as expressed by specific link CH functions
2150 deltaC.pos.x() = motion_X->Get_y(time);
2151 deltaC_dt.pos.x() = motion_X->Get_y_dx(time);
2152 deltaC_dtdt.pos.x() = motion_X->Get_y_dxdx(time);
2153
2154 deltaC.pos.y() = motion_Y->Get_y(time);
2155 deltaC_dt.pos.y() = motion_Y->Get_y_dx(time);
2156 deltaC_dtdt.pos.y() = motion_Y->Get_y_dxdx(time);
2157
2158 deltaC.pos.z() = motion_Z->Get_y(time);
2159 deltaC_dt.pos.z() = motion_Z->Get_y_dx(time);
2160 deltaC_dtdt.pos.z() = motion_Z->Get_y_dxdx(time);
2161
2162 switch (angleset) {
2163 case AngleSet::ANGLE_AXIS:
2164 ang = motion_ang->Get_y(time);
2165 ang_dt = motion_ang->Get_y_dx(time);
2166 ang_dtdt = motion_ang->Get_y_dxdx(time);
2167
2168 if ((ang != 0) || (ang_dt != 0) || (ang_dtdt != 0)) {
2169 deltaC.rot = Q_from_AngAxis(ang, motion_axis);
2170 deltaC_dt.rot = Qdt_from_AngAxis(deltaC.rot, ang_dt, motion_axis);
2171 deltaC_dtdt.rot = Qdtdt_from_AngAxis(ang_dtdt, motion_axis, deltaC.rot, deltaC_dt.rot);
2172 } else {
2173 deltaC.rot = QUNIT;
2174 deltaC_dt.rot = QNULL;
2175 deltaC_dtdt.rot = QNULL;
2176 }
2177 break;
2178 case AngleSet::EULERO:
2179 case AngleSet::CARDANO:
2180 case AngleSet::HPB:
2181 case AngleSet::RXYZ: {
2182 Vector vangles, vangles_dt, vangles_dtdt;
2183 vangles.x() = motion_ang->Get_y(time);
2184 vangles.y() = motion_ang2->Get_y(time);
2185 vangles.z() = motion_ang3->Get_y(time);
2186 vangles_dt.x() = motion_ang->Get_y_dx(time);
2187 vangles_dt.y() = motion_ang2->Get_y_dx(time);
2188 vangles_dt.z() = motion_ang3->Get_y_dx(time);
2189 vangles_dtdt.x() = motion_ang->Get_y_dxdx(time);
2190 vangles_dtdt.y() = motion_ang2->Get_y_dxdx(time);
2191 vangles_dtdt.z() = motion_ang3->Get_y_dxdx(time);
2192 deltaC.rot = Angle_to_Quat(angleset, vangles);
2193 deltaC_dt.rot = AngleDT_to_QuatDT(angleset, vangles_dt, deltaC.rot);
2194 deltaC_dtdt.rot = AngleDTDT_to_QuatDTDT(angleset, vangles_dtdt, deltaC.rot);
2195 break;
2196 }
2197 default:
2198 break;
2199 }
2200 }
2201
2202 // Updates Cq1_temp, Cq2_temp, Qc_temp, etc., i.e. all LOCK-FORMULATION temp.matrices
UpdateState()2203 void ChLinkLockLock::UpdateState() {
2204 // ----------- SOME PRECALCULATED VARIABLES, to optimize speed
2205
2206 ChStarMatrix33<> P1star(marker1->GetCoord().pos); // [P] star matrix of rel pos of mark1
2207 ChStarMatrix33<> Q2star(marker2->GetCoord().pos); // [Q] star matrix of rel pos of mark2
2208
2209 ChGlMatrix34<> body1Gl(Body1->GetCoord().rot);
2210 ChGlMatrix34<> body2Gl(Body2->GetCoord().rot);
2211
2212 // ----------- RELATIVE LINK-LOCK COORDINATES (violations)
2213
2214 // relC.pos
2215 relC.pos = Vsub(relM.pos, deltaC.pos);
2216
2217 // relC.rot
2218 relC.rot = Qcross(Qconjugate(deltaC.rot), relM.rot);
2219
2220 // relC_dt.pos
2221 relC_dt.pos = Vsub(relM_dt.pos, deltaC_dt.pos);
2222
2223 // relC_dt.rot
2224 relC_dt.rot = Qadd(Qcross(Qconjugate(deltaC_dt.rot), relM.rot), Qcross(Qconjugate(deltaC.rot), relM_dt.rot));
2225
2226 // relC_dtdt.pos
2227 relC_dtdt.pos = Vsub(relM_dtdt.pos, deltaC_dtdt.pos);
2228
2229 // relC_dtdt.rot
2230 relC_dtdt.rot =
2231 Qadd(Qadd(Qcross(Qconjugate(deltaC_dtdt.rot), relM.rot), Qcross(Qconjugate(deltaC.rot), relM_dtdt.rot)),
2232 Qscale(Qcross(Qconjugate(deltaC_dt.rot), relM_dt.rot), 2));
2233
2234 // Compute the Cq Ct Qc matrices (temporary, for complete lock constraint)
2235
2236 ChMatrix33<> m2_Rel_A_dt;
2237 marker2->Compute_Adt(m2_Rel_A_dt);
2238 ChMatrix33<> m2_Rel_A_dtdt;
2239 marker2->Compute_Adtdt(m2_Rel_A_dtdt);
2240
2241 // ----------- PARTIAL DERIVATIVE Ct OF CONSTRAINT
2242 Ct_temp.pos =
2243 m2_Rel_A_dt.transpose() * (Body2->GetA().transpose() * PQw) +
2244 marker2->GetA().transpose() *
2245 (Body2->GetA().transpose() * (Body1->GetA() * marker1->GetCoord_dt().pos) - marker2->GetCoord_dt().pos);
2246 Ct_temp.pos -= deltaC_dt.pos; // the deltaC contribute
2247
2248 // deltaC^*(q_AD) + deltaC_dt^*q_pq
2249 Ct_temp.rot = Qcross(Qconjugate(deltaC.rot), q_AD) + Qcross(Qconjugate(deltaC_dt.rot), relM.rot);
2250
2251 //------------ COMPLETE JACOBIANS Cq1_temp AND Cq2_temp AND Qc_temp VECTOR.
2252 // [Cq_temp]= [[CqxT] [CqxR]] {Qc_temp} ={[Qcx]}
2253 // [[ 0 ] [CqrR]] {[Qcr]}
2254
2255 // JACOBIANS Cq1_temp, Cq2_temp:
2256
2257 ChMatrix33<> CqxT = marker2->GetA().transpose() * Body2->GetA().transpose(); // [CqxT]=[Aq]'[Ao2]'
2258 ChStarMatrix33<> tmpStar(Body2->GetA().transpose() * PQw);
2259
2260 Cq1_temp.topLeftCorner<3, 3>() = CqxT; // *- -- Cq1_temp(1-3) =[Aqo2]
2261 Cq2_temp.topLeftCorner<3, 3>() = -CqxT; // -- *- Cq2_temp(1-3) =-[Aqo2]
2262 Cq1_temp.topRightCorner<3, 4>() = -CqxT * Body1->GetA() * P1star * body1Gl; // -* -- Cq1_temp(4-7)
2263 Cq2_temp.topRightCorner<3, 4>() = CqxT * Body2->GetA() * Q2star * body2Gl + //
2264 marker2->GetA().transpose() * tmpStar * body2Gl; // -- -* Cq2_temp(4-7)
2265
2266 {
2267 ChStarMatrix44<> stempQ1(Qcross(Qconjugate(marker2->GetCoord().rot), Qconjugate(Body2->GetCoord().rot)));
2268 ChStarMatrix44<> stempQ2(marker1->GetCoord().rot);
2269 ChStarMatrix44<> stempDC(Qconjugate(deltaC.rot));
2270 stempQ2.semiTranspose();
2271 Cq1_temp.bottomRightCorner<4, 4>() = stempDC * stempQ1 * stempQ2; // =* == Cq1_temp(col 4-7, row 4-7) ... CqrR
2272 }
2273
2274 {
2275 ChStarMatrix44<> stempQ1(Qconjugate(marker2->GetCoord().rot));
2276 ChStarMatrix44<> stempQ2(Qcross(Body1->GetCoord().rot, marker1->GetCoord().rot));
2277 ChStarMatrix44<> stempDC(Qconjugate(deltaC.rot));
2278 stempQ2.semiTranspose();
2279 stempQ2.semiNegate();
2280 Cq2_temp.bottomRightCorner<4, 4>() = stempDC * stempQ1 * stempQ2; // == =* Cq2_temp(col 4-7, row 4-7) ... CqrR
2281 }
2282
2283 //--------- COMPLETE Qc VECTOR
2284 ChVector<> Qcx;
2285 ChQuaternion<> Qcr;
2286 ChVector<> vtemp1;
2287 ChVector<> vtemp2;
2288
2289 vtemp1 = Vcross(Body1->GetWvel_loc(), Vcross(Body1->GetWvel_loc(), marker1->GetCoord().pos));
2290 vtemp1 = Vadd(vtemp1, marker1->GetCoord_dtdt().pos);
2291 vtemp1 = Vadd(vtemp1, Vmul(Vcross(Body1->GetWvel_loc(), marker1->GetCoord_dt().pos), 2));
2292
2293 vtemp2 = Vcross(Body2->GetWvel_loc(), Vcross(Body2->GetWvel_loc(), marker2->GetCoord().pos));
2294 vtemp2 = Vadd(vtemp2, marker2->GetCoord_dtdt().pos);
2295 vtemp2 = Vadd(vtemp2, Vmul(Vcross(Body2->GetWvel_loc(), marker2->GetCoord_dt().pos), 2));
2296
2297 Qcx = CqxT * (Body1->GetA() * vtemp1 - Body2->GetA() * vtemp2);
2298
2299 ChStarMatrix33<> mtemp1(Body2->GetWvel_loc());
2300 ChMatrix33<> mtemp3 = Body2->GetA() * mtemp1 * mtemp1;
2301 vtemp2 = marker2->GetA().transpose() * (mtemp3.transpose() * PQw); // [Aq]'[[A2][w2][w2]]'*Qpq,w
2302 Qcx = Vadd(Qcx, vtemp2);
2303 Qcx = Vadd(Qcx, q_4); // [Adtdt]'[A]'q + 2[Adt]'[Adt]'q + 2[Adt]'[A]'qdt + 2[A]'[Adt]'qdt
2304 Qcx = Vsub(Qcx, deltaC_dtdt.pos); // ... - deltaC_dtdt
2305
2306 Qcr = Qcross(Qconjugate(deltaC.rot), q_8);
2307 Qcr = Qadd(Qcr, Qscale(Qcross(Qconjugate(deltaC_dt.rot), relM_dt.rot), 2));
2308 Qcr = Qadd(Qcr, Qcross(Qconjugate(deltaC_dtdt.rot), relM.rot)); // = deltaC'*q_8 + 2*deltaC_dt'*q_dt,po +
2309 // deltaC_dtdt'*q,po
2310
2311 Qc_temp.segment(0, 3) = Qcx.eigen(); // * Qc_temp, for all translational coords
2312 Qc_temp.segment(3, 4) = Qcr.eigen(); // * Qc_temp, for all rotational coords
2313
2314 // *** NOTE! The definitive Qc must change sign, to be used in
2315 // lagrangian equation: [Cq]*q_dtdt = Qc
2316 // because until now we have computed it as [Cq]*q_dtdt + "Qc" = 0,
2317 // but the most used form is the previous, so let's change sign!!
2318 Qc_temp *= -1;
2319
2320 // ---------------------
2321 // Updates Cq1, Cq2, Qc,
2322 // C, C_dt, C_dtdt, Ct.
2323 // ---------------------
2324 int index = 0;
2325
2326 if (mask.Constr_X().IsActive()) {
2327 Cq1.block<1, 7>(index, 0) = Cq1_temp.block<1, 7>(0, 0);
2328 Cq2.block<1, 7>(index, 0) = Cq2_temp.block<1, 7>(0, 0);
2329
2330 Qc(index) = Qc_temp(0);
2331
2332 C(index) = relC.pos.x();
2333 C_dt(index) = relC_dt.pos.x();
2334 C_dtdt(index) = relC_dtdt.pos.x();
2335
2336 Ct(index) = Ct_temp.pos.x();
2337
2338 index++;
2339 }
2340
2341 if (mask.Constr_Y().IsActive()) {
2342 Cq1.block<1, 7>(index, 0) = Cq1_temp.block<1, 7>(1, 0);
2343 Cq2.block<1, 7>(index, 0) = Cq2_temp.block<1, 7>(1, 0);
2344
2345 Qc(index) = Qc_temp(1);
2346
2347 C(index) = relC.pos.y();
2348 C_dt(index) = relC_dt.pos.y();
2349 C_dtdt(index) = relC_dtdt.pos.y();
2350
2351 Ct(index) = Ct_temp.pos.y();
2352
2353 index++;
2354 }
2355
2356 if (mask.Constr_Z().IsActive()) {
2357 Cq1.block<1, 7>(index, 0) = Cq1_temp.block<1, 7>(2, 0);
2358 Cq2.block<1, 7>(index, 0) = Cq2_temp.block<1, 7>(2, 0);
2359
2360 Qc(index) = Qc_temp(2);
2361
2362 C(index) = relC.pos.z();
2363 C_dt(index) = relC_dt.pos.z();
2364 C_dtdt(index) = relC_dtdt.pos.z();
2365
2366 Ct(index) = Ct_temp.pos.z();
2367
2368 index++;
2369 }
2370
2371 if (mask.Constr_E0().IsActive()) {
2372 Cq1.block<1, 4>(index, 3) = Cq1_temp.block<1, 4>(3, 3);
2373 Cq2.block<1, 4>(index, 3) = Cq2_temp.block<1, 4>(3, 3);
2374
2375 Qc(index) = Qc_temp(3);
2376
2377 C(index) = relC.rot.e0();
2378 C_dt(index) = relC_dt.rot.e0();
2379 C_dtdt(index) = relC_dtdt.rot.e0();
2380
2381 Ct(index) = Ct_temp.rot.e0();
2382
2383 index++;
2384 }
2385
2386 if (mask.Constr_E1().IsActive()) {
2387 Cq1.block<1, 4>(index, 3) = Cq1_temp.block<1, 4>(4, 3);
2388 Cq2.block<1, 4>(index, 3) = Cq2_temp.block<1, 4>(4, 3);
2389
2390 Qc(index) = Qc_temp(4);
2391
2392 C(index) = relC.rot.e1();
2393 C_dt(index) = relC_dt.rot.e1();
2394 C_dtdt(index) = relC_dtdt.rot.e1();
2395
2396 Ct(index) = Ct_temp.rot.e1();
2397
2398 index++;
2399 }
2400
2401 if (mask.Constr_E2().IsActive()) {
2402 Cq1.block<1, 4>(index, 3) = Cq1_temp.block<1, 4>(5, 3);
2403 Cq2.block<1, 4>(index, 3) = Cq2_temp.block<1, 4>(5, 3);
2404
2405 Qc(index) = Qc_temp(5);
2406
2407 C(index) = relC.rot.e2();
2408 C_dt(index) = relC_dt.rot.e2();
2409 C_dtdt(index) = relC_dtdt.rot.e2();
2410
2411 Ct(index) = Ct_temp.rot.e2();
2412
2413 index++;
2414 }
2415
2416 if (mask.Constr_E3().IsActive()) {
2417 Cq1.block<1, 4>(index, 3) = Cq1_temp.block<1, 4>(6, 3);
2418 Cq2.block<1, 4>(index, 3) = Cq2_temp.block<1, 4>(6, 3);
2419
2420 Qc(index) = Qc_temp(6);
2421
2422 C(index) = relC.rot.e3();
2423 C_dt(index) = relC_dt.rot.e3();
2424 C_dtdt(index) = relC_dtdt.rot.e3();
2425
2426 Ct(index) = Ct_temp.rot.e3();
2427
2428 index++;
2429 }
2430 }
2431
2432 // To avoid putting the following mapper macro inside the class definition,
2433 // enclose macros in local 'my_enum_mappers_angles'.
2434 class my_enum_mappers_angles : public ChLinkLockLock {
2435 public:
2436 CH_ENUM_MAPPER_BEGIN(AngleSet);
2437 CH_ENUM_VAL(AngleSet::ANGLE_AXIS);
2438 CH_ENUM_VAL(AngleSet::EULERO);
2439 CH_ENUM_VAL(AngleSet::CARDANO);
2440 CH_ENUM_VAL(AngleSet::HPB);
2441 CH_ENUM_VAL(AngleSet::RXYZ);
2442 CH_ENUM_VAL(AngleSet::RODRIGUEZ);
2443 CH_ENUM_VAL(AngleSet::QUATERNION);
2444 CH_ENUM_MAPPER_END(AngleSet);
2445 };
2446
ArchiveOUT(ChArchiveOut & marchive)2447 void ChLinkLockLock::ArchiveOUT(ChArchiveOut& marchive) {
2448 // version number
2449 marchive.VersionWrite<ChLinkLockLock>();
2450
2451 // serialize parent class
2452 ChLinkMarkers::ArchiveOUT(marchive);
2453
2454 // serialize all member data
2455 ////marchive << CHNVP(mask); //// TODO: needed?
2456
2457 marchive << CHNVP(d_restlength);
2458
2459 ////marchive << CHNVP(force_D);
2460 ////marchive << CHNVP(force_R);
2461 ////marchive << CHNVP(force_X);
2462 ////marchive << CHNVP(force_Y);
2463 ////marchive << CHNVP(force_Z);
2464 ////marchive << CHNVP(force_Rx);
2465 ////marchive << CHNVP(force_Ry);
2466 ////marchive << CHNVP(force_Rz);
2467
2468 ////marchive << CHNVP(limit_X);
2469 ////marchive << CHNVP(limit_Y);
2470 ////marchive << CHNVP(limit_Z);
2471 ////marchive << CHNVP(limit_Rx);
2472 ////marchive << CHNVP(limit_Ry);
2473 ////marchive << CHNVP(limit_Rz);
2474 ////marchive << CHNVP(limit_Rp);
2475 ////marchive << CHNVP(limit_D);
2476
2477 marchive << CHNVP(motion_X);
2478 marchive << CHNVP(motion_Y);
2479 marchive << CHNVP(motion_Z);
2480 marchive << CHNVP(motion_ang);
2481 marchive << CHNVP(motion_ang2);
2482 marchive << CHNVP(motion_ang3);
2483 marchive << CHNVP(motion_axis);
2484
2485 my_enum_mappers_angles::AngleSet_mapper setmapper;
2486 marchive << CHNVP(setmapper(angleset), "angle_set");
2487 }
2488
ArchiveIN(ChArchiveIn & marchive)2489 void ChLinkLockLock::ArchiveIN(ChArchiveIn& marchive) {
2490 // version number
2491 /*int version =*/ marchive.VersionRead<ChLinkLockLock>();
2492
2493 // deserialize parent class
2494 ChLinkMarkers::ArchiveIN(marchive);
2495
2496 // deserialize all member data
2497 ////if (mask) delete (mask); marchive >> CHNVP(mask); //// TODO: needed?
2498
2499 marchive >> CHNVP(d_restlength);
2500
2501 ////marchive >> CHNVP(force_D);
2502 ////marchive >> CHNVP(force_R);
2503 ////marchive >> CHNVP(force_X);
2504 ////marchive >> CHNVP(force_Y);
2505 ////marchive >> CHNVP(force_Z);
2506 ////marchive >> CHNVP(force_Rx);
2507 ////marchive >> CHNVP(force_Ry);
2508 ////marchive >> CHNVP(force_Rz);
2509
2510 ////marchive >> CHNVP(limit_X);
2511 ////marchive >> CHNVP(limit_Y);
2512 ////marchive >> CHNVP(limit_Z);
2513 ////marchive >> CHNVP(limit_Rx);
2514 ////marchive >> CHNVP(limit_Ry);
2515 ////marchive >> CHNVP(limit_Rz);
2516 ////marchive >> CHNVP(limit_Rp);
2517 ////marchive >> CHNVP(limit_D);
2518
2519 marchive >> CHNVP(motion_X);
2520 marchive >> CHNVP(motion_Y);
2521 marchive >> CHNVP(motion_Z);
2522 marchive >> CHNVP(motion_ang);
2523 marchive >> CHNVP(motion_ang2);
2524 marchive >> CHNVP(motion_ang3);
2525 marchive >> CHNVP(motion_axis);
2526
2527 my_enum_mappers_angles::AngleSet_mapper setmapper;
2528 marchive >> CHNVP(setmapper(angleset), "angle_set");
2529 }
2530
2531 // =======================================================================================
2532
2533 // Register into the object factory, to enable run-time dynamic creation and persistence
2534 CH_FACTORY_REGISTER(ChLinkLockRevolute)
2535 CH_FACTORY_REGISTER(ChLinkLockSpherical)
2536 CH_FACTORY_REGISTER(ChLinkLockCylindrical)
2537 CH_FACTORY_REGISTER(ChLinkLockPrismatic)
2538 CH_FACTORY_REGISTER(ChLinkLockPointPlane)
2539 CH_FACTORY_REGISTER(ChLinkLockPointLine)
2540 CH_FACTORY_REGISTER(ChLinkLockPlanePlane)
2541 CH_FACTORY_REGISTER(ChLinkLockOldham)
2542 CH_FACTORY_REGISTER(ChLinkLockFree)
2543 CH_FACTORY_REGISTER(ChLinkLockAlign)
2544 CH_FACTORY_REGISTER(ChLinkLockParallel)
2545 CH_FACTORY_REGISTER(ChLinkLockPerpend)
2546 CH_FACTORY_REGISTER(ChLinkLockRevolutePrismatic)
2547
2548 } // end namespace chrono
2549