MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
module-muscles.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/modules/module-muscles/module-muscles.cc,v 1.20 2017/01/12 14:55:27 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 /*
32  * Author: Andrea Zanoni <andrea.zanoni@polimi.it>
33  * Pierangelo Masarati <masarati@aero.polimi.it>
34  */
35 
36 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
37 
38 #include <cmath>
39 #include <cfloat>
40 
41 #include "dataman.h"
42 #include "constltp_impl.h"
43 
45 : public ElasticConstitutiveLaw<doublereal, doublereal> {
46 protected:
54 
55  virtual std::ostream& Restart_int(std::ostream& out) const {
56  return out;
57  };
58 
59 public:
62  const DriveCaller *pAct, bool bActivationOverflow)
63  : ElasticConstitutiveLaw<doublereal, doublereal>(pTplDC, dPreStress),
64  Li(Li), L0(L0), V0(V0), F0(F0),
65  Activation(pAct), bActivationOverflow(bActivationOverflow)
66  {
67  NO_OP;
68  };
69 
70  virtual ~MusclePennestriCL(void) {
71  NO_OP;
72  };
73 
74  virtual ConstLawType::Type GetConstLawType(void) const {
76  };
77 
80 
81  // pass parameters to copy constructor
84  PreStress,
85  Li, L0, V0, F0,
88  return pCL;
89  };
90 
91  virtual std::ostream& Restart(std::ostream& out) const {
92  out << "muscle pennestri"
93  ", initial length, " << Li
94  << ", reference length, " << L0
95  << ", reference velocity, " << V0
96  << ", reference force, " << F0
97  << ", activation, ", Activation.pGetDriveCaller()->Restart(out)
98  << ", activation check, " << bActivationOverflow,
99  Restart_int(out)
101  return out;
102  };
103 
104  virtual std::ostream& OutputAppend(std::ostream& out) const {
105  return out << " " << a;
106  };
107 
108  virtual void Update(const doublereal& Eps, const doublereal& EpsPrime) {
111 
112  a = Activation.dGet();
113  if (a < 0.) {
114  silent_cerr("MusclePennestriCL: activation underflow (a=" << a << ")" << std::endl);
115  if (bActivationOverflow) {
116  a = 0.;
117  }
118 
119  } else if (a > 1.) {
120  silent_cerr("MusclePennestriCL: activation overflow (a=" << a << ")" << std::endl);
121  if (bActivationOverflow) {
122  a = 1.;
123  }
124  }
125 
126  doublereal dxdEps = Li/L0;
127  doublereal dvdEpsPrime = Li/V0;
130 
131  doublereal f1 = std::exp(std::pow(x - 0.95, 2) - 40*std::pow(x - 0.95, 4));
132  doublereal f2 = 1.6 - 1.6*std::exp(0.1/std::pow(v - 1., 2) - 1.1/std::pow(v - 1., 4));
133  doublereal f3 = 1.3*std::atan(0.1*std::pow(x - 0.22, 10));
134 
135  doublereal df1dx = f1*(2*(x - 0.95) - 4*40.*std::pow(x - 0.95, 3));
136  doublereal df2dv = 1.6*std::exp(0.1/std::pow(v - 1., 2) - 1.1/std::pow(v - 1, 4))*(2*0.1/std::pow(v - 1., 3) - 4*1.1/std::pow(v - 1., 5));
137  doublereal df3dx = 1.3*std::pow(x - 0.22, 9)/(0.01*std::pow(x - 0.22, 20) + 1);
138 
140  ConstitutiveLaw<doublereal, doublereal>::FDE = F0*(df1dx*f2*a + df3dx)*dxdEps;
142  };
143 };
144 
146 : public MusclePennestriCL {
147 public:
150  const DriveCaller *pAct, bool bActivationOverflow)
151  : MusclePennestriCL(pTplDC, dPreStress, Li, L0, V0, F0, pAct, bActivationOverflow)
152  {
153  NO_OP;
154  };
155 
156  virtual ~MusclePennestriErgoCL(void) {
157  NO_OP;
158  };
159 
160  virtual ConstLawType::Type GetConstLawType(void) const {
161  return ConstLawType::ELASTIC;
162  };
163 
166 
167  // pass parameters to copy constructor
170  PreStress,
171  Li, L0, V0, F0,
174  return pCL;
175  };
176 
177  virtual void Update(const doublereal& Eps, const doublereal& EpsPrime) {
178  MusclePennestriCL::Update(Eps, 0.);
179  };
180 
181 protected:
182  virtual std::ostream& Restart_int(std::ostream& out) const {
183  out << ", ergonomy, yes";
184  return out;
185  };
186 };
187 
189 : public MusclePennestriCL {
190 protected:
194 
195 public:
198  const DriveCaller *pAct, bool bActivationOverflow,
199  doublereal dKp, doublereal dKd, const DriveCaller *pReferenceLength)
200  : MusclePennestriCL(pTplDC, dPreStress, Li, L0, V0, F0, pAct, bActivationOverflow),
201  dKp(dKp), dKd(dKd), ReferenceLength(pReferenceLength)
202  {
203  NO_OP;
204  };
205 
207  NO_OP;
208  };
209 
212 
213  // pass parameters to copy constructor
216  PreStress,
217  Li, L0, V0, F0,
220  dKp, dKd,
222  return pCL;
223  };
224 
225  virtual void Update(const doublereal& Eps, const doublereal& EpsPrime) {
228 
229  doublereal dxdEps = Li/L0;
230  doublereal dvdEpsPrime = Li/V0;
233 
234  doublereal dLRef = ReferenceLength.dGet()/L0;
235 
236  doublereal aRef = Activation.dGet();
237  a = aRef + dKp*(x - dLRef) + dKd*v;
238  if (a < 0.) {
239  silent_cerr("MusclePennestriCL: activation underflow (a=" << a << ")" << std::endl);
240  if (bActivationOverflow) {
241  a = 0.;
242  }
243 
244  } else if (a > 1.) {
245  silent_cerr("MusclePennestriCL: activation overflow (a=" << a << ")" << std::endl);
246  if (bActivationOverflow) {
247  a = 1.;
248  }
249  }
250 
251  doublereal f1 = std::exp(std::pow(x - 0.95, 2) - 40*std::pow(x - 0.95, 4));
252  doublereal f2 = 1.6 - 1.6*std::exp(0.1/std::pow(v - 1., 2) - 1.1/std::pow(v - 1., 4));
253  doublereal f3 = 1.3*std::atan(0.1*std::pow(x - 0.22, 10));
254 
255  doublereal df1dx = f1*(2*(x - 0.95) - 4*40.*std::pow(x - 0.95, 3));
256  doublereal df2dv = 1.6*std::exp(0.1/std::pow(v - 1., 2) - 1.1/std::pow(v - 1, 4))*(2*0.1/std::pow(v - 1., 3) - 4*1.1/std::pow(v - 1., 5));
257  doublereal df3dx = 1.3*std::pow(x - 0.22, 9)/(0.01*std::pow(x - 0.22, 20) + 1);
258 
260  ConstitutiveLaw<doublereal, doublereal>::FDE = F0*((df1dx*aRef + f1*dKp)*f2 + df3dx)*dxdEps;
261  ConstitutiveLaw<doublereal, doublereal>::FDEPrime = F0*f1*(df2dv*aRef + f2*dKd)*dvdEpsPrime;
262  };
263 
264 protected:
265  virtual std::ostream& Restart_int(std::ostream& out) const {
266  out
267  << ", reflexive"
268  << ", proportional gain, " << dKp
269  << ", derivative gain, " << dKd
270  << ", reference length, ", ReferenceLength.pGetDriveCaller()->Restart(out);
271  return out;
272  };
273 };
274 
275 /* specific functional object(s) */
276 struct MusclePennestriCLR : public ConstitutiveLawRead<doublereal, doublereal> {
278  Read(const DataManager* pDM, MBDynParser& HP, ConstLawType::Type& CLType) {
280 
282 
283  if (HP.IsKeyWord("help")) {
284  silent_cerr("MusclePennestriCL:\n"
285  " muscle Pennestri ,\n"
286  " [ initial length , <Li> , ]\n"
287  " reference length , <L0> ,\n"
288  " [ reference velocity , <V0> , ]\n"
289  " reference force , <F0> ,\n"
290  " activation , (DriveCaller) <activation>\n"
291  " [ , activation check , (bool)<activation_check> ]\n"
292  " [ , ergonomy , { yes | no } , ]\n"
293  " [ , reflexive , # only when ergonomy == no\n"
294  " proportional gain , <kp> ,\n"
295  " derivative gain , <kd> ,\n"
296  " reference length, (DriveCaller) <lref> ]\n"
297  " [ , prestress, <prestress> ]\n"
298  " [ , prestrain, (DriveCaller) <prestrain> ]\n"
299  << std::endl);
300 
301  if (!HP.IsArg()) {
302  throw NoErr(MBDYN_EXCEPT_ARGS);
303  }
304  }
305 
306  bool bErgo(false);
307  bool bGotErgo(false);
308  if (HP.IsKeyWord("ergonomy")) {
309  silent_cerr("MusclePennestriCL: deprecated, \"ergonomy\" "
310  "at line " << HP.GetLineData()
311  << " should be at end of definition" << std::endl);
312  bErgo = HP.GetYesNoOrBool(bErgo);
313  bGotErgo = true;
314  }
315 
316  doublereal Li = -1.;
317  if (HP.IsKeyWord("initial" "length")) {
318  Li = HP.GetReal();
319  if (Li <= 0.) {
320  silent_cerr("MusclePennestriCL: null or negative initial length "
321  "at line " << HP.GetLineData() << std::endl);
323  }
324  }
325 
326  if (!HP.IsKeyWord("reference" "length")) {
327  silent_cerr("MusclePennestriCL: \"reference length\" expected "
328  "at line " << HP.GetLineData() << std::endl);
330  }
331  doublereal L0 = HP.GetReal();
332  if (L0 <= 0.) {
333  silent_cerr("MusclePennestriCL: null or negative reference length "
334  "at line " << HP.GetLineData() << std::endl);
336  }
337 
338  // default (mm/s? m/s?)
339  doublereal V0 = 2.5;
340  if (HP.IsKeyWord("reference" "velocity")) {
341  V0 = HP.GetReal();
342  if (V0 <= 0.) {
343  silent_cerr("MusclePennestriCL: null or negative reference velocity "
344  "at line " << HP.GetLineData() << std::endl);
346  }
347  }
348 
349  if (!HP.IsKeyWord("reference" "force")) {
350  silent_cerr("MusclePennestriCL: \"reference force\" expected "
351  "at line " << HP.GetLineData() << std::endl);
353  }
354  doublereal F0 = HP.GetReal();
355  if (F0 <= 0.) {
356  silent_cerr("MusclePennestriCL: null or negative reference force "
357  "at line " << HP.GetLineData() << std::endl);
359  }
360 
361  if (!HP.IsKeyWord("activation")) {
362  silent_cerr("MusclePennestriCL: \"activation\" expected "
363  "at line " << HP.GetLineData() << std::endl);
365  }
366  const DriveCaller *pAct = HP.GetDriveCaller();
367 
368  bool bActivationOverflow(false);
369  if (HP.IsKeyWord("activation" "check")) {
370  bActivationOverflow = HP.GetYesNoOrBool(bActivationOverflow);
371  }
372 
373  // FIXME: "ergonomy" must be here
374  if (!bGotErgo && HP.IsKeyWord("ergonomy")) {
375  bErgo = HP.GetYesNoOrBool(bErgo);
376  }
377 
378  bool bReflexive(false);
379  doublereal dKp(0.);
380  doublereal dKd(0.);
381  const DriveCaller *pRefLen(0);
382  if (HP.IsKeyWord("reflexive")) {
383  if (bErgo) {
384  silent_cerr("MusclePennestriCL: "
385  "\"reflexive\" and \"ergonomy\" incompatible "
386  "at line " << HP.GetLineData() << std::endl);
388  }
389  bReflexive = true;
390 
391  if (!HP.IsKeyWord("proportional" "gain")) {
392  silent_cerr("MusclePennestriCL: \"proportional gain\" expected "
393  "at line " << HP.GetLineData() << std::endl);
395  }
396  dKp = HP.GetReal();
397 
398  if (!HP.IsKeyWord("derivative" "gain")) {
399  silent_cerr("MusclePennestriCL: \"derivative gain\" expected "
400  "at line " << HP.GetLineData() << std::endl);
402  }
403  dKd = HP.GetReal();
404 
405  if (!HP.IsKeyWord("reference" "length")) {
406  silent_cerr("MusclePennestriCL: \"reference length\" expected "
407  "at line " << HP.GetLineData() << std::endl);
409  }
410  pRefLen = HP.GetDriveCaller();
411  }
412 
413  /* Prestress and prestrain */
414  doublereal PreStress(0.);
415  GetPreStress(HP, PreStress);
416  TplDriveCaller<doublereal> *pTplDC = GetPreStrain<doublereal>(pDM, HP);
417 
418  if (Li == -1.) {
419  Li = L0;
420  }
421 
422  if (bErgo) {
424  MusclePennestriErgoCL(pTplDC, PreStress,
425  Li, L0, V0, F0, pAct,
426  bActivationOverflow));
427 
428  } else if (bReflexive) {
430  MusclePennestriReflexiveCL(pTplDC, PreStress,
431  Li, L0, V0, F0, pAct,
432  bActivationOverflow,
433  dKp, dKd, pRefLen));
434 
435  } else {
437  MusclePennestriCL(pTplDC, PreStress,
438  Li, L0, V0, F0, pAct,
439  bActivationOverflow));
440  }
441 
442  return pCL;
443  };
444 };
445 
446 extern "C" int
447 module_init(const char *module_name, void *pdm, void *php)
448 {
449 #if 0
450  DataManager *pDM = (DataManager *)pdm;
451  MBDynParser *pHP = (MBDynParser *)php;
452 #endif
453 
455  if (!SetCL1D("muscle" "pennestri", rf1D)) {
456  delete rf1D;
457 
458  silent_cerr("MusclePennestriCL: "
459  "module_init(" << module_name << ") "
460  "failed" << std::endl);
461 
462  return -1;
463  }
464 
465  return 0;
466 }
467 
GradientExpression< UnaryExpr< FuncExp, Expr > > exp(const GradientExpression< Expr > &u)
Definition: gradient.h:2975
virtual ConstitutiveLaw< doublereal, doublereal > * pCopy(void) const
virtual ~MusclePennestriCL(void)
virtual void Update(const doublereal &Eps, const doublereal &EpsPrime)
virtual void Update(const doublereal &Eps, const doublereal &EpsPrime)
virtual ~MusclePennestriErgoCL(void)
GradientExpression< BinaryExpr< FuncPow, LhsExpr, RhsExpr > > pow(const GradientExpression< LhsExpr > &u, const GradientExpression< RhsExpr > &v)
Definition: gradient.h:2961
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
virtual ConstLawType::Type GetConstLawType(void) const
virtual ConstLawType::Type GetConstLawType(void) const
virtual std::ostream & Restart(std::ostream &out) const
MusclePennestriCL(const TplDriveCaller< doublereal > *pTplDC, doublereal dPreStress, doublereal Li, doublereal L0, doublereal V0, doublereal F0, const DriveCaller *pAct, bool bActivationOverflow)
MusclePennestriReflexiveCL(const TplDriveCaller< doublereal > *pTplDC, doublereal dPreStress, doublereal Li, doublereal L0, doublereal V0, doublereal F0, const DriveCaller *pAct, bool bActivationOverflow, doublereal dKp, doublereal dKd, const DriveCaller *pReferenceLength)
#define NO_OP
Definition: myassert.h:74
virtual std::ostream & Restart_int(std::ostream &out) const
virtual std::ostream & Restart(std::ostream &out) const =0
virtual std::ostream & Restart_int(std::ostream &out) const
virtual ConstitutiveLaw< doublereal, doublereal > * Read(const DataManager *pDM, MBDynParser &HP, ConstLawType::Type &CLType)
virtual ConstitutiveLaw< doublereal, doublereal > * pCopy(void) const
virtual bool GetYesNoOrBool(bool bDefval=false)
Definition: parser.cc:1038
virtual std::ostream & Restart_int(std::ostream &out) const
virtual ConstitutiveLaw< doublereal, doublereal > * pCopy(void) const
TplDriveCaller< doublereal > * pGetDriveCaller(void) const
Definition: tpldrive.h:105
virtual bool IsKeyWord(const char *sKeyWord)
Definition: parser.cc:910
MusclePennestriErgoCL(const TplDriveCaller< doublereal > *pTplDC, doublereal dPreStress, doublereal Li, doublereal L0, doublereal V0, doublereal F0, const DriveCaller *pAct, bool bActivationOverflow)
virtual void Update(const doublereal &Eps, const doublereal &EpsPrime)
bool SetCL1D(const char *name, ConstitutiveLawRead< doublereal, doublereal > *rf)
#define SAFENEWWITHCONSTRUCTOR(pnt, item, constructor)
Definition: mynewmem.h:698
DriveCaller * pGetDriveCaller(void) const
Definition: drive.cc:658
void GetPreStress(MBDynParser &HP, T &PreStress)
Definition: except.h:79
virtual DriveCaller * pCopy(void) const =0
virtual bool IsArg(void)
Definition: parser.cc:807
DriveOwner Activation
doublereal dGet(const doublereal &dVar) const
Definition: drive.cc:664
virtual ~MusclePennestriReflexiveCL(void)
DriveCaller * GetDriveCaller(bool bDeferred=false)
Definition: mbpar.cc:2033
GradientExpression< UnaryExpr< FuncAtan, Expr > > atan(const GradientExpression< Expr > &u)
Definition: gradient.h:2985
double doublereal
Definition: colamd.c:52
int module_init(const char *module_name, void *pdm, void *php)
This function registers our user defined element for the math parser.
virtual HighParser::ErrOut GetLineData(void) const
Definition: parsinc.cc:697
static std::stack< const HighParser * > pHP
Definition: parser.cc:598
virtual std::ostream & Restart_int(std::ostream &out) const
T Get(void) const
Definition: tpldrive.h:113
virtual std::ostream & OutputAppend(std::ostream &out) const
virtual doublereal GetReal(const doublereal &dDefval=0.0)
Definition: parser.cc:1056