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/ChLoad.h"
16
17 namespace chrono {
18
SetVariables(std::vector<ChVariables * > mvariables)19 void ChLoadJacobians::SetVariables(std::vector<ChVariables*> mvariables) {
20 KRM.SetVariables(mvariables);
21 auto nscalar_coords = KRM.Get_K().cols();
22 K.setZero(nscalar_coords, nscalar_coords);
23 R.setZero(nscalar_coords, nscalar_coords);
24 M.setZero(nscalar_coords, nscalar_coords);
25 }
26
27 // -----------------------------------------------------------------------------
28
ChLoadBase()29 ChLoadBase::ChLoadBase() : jacobians(nullptr) {}
30
~ChLoadBase()31 ChLoadBase::~ChLoadBase() {
32 delete jacobians;
33 }
34
Update(double time)35 void ChLoadBase::Update(double time) {
36 // current state speed & position
37 ChState mstate_x(LoadGet_ndof_x(), 0);
38 LoadGetStateBlock_x(mstate_x);
39 ChStateDelta mstate_w(LoadGet_ndof_w(), 0);
40 LoadGetStateBlock_w(mstate_w);
41 // compute the applied load, at current state
42 ComputeQ(&mstate_x, &mstate_w);
43 // compute the jacobian, at current state
44 if (IsStiff()) {
45 if (!jacobians)
46 CreateJacobianMatrices();
47 ComputeJacobian(&mstate_x, &mstate_w, jacobians->K, jacobians->R, jacobians->M);
48 }
49 };
50
InjectKRMmatrices(ChSystemDescriptor & mdescriptor)51 void ChLoadBase::InjectKRMmatrices(ChSystemDescriptor& mdescriptor) {
52 if (jacobians) {
53 mdescriptor.InsertKblock(&jacobians->KRM);
54 }
55 }
56
KRMmatricesLoad(double Kfactor,double Rfactor,double Mfactor)57 void ChLoadBase::KRMmatricesLoad(double Kfactor, double Rfactor, double Mfactor) {
58 if (jacobians) {
59 jacobians->KRM.Get_K().setZero();
60 jacobians->KRM.Get_K() += jacobians->K * Kfactor;
61 jacobians->KRM.Get_K() += jacobians->R * Rfactor;
62 jacobians->KRM.Get_K() += jacobians->M * Mfactor;
63 }
64 }
65
66 // -----------------------------------------------------------------------------
67
ChLoadCustom(std::shared_ptr<ChLoadable> mloadable)68 ChLoadCustom::ChLoadCustom(std::shared_ptr<ChLoadable> mloadable) : loadable(mloadable) {
69 load_Q.setZero(LoadGet_ndof_w());
70 }
71
LoadGet_ndof_x()72 int ChLoadCustom::LoadGet_ndof_x() {
73 return loadable->LoadableGet_ndof_x();
74 }
LoadGet_ndof_w()75 int ChLoadCustom::LoadGet_ndof_w() {
76 return loadable->LoadableGet_ndof_w();
77 }
LoadGetStateBlock_x(ChState & mD)78 void ChLoadCustom::LoadGetStateBlock_x(ChState& mD) {
79 loadable->LoadableGetStateBlock_x(0, mD);
80 }
LoadGetStateBlock_w(ChStateDelta & mD)81 void ChLoadCustom::LoadGetStateBlock_w(ChStateDelta& mD) {
82 loadable->LoadableGetStateBlock_w(0, mD);
83 }
LoadStateIncrement(const ChState & x,const ChStateDelta & dw,ChState & x_new)84 void ChLoadCustom::LoadStateIncrement(const ChState& x, const ChStateDelta& dw, ChState& x_new) {
85 loadable->LoadableStateIncrement(0, x_new, x, 0, dw);
86 }
LoadGet_field_ncoords()87 int ChLoadCustom::LoadGet_field_ncoords() {
88 return loadable->Get_field_ncoords();
89 }
90
ComputeJacobian(ChState * state_x,ChStateDelta * state_w,ChMatrixRef mK,ChMatrixRef mR,ChMatrixRef mM)91 void ChLoadCustom::ComputeJacobian(ChState* state_x, // state position to evaluate jacobians
92 ChStateDelta* state_w, // state speed to evaluate jacobians
93 ChMatrixRef mK, // result dQ/dx
94 ChMatrixRef mR, // result dQ/dv
95 ChMatrixRef mM) // result dQ/da
96 {
97 double Delta = 1e-8;
98
99 int mrows_w = LoadGet_ndof_w();
100 int mrows_x = LoadGet_ndof_x();
101
102 // compute Q at current speed & position, x_0, v_0
103 ChVectorDynamic<> Q0(mrows_w);
104 ComputeQ(state_x, state_w); // Q0 = Q(x, v)
105 Q0 = load_Q;
106
107 ChVectorDynamic<> Q1(mrows_w);
108 ChVectorDynamic<> Jcolumn(mrows_w);
109 ChState state_x_inc(mrows_x, nullptr);
110 ChStateDelta state_delta(mrows_w, nullptr);
111
112 // Compute K=-dQ(x,v)/dx by backward differentiation
113 state_delta.setZero(mrows_w, nullptr);
114
115 for (int i = 0; i < mrows_w; ++i) {
116 state_delta(i) += Delta;
117 LoadStateIncrement(*state_x, state_delta,
118 state_x_inc); // exponential, usually state_x_inc(i) = state_x(i) + Delta;
119 ComputeQ(&state_x_inc, state_w); // Q1 = Q(x+Dx, v)
120 Q1 = load_Q;
121 state_delta(i) -= Delta;
122
123 Jcolumn = (Q1 - Q0) * (-1.0 / Delta); // - sign because K=-dQ/dx
124 jacobians->K.block(0, i, mrows_w, 1) = Jcolumn;
125 }
126 // Compute R=-dQ(x,v)/dv by backward differentiation
127 for (int i = 0; i < mrows_w; ++i) {
128 (*state_w)(i) += Delta;
129 ComputeQ(state_x, state_w); // Q1 = Q(x, v+Dv)
130 Q1 = load_Q;
131 (*state_w)(i) -= Delta;
132
133 Jcolumn = (Q1 - Q0) * (-1.0 / Delta); // - sign because R=-dQ/dv
134 jacobians->R.block(0, i, mrows_w, 1) = Jcolumn;
135 }
136 }
137
LoadIntLoadResidual_F(ChVectorDynamic<> & R,const double c)138 void ChLoadCustom::LoadIntLoadResidual_F(ChVectorDynamic<>& R, const double c) {
139 unsigned int rowQ = 0;
140 for (int i = 0; i < loadable->GetSubBlocks(); ++i) {
141 if (loadable->IsSubBlockActive(i)) {
142 unsigned int moffset = loadable->GetSubBlockOffset(i);
143 for (unsigned int row = 0; row < loadable->GetSubBlockSize(i); ++row) {
144 R(row + moffset) += load_Q(rowQ) * c;
145 ++rowQ;
146 }
147 }
148 }
149 }
150
CreateJacobianMatrices()151 void ChLoadCustom::CreateJacobianMatrices() {
152 if (!jacobians) {
153 // create jacobian structure
154 jacobians = new ChLoadJacobians;
155 // set variables forsparse KRM block
156 std::vector<ChVariables*> mvars;
157 loadable->LoadableGetVariables(mvars);
158 jacobians->SetVariables(mvars);
159 }
160 }
161
162 // -----------------------------------------------------------------------------
163
ChLoadCustomMultiple(std::vector<std::shared_ptr<ChLoadable>> & mloadables)164 ChLoadCustomMultiple::ChLoadCustomMultiple(std::vector<std::shared_ptr<ChLoadable>>& mloadables)
165 : loadables(mloadables) {
166 load_Q.setZero(LoadGet_ndof_w());
167 }
168
ChLoadCustomMultiple(std::shared_ptr<ChLoadable> mloadableA,std::shared_ptr<ChLoadable> mloadableB)169 ChLoadCustomMultiple::ChLoadCustomMultiple(std::shared_ptr<ChLoadable> mloadableA,
170 std::shared_ptr<ChLoadable> mloadableB) {
171 loadables.push_back(mloadableA);
172 loadables.push_back(mloadableB);
173 load_Q.setZero(LoadGet_ndof_w());
174 }
175
ChLoadCustomMultiple(std::shared_ptr<ChLoadable> mloadableA,std::shared_ptr<ChLoadable> mloadableB,std::shared_ptr<ChLoadable> mloadableC)176 ChLoadCustomMultiple::ChLoadCustomMultiple(std::shared_ptr<ChLoadable> mloadableA,
177 std::shared_ptr<ChLoadable> mloadableB,
178 std::shared_ptr<ChLoadable> mloadableC) {
179 loadables.push_back(mloadableA);
180 loadables.push_back(mloadableB);
181 loadables.push_back(mloadableC);
182 load_Q.setZero(LoadGet_ndof_w());
183 }
184
LoadGet_ndof_x()185 int ChLoadCustomMultiple::LoadGet_ndof_x() {
186 int ndoftot = 0;
187 for (int i = 0; i < loadables.size(); ++i)
188 ndoftot += loadables[i]->LoadableGet_ndof_x();
189 return ndoftot;
190 }
191
LoadGet_ndof_w()192 int ChLoadCustomMultiple::LoadGet_ndof_w() {
193 int ndoftot = 0;
194 for (int i = 0; i < loadables.size(); ++i)
195 ndoftot += loadables[i]->LoadableGet_ndof_w();
196 return ndoftot;
197 }
198
LoadGetStateBlock_x(ChState & mD)199 void ChLoadCustomMultiple::LoadGetStateBlock_x(ChState& mD) {
200 int ndoftot = 0;
201 for (int i = 0; i < loadables.size(); ++i) {
202 loadables[i]->LoadableGetStateBlock_x(ndoftot, mD);
203 ndoftot += loadables[i]->LoadableGet_ndof_x();
204 }
205 }
206
LoadGetStateBlock_w(ChStateDelta & mD)207 void ChLoadCustomMultiple::LoadGetStateBlock_w(ChStateDelta& mD) {
208 int ndoftot = 0;
209 for (int i = 0; i < loadables.size(); ++i) {
210 loadables[i]->LoadableGetStateBlock_w(ndoftot, mD);
211 ndoftot += loadables[i]->LoadableGet_ndof_w();
212 }
213 }
214
LoadStateIncrement(const ChState & x,const ChStateDelta & dw,ChState & x_new)215 void ChLoadCustomMultiple::LoadStateIncrement(const ChState& x, const ChStateDelta& dw, ChState& x_new) {
216 int ndoftotx = 0;
217 int ndoftotw = 0;
218 for (int i = 0; i < loadables.size(); ++i) {
219 loadables[i]->LoadableStateIncrement(ndoftotx, x_new, x, ndoftotw, dw);
220 ndoftotx += loadables[i]->LoadableGet_ndof_x();
221 ndoftotw += loadables[i]->LoadableGet_ndof_w();
222 }
223 }
224
LoadGet_field_ncoords()225 int ChLoadCustomMultiple::LoadGet_field_ncoords() {
226 return loadables[0]->Get_field_ncoords();
227 }
228
ComputeJacobian(ChState * state_x,ChStateDelta * state_w,ChMatrixRef mK,ChMatrixRef mR,ChMatrixRef mM)229 void ChLoadCustomMultiple::ComputeJacobian(ChState* state_x, // state position to evaluate jacobians
230 ChStateDelta* state_w, // state speed to evaluate jacobians
231 ChMatrixRef mK, // result dQ/dx
232 ChMatrixRef mR, // result dQ/dv
233 ChMatrixRef mM) // result dQ/da
234 {
235 double Delta = 1e-8;
236
237 int mrows_w = LoadGet_ndof_w();
238 int mrows_x = LoadGet_ndof_x();
239
240 // compute Q at current speed & position, x_0, v_0
241 ChVectorDynamic<> Q0(mrows_w);
242 ComputeQ(state_x, state_w); // Q0 = Q(x, v)
243 Q0 = load_Q;
244
245 ChVectorDynamic<> Q1(mrows_w);
246 ChVectorDynamic<> Jcolumn(mrows_w);
247 ChState state_x_inc(mrows_x, nullptr);
248 ChStateDelta state_delta(mrows_w, nullptr);
249
250 // Compute K=-dQ(x,v)/dx by backward differentiation
251 state_delta.setZero(mrows_w, nullptr);
252
253 for (int i = 0; i < mrows_w; ++i) {
254 state_delta(i) += Delta;
255 LoadStateIncrement(*state_x, state_delta,
256 state_x_inc); // exponential, usually state_x_inc(i) = state_x(i) + Delta;
257 ComputeQ(&state_x_inc, state_w); // Q1 = Q(x+Dx, v)
258 Q1 = load_Q;
259 state_delta(i) -= Delta;
260
261 Jcolumn = (Q1 - Q0) * (-1.0 / Delta); // - sign because K=-dQ/dx
262 jacobians->K.block(0, i, mrows_w, 1) = Jcolumn;
263 }
264 // Compute R=-dQ(x,v)/dv by backward differentiation
265 for (int i = 0; i < mrows_w; ++i) {
266 (*state_w)(i) += Delta;
267 ComputeQ(state_x, state_w); // Q1 = Q(x, v+Dv)
268 Q1 = load_Q;
269 (*state_w)(i) -= Delta;
270
271 Jcolumn = (Q1 - Q0) * (-1.0 / Delta); // - sign because R=-dQ/dv
272 jacobians->R.block(0, i, mrows_w, 1) = Jcolumn;
273 }
274 }
275
LoadIntLoadResidual_F(ChVectorDynamic<> & R,const double c)276 void ChLoadCustomMultiple::LoadIntLoadResidual_F(ChVectorDynamic<>& R, const double c) {
277 unsigned int mQoffset = 0;
278 for (int k = 0; k < loadables.size(); ++k) {
279 for (int i = 0; i < loadables[k]->GetSubBlocks(); ++i) {
280 if (loadables[k]->IsSubBlockActive(i)) {
281 unsigned int mblockoffset = loadables[k]->GetSubBlockOffset(i);
282 for (unsigned int row = 0; row < loadables[k]->GetSubBlockSize(i); ++row) {
283 R(row + mblockoffset) += load_Q(row + mQoffset) * c;
284 }
285 }
286 mQoffset += loadables[k]->GetSubBlockSize(i);
287 }
288 }
289 // GetLog() << " debug: R=" << R << "\n";
290 }
291
CreateJacobianMatrices()292 void ChLoadCustomMultiple::CreateJacobianMatrices() {
293 if (!jacobians) {
294 // create jacobian structure
295 jacobians = new ChLoadJacobians;
296 // set variables for sparse KRM block appending them to mvars list
297 std::vector<ChVariables*> mvars;
298 for (int i = 0; i < loadables.size(); ++i)
299 loadables[i]->LoadableGetVariables(mvars);
300 jacobians->SetVariables(mvars);
301 }
302 }
303
304 } // end namespace chrono
305