163 const char* sKeyWords[] = {
167 "discrete" "control",
170 "adaptive" "control",
199 switch (CurrKeyWord) {
200 case ACCELEROMETER: {
224 silent_cerr(
"Accelerometer(" << uLabel <<
"): "
230 DEBUGCOUT(
"Direction: " << std::endl << Dir << std::endl);
255 silent_cerr(
"you shouldn't be here!" << std::endl);
274 silent_cerr(
"Accelerometer(" << uLabel <<
"): "
280 DEBUGCOUT(
"Direction: " << std::endl << Dir << std::endl);
285 silent_cerr(
"Accelerometer(" << uLabel <<
"): "
294 silent_cerr(
"Accelerometer(" << uLabel <<
"): "
302 if (dCsi <= 0. || dCsi > 1.) {
303 silent_cerr(
"Accelerometer(" << uLabel <<
"): "
312 silent_cerr(
"Accelerometer(" << uLabel <<
"): "
322 <<
", Kappa: " << dKappa
329 Dir, dOmega, dTau, dCsi, dKappa,
366 pStrNode1, pStrNode2, pAbsNode,
367 Tmpf1, Tmpf2, fOut));
381 bool bHaveOrientation =
false;
385 bHaveOrientation =
true;
395 for (
integer i = 1; i <= 3; ++i) {
415 silent_cerr(
"Motor(" << uLabel <<
"): "
416 "illegal motor direction "
442 bool bHaveM0orM1 =
false;
465 if (!bHaveOrientation) {
466 silent_cerr(
"motor(" << uLabel
467 <<
"): \"M0\" and \"M1\" require rotation matrix instead of axis at line " << HP.
GetLineData() << std::endl);
472 silent_cerr(
"motor(" << uLabel <<
"): keyword \"terminal pairs\" expected at line "
480 silent_cerr(
"motor(" << uLabel
481 <<
"): number of terminal pairs must be greater than zero at line "
491 pStrNode1, pStrNode2, pVoltage1, pVoltage2,
492 Rn, dG, dL, dR, i0, p, pM0, pM1, fOut));
495 case DISCRETECONTROL: {
507 DEBUGCOUT(
"Discrete controller of order " << iOrderA);
508 if (iOrderB != iOrderA) {
509 DEBUGCOUT(
" (fir order " << iOrderB <<
')' << std::endl);
511 DEBUGCOUT(
": " << iNumOutputs <<
" output(s) and "
512 << iNumInputs <<
" input(s)" << std::endl
513 <<
"Update every " << iNumIter <<
" iterations" << std::endl);
521 std::string infile = s;
524 "from file \"" << infile <<
"\""
537 case IDENTIFICATION: {
546 silent_cerr(
"DiscreteControl(" << uLabel <<
"): "
547 "unknown identification type "
575 case ADAPTIVECONTROL: {
581 DEBUGCOUT(
"ARX adaptive control" << std::endl);
585 DEBUGCOUT(
"ARMAX adaptive control" << std::endl);
590 dPeriodicFactor = HP.
GetReal();
595 DEBUGCOUT(
"GPC adaptive control" << std::endl);
602 DEBUGCOUT(
"prediction advancing horizon: " << iPredS << std::endl
603 <<
"prediction receding horizon: " << iPredH << std::endl
604 <<
"control advancing horizon: " << iContrS << std::endl
605 <<
"control receding horizon: " << iContrH << std::endl);
608 silent_cerr(
"DiscreteControl(" << uLabel <<
"): "
609 "prediction advancing horizon "
617 silent_cerr(
"DiscreteControl(" << uLabel <<
"): "
618 "prediction receding horizon "
625 if (iPredH >= iPredS) {
626 silent_cerr(
"DiscreteControl(" << uLabel <<
"): "
627 "prediction receding horizon "
629 "prediction advancing horizon "
636 silent_cerr(
"DiscreteControl(" << uLabel <<
"): "
637 "control advancing horizon "
649 if (HP.
IsKeyWord(
"prediction" "weights")) {
650 DEBUGCOUT(
"prediction weights:" << std::endl);
651 for (
integer i = iPredS - iPredH; i-- > 0; ) {
653 DEBUGCOUT(
"W[" << i + 1 <<
"] = " << pW[i] << std::endl);
657 for (
integer i = 0; i < iPredS - iPredH; i++) {
663 DEBUGCOUT(
"control weights:" << std::endl);
664 for (
integer i = iContrS - iContrH; i-- > 0; ) {
666 DEBUGCOUT(
"R[" << i + 1 <<
"] = " << pR[i] << std::endl);
669 for (
integer i = 0; i < iContrS-iContrH; i++) {
678 GPC(iNumOutputs, iNumInputs,
683 dPeriodicFactor, f_proc));
686 DEBUGCOUT(
"DeadBeat adaptive control" << std::endl);
689 int iContrS = HP.
GetInt();
691 DeadBeat(iNumOutputs, iNumInputs,
694 dPeriodicFactor, f_proc));
697 silent_cerr(
"DiscreteControl(" << uLabel <<
"): "
698 "unknown adaptive control type at line " << HP.
GetLineData() << std::endl);
703 DEBUGCOUT(
"Forgetting Factor:" << std::endl);
707 DEBUGCOUT(
"Persistent Excitation:" << std::endl);
719 std::vector<DriveCaller*> vDesiredOut;
721 DEBUGCOUT(
"Desired output:" << std::endl);
722 vDesiredOut.resize(iNumOutputs);
724 for (
integer i = 0; i < iNumOutputs; i++) {
725 DEBUGCOUT(
"output[" << i + 1 <<
"]:" << std::endl);
734 DEBUGCOUT(
"Identified matrices will be output in file \"" << s <<
"\"" << std::endl);
743 pFF, pCD, pPX, pTrig, vDesiredOut,
746 silent_cerr(
"DiscreteControl(" << uLabel <<
"): "
747 "GPC/deadbeat control is not available "
754 silent_cerr(
"Sorry, not implemented yed" << std::endl);
761 silent_cerr(
"DiscreteControl(" << uLabel <<
"): "
762 "\"outputs\" expected "
769 std::vector<ScalarValue *> vOutputs(iNumOutputs);
770 std::vector<DriveCaller *> vOutScaleFact(iNumOutputs);
773 for (
int i = 0; i < iNumOutputs; i++) {
779 vOutScaleFact[i] = 0;
785 silent_cerr(
"DiscreteControl(" << uLabel <<
"): "
786 "\"inputs\" expected "
797 for (
int i = 0; i < iNumInputs; i++) {
805 iNumOutputs, vOutputs, vOutScaleFact,
807 pDCP, iNumIter, fOut));
813 silent_cerr(
"Electric(" << uLabel <<
"): "
822 silent_cerr(
"semicolon expected at line " << HP.
GetLineData() << std::endl);
flag fReadOutput(MBDynParser &HP, const T &t) const
Mat3x3 GetRotRel(const ReferenceFrame &rf)
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
#define MBDYN_EXCEPT_ARGS
ForgettingFactor * ReadFF(MBDynParser &HP, integer iNumOutputs)
#define DEBUGCOUTFNAME(fname)
virtual integer GetInt(integer iDefval=0)
doublereal Norm(void) const
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
virtual const char * GetFileName(enum Delims Del=DEFAULTDELIM)
Vec3 GetCol(unsigned short int i) const
const Mat3x3 Zero3x3(0., 0., 0., 0., 0., 0., 0., 0., 0.)
#define SAFENEW(pnt, item)
Vec3 GetPosRel(const ReferenceFrame &rf)
virtual bool IsKeyWord(const char *sKeyWord)
ScalarValue * ReadScalarValue(DataManager *pDM, MBDynParser &HP)
PersistentExcitation * ReadPX(DataManager *pDM, MBDynParser &HP, integer iNumInputs)
#define ASSERT(expression)
GradientExpression< UnaryExpr< FuncSqrt, Expr > > sqrt(const GradientExpression< Expr > &u)
#define SAFENEWWITHCONSTRUCTOR(pnt, item, constructor)
#define SAFENEWARRNOFILL(pnt, item, sz)
virtual int GetWord(void)
#define SAFENEWARR(pnt, item, sz)
DriveCaller * GetDriveCaller(bool bDeferred=false)
Vec3 GetUnitVecRel(const ReferenceFrame &rf)
virtual HighParser::ErrOut GetLineData(void) const
Node * ReadNode(MBDynParser &HP, Node::Type type) const
ScalarDof ReadScalarDof(const DataManager *pDM, MBDynParser &HP, bool bDof, bool bOrder)
bool IsSame(const Mat3x3 &m, const doublereal &dTol) const
virtual doublereal GetReal(const doublereal &dDefval=0.0)