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