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/ChLimit.h"
16
17 namespace chrono {
18
ChLinkLimit()19 ChLinkLimit::ChLinkLimit()
20 : m_active(false),
21 m_penalty_only(false),
22 m_polar(false),
23 m_rotation(false),
24 m_max(1),
25 m_min(-1),
26 m_maxCushion(0),
27 m_minCushion(0),
28 m_Kmax(1000),
29 m_Kmin(1000),
30 m_Rmax(100),
31 m_Rmin(100),
32 m_minElastic(0),
33 m_maxElastic(0) {
34 // Default: no modulation
35 m_Kmax_modul = chrono_types::make_shared<ChFunction_Const>(1);
36 m_Kmin_modul = chrono_types::make_shared<ChFunction_Const>(1);
37 m_Rmax_modul = chrono_types::make_shared<ChFunction_Const>(1);
38 m_Rmin_modul = chrono_types::make_shared<ChFunction_Const>(1);
39 m_polarMax_funct = chrono_types::make_shared<ChFunction_Const>(1);
40
41 constr_upper.SetMode(CONSTRAINT_UNILATERAL);
42 constr_lower.SetMode(CONSTRAINT_UNILATERAL);
43 }
44
ChLinkLimit(const ChLinkLimit & other)45 ChLinkLimit::ChLinkLimit(const ChLinkLimit& other) {
46 m_active = other.m_active;
47 m_penalty_only = other.m_penalty_only;
48 m_polar = other.m_polar;
49 m_rotation = other.m_rotation;
50
51 m_max = other.m_max;
52 m_min = other.m_min;
53 m_maxCushion = other.m_maxCushion;
54 m_minCushion = other.m_minCushion;
55 m_Kmax = other.m_Kmax;
56 m_Kmin = other.m_Kmin;
57 m_Rmax = other.m_Rmax;
58 m_Rmin = other.m_Rmin;
59 m_minElastic = other.m_minElastic;
60 m_maxElastic = other.m_maxElastic;
61
62 m_Kmax_modul = std::shared_ptr<ChFunction>(other.m_Kmax_modul->Clone());
63 m_Kmin_modul = std::shared_ptr<ChFunction>(other.m_Kmin_modul->Clone());
64 m_Rmax_modul = std::shared_ptr<ChFunction>(other.m_Rmax_modul->Clone());
65 m_Rmin_modul = std::shared_ptr<ChFunction>(other.m_Rmin_modul->Clone());
66 m_polarMax_funct = std::shared_ptr<ChFunction>(other.m_polarMax_funct->Clone());
67 }
68
SetMax(double val)69 void ChLinkLimit::SetMax(double val) {
70 m_max = val;
71 if (m_max < m_min)
72 m_min = m_max;
73 if (m_max - m_maxCushion < m_min)
74 m_maxCushion = m_max - m_min;
75 if (m_max - m_maxCushion < m_min + m_minCushion)
76 m_minCushion = m_max - m_min - m_maxCushion;
77 constr_upper.SetActive(true);
78 }
79
SetMin(double val)80 void ChLinkLimit::SetMin(double val) {
81 m_min = val;
82 if (m_min > m_max)
83 m_max = m_min;
84 if (m_min + m_minCushion > m_max)
85 m_minCushion = m_max - m_min;
86 if (m_min + m_minCushion > m_max - m_maxCushion)
87 m_maxCushion = m_max - m_min - m_minCushion;
88 constr_lower.SetActive(true);
89 }
90
SetMaxCushion(double val)91 void ChLinkLimit::SetMaxCushion(double val) {
92 m_maxCushion = val;
93 if (m_max - m_maxCushion < m_min)
94 m_maxCushion = m_max - m_min;
95 if (m_max - m_maxCushion < m_min + m_minCushion)
96 m_minCushion = m_max - m_min - m_maxCushion;
97 }
98
SetMinCushion(double val)99 void ChLinkLimit::SetMinCushion(double val) {
100 m_minCushion = val;
101 if (m_min + m_minCushion > m_max)
102 m_minCushion = m_max - m_min;
103 if (m_min + m_minCushion > m_max - m_maxCushion)
104 m_maxCushion = m_max - m_min - m_minCushion;
105 }
106
107 // file parsing / dumping
ArchiveOUT(ChArchiveOut & marchive)108 void ChLinkLimit::ArchiveOUT(ChArchiveOut& marchive) {
109 // class version number
110 marchive.VersionWrite<ChLinkLimit>();
111
112 // stream out all member data
113 marchive << CHNVP(m_active);
114 marchive << CHNVP(m_penalty_only);
115 marchive << CHNVP(m_polar);
116 marchive << CHNVP(m_rotation);
117 marchive << CHNVP(m_max);
118 marchive << CHNVP(m_min);
119 marchive << CHNVP(m_maxCushion);
120 marchive << CHNVP(m_minCushion);
121 marchive << CHNVP(m_Kmax);
122 marchive << CHNVP(m_Kmin);
123 marchive << CHNVP(m_Rmax);
124 marchive << CHNVP(m_Rmin);
125 marchive << CHNVP(m_maxElastic);
126 marchive << CHNVP(m_minElastic);
127 marchive << CHNVP(m_Kmax_modul);
128 marchive << CHNVP(m_Kmin_modul);
129 marchive << CHNVP(m_Rmax_modul);
130 marchive << CHNVP(m_Rmin_modul);
131 marchive << CHNVP(m_polarMax_funct);
132 }
133
ArchiveIN(ChArchiveIn & marchive)134 void ChLinkLimit::ArchiveIN(ChArchiveIn& marchive) {
135 // class version number
136 /*int version =*/ marchive.VersionRead<ChLinkLimit>();
137
138 // stream in all member data
139 marchive >> CHNVP(m_active);
140 marchive >> CHNVP(m_penalty_only);
141 marchive >> CHNVP(m_polar);
142 marchive >> CHNVP(m_rotation);
143 marchive >> CHNVP(m_max);
144 marchive >> CHNVP(m_min);
145 marchive >> CHNVP(m_maxCushion);
146 marchive >> CHNVP(m_minCushion);
147 marchive >> CHNVP(m_Kmax);
148 marchive >> CHNVP(m_Kmin);
149 marchive >> CHNVP(m_Rmax);
150 marchive >> CHNVP(m_Rmin);
151 marchive >> CHNVP(m_maxElastic);
152 marchive >> CHNVP(m_minElastic);
153 marchive >> CHNVP(m_Kmax_modul);
154 marchive >> CHNVP(m_Kmin_modul);
155 marchive >> CHNVP(m_Rmax_modul);
156 marchive >> CHNVP(m_Rmin_modul);
157 marchive >> CHNVP(m_polarMax_funct);
158 }
159
GetViolation(double x) const160 double ChLinkLimit::GetViolation(double x) const {
161 if (!m_active || m_penalty_only)
162 return 0;
163
164 if (x > m_min && x < m_max)
165 return 0;
166 if (x <= m_min)
167 return (x - m_min);
168 if (x >= m_max)
169 return (x - m_max);
170
171 return 0;
172 }
173
GetForce(double x,double x_dt) const174 double ChLinkLimit::GetForce(double x, double x_dt) const {
175 double cush_coord;
176 double cush_coord_norm;
177 double force;
178 double min_val, max_val;
179
180 if (!m_penalty_only) {
181 min_val = m_min;
182 max_val = m_max;
183 } else {
184 min_val = -999999999;
185 max_val = 999999999;
186 }
187
188 if (x > min_val && x < m_min + m_minCushion) {
189 cush_coord = (m_min + m_minCushion) - x;
190
191 if (m_minCushion >= 0.0000001)
192 cush_coord_norm = cush_coord / m_minCushion;
193 else
194 cush_coord_norm = 1;
195
196 if (cush_coord_norm > 1)
197 cush_coord_norm = 1; // clip cushion forces at stopper limit
198
199 force = cush_coord * m_Kmin * m_Kmin_modul->Get_y(cush_coord_norm);
200 force += (-x_dt) * m_Rmin * m_Rmin_modul->Get_y(cush_coord_norm);
201 if (force < 0) {
202 force = 0;
203 } // damping could cause neg force while going away,
204 // so -as the limit is not "sticky"- clip force sign.
205
206 return (force);
207 }
208
209 if (x < max_val && x > m_max - m_maxCushion) {
210 cush_coord = x - (m_max - m_maxCushion);
211
212 if (m_maxCushion >= 0.0000001)
213 cush_coord_norm = cush_coord / m_maxCushion;
214 else
215 cush_coord_norm = 1;
216
217 if (cush_coord_norm > 1)
218 cush_coord_norm = 1; // clip cushion forces at stopper limit
219
220 force = (-cush_coord) * m_Kmax * m_Kmax_modul->Get_y(cush_coord_norm);
221 force += (-x_dt) * m_Rmax * m_Rmax_modul->Get_y(cush_coord_norm);
222 if (force > 0) {
223 force = 0;
224 } // damping could cause pos force while going away,
225 // so -as the limit is not "sticky"- clip force sign.
226 return (force);
227 }
228 return 0;
229 }
230
GetMaxPolarAngle(double pol_ang) const231 double ChLinkLimit::GetMaxPolarAngle(double pol_ang) const {
232 if (!m_polarMax_funct)
233 return 0.001;
234 return m_polarMax_funct->Get_y(pol_ang);
235 }
236
237 // The same, but for conical limits, in polar coordinates
GetPolarForce(double x,double x_dt,double pol_ang) const238 double ChLinkLimit::GetPolarForce(double x, double x_dt, double pol_ang) const {
239 double cush_coord;
240 double cush_coord_norm;
241 double cushion_thick;
242 double force;
243 double max_val;
244 double ang_max;
245
246 if (!m_polarMax_funct)
247 return 0;
248
249 if (!m_penalty_only) {
250 max_val = m_max;
251 } else {
252 max_val = 999999999;
253 }
254
255 ang_max = m_polarMax_funct->Get_y(pol_ang);
256
257 if (x < max_val && x > ang_max - m_maxCushion) {
258 cushion_thick = m_maxCushion;
259 if (cushion_thick > ang_max)
260 cushion_thick = ang_max;
261
262 cush_coord = x - (ang_max - m_maxCushion);
263
264 if (cushion_thick >= 0.0000001)
265 cush_coord_norm = cush_coord / cushion_thick;
266 else
267 cush_coord_norm = 1;
268
269 // clip cushion forces at stopper limit
270 if (cush_coord_norm > 1)
271 cush_coord_norm = 1;
272
273 force = (-cush_coord) * m_Kmax * m_Kmax_modul->Get_y(cush_coord_norm);
274 force += (-x_dt) * m_Rmax * m_Rmax_modul->Get_y(cush_coord_norm);
275
276 // damping could cause pos force while going away,
277 // so, since the limit is not "sticky", clip force sign.
278 if (force > 0) {
279 force = 0;
280 }
281 return (force);
282 }
283
284 return 0;
285 }
286
287 } // end namespace chrono
288