YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ControlBoardHelper.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
7#include <yarp/dev/PidEnums.h>
8#include <cstdio> // for printf
9#include <cmath> //fabs
10#include <yarp/os/Log.h>
11#include <yarp/os/LogStream.h>
12#include <map>
13
14using namespace yarp::dev;
15
16namespace {
17YARP_LOG_COMPONENT(CB_HELPER, "yarp.ControlBoardHelper")
18}
19
21{
25 fbk_units = PidFeedbackUnitsEnum::RAW_MACHINE_UNITS;
26 out_units = PidOutputUnitsEnum::RAW_MACHINE_UNITS;
27 }
28};
29
31{
32public:
33 int nj;
34 int *axisMap;
36 bool verbose;
37
39 double *helper_ones;
45 double *bemfToRaws;
46 double *ktauToRaws;
52
60 std::map<PidControlTypeEnum, PidUnits*> pid_units;
61
62 explicit PrivateUnitsHandler(int size) :
63 axisMap(nullptr),
64 invAxisMap(nullptr),
65 verbose(true),
66 position_zeros(nullptr),
67 helper_ones(nullptr),
68 angleToEncoders(nullptr),
69 newtonsToSensors(nullptr),
70 ampereToSensors(nullptr),
71 voltToSensors(nullptr),
72 dutycycleToPWMs(nullptr),
73 bemfToRaws(nullptr),
74 ktauToRaws(nullptr),
75 viscousPosToRaws(nullptr),
76 viscousNegToRaws(nullptr),
77 coulombPosToRaws(nullptr),
78 coulombNegToRaws(nullptr),
79 velocityThresToRaws(nullptr)
80 {
81 alloc(size);
82 std::fill_n(helper_ones, size, 1.0);
83 std::fill_n(position_zeros, size, 0.0);
84 std::fill_n(angleToEncoders, size, 1.0);
85 std::fill_n(newtonsToSensors, size, 1.0);
86 std::fill_n(ampereToSensors, size, 1.0);
87 std::fill_n(voltToSensors, size, 1.0);
88 std::fill_n(dutycycleToPWMs, size, 1.0);
89 std::fill_n(bemfToRaws, size, 1.0);
90 std::fill_n(ktauToRaws, size, 1.0);
91 std::fill_n(viscousPosToRaws, size, 1.0);
92 std::fill_n(viscousNegToRaws, size, 1.0);
93 std::fill_n(coulombPosToRaws, size, 1.0);
94 std::fill_n(coulombNegToRaws, size, 1.0);
95 std::fill_n(velocityThresToRaws, size, 1.0);
96 }
97
99 {
107 checkAndDestroy<double>(position_zeros);
108 checkAndDestroy<double>(helper_ones);
109 checkAndDestroy<int>(axisMap);
110 checkAndDestroy<int>(invAxisMap);
111 checkAndDestroy<double>(angleToEncoders);
112 checkAndDestroy<double>(newtonsToSensors);
113 checkAndDestroy<double>(ampereToSensors);
114 checkAndDestroy<double>(voltToSensors);
115 checkAndDestroy<double>(dutycycleToPWMs);
116 checkAndDestroy<double>(bemfToRaws);
117 checkAndDestroy<double>(ktauToRaws);
118 checkAndDestroy<double>(viscousPosToRaws);
119 checkAndDestroy<double>(viscousNegToRaws);
120 checkAndDestroy<double>(coulombPosToRaws);
121 checkAndDestroy<double>(coulombNegToRaws);
122 checkAndDestroy<double>(velocityThresToRaws);
123 }
124
125 void alloc(int n)
126 {
127 nj = n;
128 position_zeros = new double[nj];
129 helper_ones = new double[nj];
130 axisMap = new int[nj];
131 invAxisMap = new int[nj];
132 angleToEncoders = new double[nj];
133 newtonsToSensors = new double[nj];
134 ampereToSensors = new double[nj];
135 voltToSensors = new double[nj];
136 dutycycleToPWMs = new double[nj];
137 PosPid_units = new PidUnits[nj];
138 VelPid_units = new PidUnits[nj];
139 TrqPid_units = new PidUnits[nj];
140 CurPid_units = new PidUnits[nj];
143 MixPid_units = new PidUnits[nj];
144 bemfToRaws = new double[nj];
145 ktauToRaws = new double[nj];
146 viscousPosToRaws = new double[nj];
147 viscousNegToRaws = new double[nj];
148 coulombPosToRaws = new double[nj];
149 coulombNegToRaws = new double[nj];
150 velocityThresToRaws = new double[nj];
151
152 yAssert(position_zeros != nullptr);
153 yAssert(helper_ones != nullptr);
154 yAssert(axisMap != nullptr);
155 yAssert(invAxisMap != nullptr);
156 yAssert(angleToEncoders != nullptr);
157 yAssert(newtonsToSensors != nullptr);
158 yAssert(ampereToSensors != nullptr);
159 yAssert(voltToSensors != nullptr);
160 yAssert(dutycycleToPWMs != nullptr);
161 yAssert(PosPid_units != nullptr);
162 yAssert(VelPid_units != nullptr);
163 yAssert(TrqPid_units != nullptr);
164 yAssert(CurPid_units != nullptr);
165 yAssert(PosDirPid_units != nullptr);
166 yAssert(VelDirPid_units != nullptr);
167 yAssert(MixPid_units != nullptr);
168 yAssert(bemfToRaws != nullptr);
169 yAssert(ktauToRaws != nullptr);
170 yAssert(viscousPosToRaws != nullptr);
171 yAssert(viscousNegToRaws != nullptr);
172 yAssert(coulombPosToRaws != nullptr);
173 yAssert(coulombNegToRaws != nullptr);
174 yAssert(velocityThresToRaws != nullptr);
175
197 }
198
200 {
201 alloc(other.nj);
202 memcpy(this->position_zeros, other.position_zeros, sizeof(*other.position_zeros)*nj);
203 memcpy(this->helper_ones, other.helper_ones, sizeof(*other.helper_ones)*nj);
204 memcpy(this->axisMap, other.axisMap, sizeof(*other.axisMap)*nj);
205 memcpy(this->invAxisMap, other.invAxisMap, sizeof(*other.invAxisMap)*nj);
206 memcpy(this->angleToEncoders, other.angleToEncoders, sizeof(*other.angleToEncoders)*nj);
207 memcpy(this->newtonsToSensors, other.newtonsToSensors, sizeof(*other.newtonsToSensors)*nj);
208 memcpy(this->ampereToSensors, other.ampereToSensors, sizeof(*other.ampereToSensors)*nj);
209 memcpy(this->voltToSensors, other.voltToSensors, sizeof(*other.voltToSensors)*nj);
210 memcpy(this->dutycycleToPWMs, other.dutycycleToPWMs, sizeof(*other.dutycycleToPWMs)*nj);
211 memcpy(this->PosPid_units, other.PosPid_units, sizeof(*other.PosPid_units)*nj);
212 memcpy(this->VelPid_units, other.VelPid_units, sizeof(*other.VelPid_units)*nj);
213 memcpy(this->TrqPid_units, other.TrqPid_units, sizeof(*other.TrqPid_units)*nj);
214 memcpy(this->CurPid_units, other.CurPid_units, sizeof(*other.CurPid_units)*nj);
215 memcpy(this->PosDirPid_units, other.PosDirPid_units, sizeof(*other.PosDirPid_units)*nj);
216 memcpy(this->VelDirPid_units, other.VelDirPid_units, sizeof(*other.VelDirPid_units)*nj);
217 memcpy(this->MixPid_units, other.MixPid_units, sizeof(*other.MixPid_units)*nj);
218 memcpy(this->bemfToRaws, other.bemfToRaws, sizeof(*other.bemfToRaws)*nj);
219 memcpy(this->ktauToRaws, other.ktauToRaws, sizeof(*other.ktauToRaws)*nj);
220 memcpy(this->viscousPosToRaws, other.viscousPosToRaws, sizeof(*other.viscousPosToRaws)*nj);
221 memcpy(this->viscousNegToRaws, other.viscousNegToRaws, sizeof(*other.viscousNegToRaws)*nj);
222 memcpy(this->coulombPosToRaws, other.coulombPosToRaws, sizeof(*other.coulombPosToRaws)*nj);
223 memcpy(this->coulombNegToRaws, other.coulombNegToRaws, sizeof(*other.coulombNegToRaws)*nj);
224 memcpy(this->velocityThresToRaws, other.velocityThresToRaws, sizeof(*other.velocityThresToRaws)*nj);
225 }
226};
227
228ControlBoardHelper::ControlBoardHelper(int n, const int *aMap, const double *angToEncs, const double *zs, const double *newtons, const double *amps, const double *volts, const double *dutycycles, const double *kbemf, const double *ktau)
229{
230 yAssert(n>=0); // if number of joints is negative complain!
231 yAssert(aMap!=nullptr); // at least the axisMap is required
232 mPriv = new PrivateUnitsHandler(n);
233
234 memcpy(mPriv->axisMap, aMap, sizeof(int)*n);
235
236 if (zs != nullptr) {
237 memcpy(mPriv->position_zeros, zs, sizeof(double) * n);
238 }
239 if (angToEncs != nullptr) {
240 memcpy(mPriv->angleToEncoders, angToEncs, sizeof(double) * n);
241 }
242 if (newtons != nullptr) {
243 memcpy(mPriv->newtonsToSensors, newtons, sizeof(double) * n);
244 }
245 if (amps != nullptr) {
246 memcpy(mPriv->ampereToSensors, amps, sizeof(double) * n);
247 }
248 if (volts != nullptr) {
249 memcpy(mPriv->voltToSensors, volts, sizeof(double) * n);
250 }
251 if (dutycycles != nullptr) {
252 memcpy(mPriv->dutycycleToPWMs, dutycycles, sizeof(double) * n);
253 }
254 if (kbemf != nullptr) {
255 memcpy(mPriv->bemfToRaws, kbemf, sizeof(double) * n);
256 }
257 if (ktau != nullptr) {
258 memcpy(mPriv->ktauToRaws, ktau, sizeof(double) * n);
259 }
260
261 // invert the axis map
262 memset (mPriv->invAxisMap, 0, sizeof(int) * n);
263 int i;
264 for (i = 0; i < n; i++)
265 {
266 int j;
267 for (j = 0; j < n; j++)
268 {
269 if (mPriv->axisMap[j] == i)
270 {
271 mPriv->invAxisMap[i] = j;
272 break;
273 }
274 }
275 }
276}
277
279{
280 if (mPriv) { delete mPriv; mPriv = nullptr; }
281}
282
287
289{
290 mPriv = new PrivateUnitsHandler(*other.mPriv);
291 return *this;
292}
293
295{
296 if( (id >= mPriv->nj) || (id< 0))
297 {
298 return false;
299 }
300 return true;
301}
302
303bool ControlBoardHelper::checkAxesIds(const std::vector<int>& axesList)
304{
305 return checkAxesIds(axesList.size(), axesList.data());
306}
307
308bool ControlBoardHelper::checkAxesIds(const int n_axes, const int* axesList)
309{
310 if(n_axes > mPriv->nj)
311 {
312 if (mPriv->verbose) {
313 yCError(CB_HELPER,"checkAxesIds: num of axes is too big");
314 }
315 return false;
316 }
317 if(axesList==nullptr)
318 {
319 if (mPriv->verbose) {
320 yCError(CB_HELPER, "checkAxesIds: axesList is nullptr");
321 }
322 return false;
323 }
324 for(int idx = 0; idx<n_axes; idx++)
325 {
326 if( (axesList[idx]<0) || (axesList[idx]>= mPriv->nj) )
327 {
328 if (mPriv->verbose) {
329 yCError(CB_HELPER, "checkAxesIds: joint id out of bound");
330 }
331
332 return false;
333 }
334 }
335 return true;
336}
337
339{ return mPriv->axisMap[axis]; }
340
342{ return mPriv->invAxisMap[axis]; }
343
344//map a vector, no conversion
345 void ControlBoardHelper::toUser(const double *hwData, double *user)
346{
347 for (int k = 0; k < mPriv->nj; k++) {
348 user[toUser(k)] = hwData[k];
349 }
350}
351
352//map a vector, no conversion
353void ControlBoardHelper::ControlBoardHelper::toUser(const int *hwData, int *user)
354{
355 for (int k = 0; k < mPriv->nj; k++) {
356 user[toUser(k)] = hwData[k];
357 }
358}
359
360//map a vector, no conversion
361 void ControlBoardHelper::toHw(const double *usr, double *hwData)
362{
363 for (int k = 0; k < mPriv->nj; k++) {
364 hwData[toHw(k)] = usr[k];
365 }
366}
367
368//map a vector, no conversion
369void ControlBoardHelper::toHw(const int *usr, int *hwData)
370{
371 for (int k = 0; k < mPriv->nj; k++) {
372 hwData[toHw(k)] = usr[k];
373 }
374}
375
376void ControlBoardHelper::posA2E(double ang, int j, double &enc, int &k)
377{
378 enc=(ang+ mPriv->position_zeros[j])*mPriv->angleToEncoders[j];
379 k=toHw(j);
380}
381
382double ControlBoardHelper::posA2E(double ang, int j)
383{
384 return (ang+ mPriv->position_zeros[j])*mPriv->angleToEncoders[j];
385}
386
387void ControlBoardHelper::posE2A(double enc, int j, double &ang, int &k)
388{
389 k=toUser(j);
390
391 ang=(enc/ mPriv->angleToEncoders[k])- mPriv->position_zeros[k];
392}
393
394double ControlBoardHelper::posE2A(double enc, int j)
395{
396 int k=toUser(j);
397
398 return (enc/ mPriv->angleToEncoders[k])- mPriv->position_zeros[k];
399}
400
401void ControlBoardHelper::impN2S(double newtons, int j, double &sens, int &k)
402{
403 sens=newtons* mPriv->newtonsToSensors[j]/ mPriv->angleToEncoders[j];
404 k=toHw(j);
405}
406
407double ControlBoardHelper::impN2S(double newtons, int j)
408{
409 return newtons* mPriv->newtonsToSensors[j]/ mPriv->angleToEncoders[j];
410}
411
412void ControlBoardHelper::impN2S(const double *newtons, double *sens)
413{
414 double tmp;
415 int index;
416 for(int j=0;j<mPriv->nj;j++)
417 {
418 impN2S(newtons[j], j, tmp, index);
419 sens[index]=tmp;
420 }
421}
422
423void ControlBoardHelper::trqN2S(double newtons, int j, double &sens, int &k)
424{
425 sens=newtons* mPriv->newtonsToSensors[j];
426 k=toHw(j);
427}
428
429double ControlBoardHelper::trqN2S(double newtons, int j)
430{
431 return newtons* mPriv->newtonsToSensors[j];
432}
433
434//map a vector, convert from newtons to sensors
435void ControlBoardHelper::trqN2S(const double *newtons, double *sens)
436{
437 double tmp;
438 int index;
439 for(int j=0;j<mPriv->nj;j++)
440 {
441 trqN2S(newtons[j], j, tmp, index);
442 sens[index]=tmp;
443 }
444}
445
446//map a vector, convert from sensor to newtons
447void ControlBoardHelper::trqS2N(const double *sens, double *newtons)
448{
449 double tmp;
450 int index;
451 for(int j=0;j<mPriv->nj;j++)
452 {
453 trqS2N(sens[j], j, tmp, index);
454 newtons[index]=tmp;
455 }
456}
457
458void ControlBoardHelper::trqS2N(double sens, int j, double &newton, int &k)
459{
460 k=toUser(j);
461 newton=(sens/ mPriv->newtonsToSensors[k]);
462}
463
464double ControlBoardHelper::trqS2N(double sens, int j)
465{
466 int k=toUser(j);
467 return (sens/ mPriv->newtonsToSensors[k]);
468}
469
470void ControlBoardHelper::impS2N(const double *sens, double *newtons)
471{
472 double tmp;
473 int index;
474 for(int j=0;j<mPriv->nj;j++)
475 {
476 impS2N(sens[j], j, tmp, index);
477 newtons[index]=tmp;
478 }
479}
480
481void ControlBoardHelper::impS2N(double sens, int j, double &newton, int &k)
482{
483 k=toUser(j);
484 newton=(sens/ mPriv->newtonsToSensors[k]* mPriv->angleToEncoders[k]);
485}
486
487double ControlBoardHelper::impS2N(double sens, int j)
488{
489 int k=toUser(j);
490
491 return (sens/ mPriv->newtonsToSensors[k]* mPriv->angleToEncoders[k]);
492}
493
494void ControlBoardHelper::velA2E(double ang, int j, double &enc, int &k)
495{
496 k=toHw(j);
497 enc=ang* mPriv->angleToEncoders[j];
498}
499
500double ControlBoardHelper::velA2E(double ang, int j)
501{
502 return ang* mPriv->angleToEncoders[j];
503}
504
505void ControlBoardHelper::velA2E_abs(double ang, int j, double &enc, int &k)
506{
507 k=toHw(j);
508 enc=ang*fabs(mPriv->angleToEncoders[j]);
509}
510
511void ControlBoardHelper::velE2A(double enc, int j, double &ang, int &k)
512{
513 k=toUser(j);
514 ang=enc/ mPriv->angleToEncoders[k];
515}
516
517void ControlBoardHelper::velE2A_abs(double enc, int j, double &ang, int &k)
518{
519 k=toUser(j);
520 ang=enc/fabs(mPriv->angleToEncoders[k]);
521}
522
523void ControlBoardHelper::accA2E(double ang, int j, double &enc, int &k)
524{
525 velA2E(ang, j, enc, k);
526}
527
528void ControlBoardHelper::accA2E_abs(double ang, int j, double &enc, int &k)
529{
530 velA2E_abs(ang, j, enc, k);
531}
532
533void ControlBoardHelper::accE2A(double enc, int j, double &ang, int &k)
534{
535 velE2A(enc, j, ang, k);
536}
537
538void ControlBoardHelper::accE2A_abs(double enc, int j, double &ang, int &k)
539{
540 velE2A_abs(enc, j, ang, k);
541}
542
543double ControlBoardHelper::velE2A(double enc, int j)
544{
545 int k=toUser(j);
546 return enc/ mPriv->angleToEncoders[k];
547}
548
549double ControlBoardHelper::velE2A_abs(double enc, int j)
550{
551 int k=toUser(j);
552 return enc/fabs(mPriv->angleToEncoders[k]);
553}
554
555
556double ControlBoardHelper::accE2A(double enc, int j)
557{
558 return velE2A(enc, j);
559}
560
561double ControlBoardHelper::accE2A_abs(double enc, int j)
562{
563 return velE2A_abs(enc, j);
564}
565
566//map a vector, convert from angles to encoders
567void ControlBoardHelper::posA2E(const double *ang, double *enc)
568{
569 double tmp;
570 int index;
571 for(int j=0;j<mPriv->nj;j++)
572 {
573 posA2E(ang[j], j, tmp, index);
574 enc[index]=tmp;
575 }
576}
577
578//map a vector, convert from encoders to angles
579void ControlBoardHelper::posE2A(const double *enc, double *ang)
580{
581 double tmp;
582 int index;
583 for(int j=0;j<mPriv->nj;j++)
584 {
585 posE2A(enc[j], j, tmp, index);
586 ang[index]=tmp;
587 }
588}
589
590void ControlBoardHelper::velA2E(const double *ang, double *enc)
591{
592 double tmp;
593 int index;
594 for(int j=0;j<mPriv->nj;j++)
595 {
596 velA2E(ang[j], j, tmp, index);
597 enc[index]=tmp;
598 }
599}
600
601void ControlBoardHelper::velA2E_abs(const double *ang, double *enc)
602{
603 double tmp;
604 int index;
605 for(int j=0;j<mPriv->nj;j++)
606 {
607 velA2E_abs(ang[j], j, tmp, index);
608 enc[index]=tmp;
609 }
610}
611
612void ControlBoardHelper::velE2A(const double *enc, double *ang)
613{
614 double tmp;
615 int index;
616 for(int j=0;j<mPriv->nj;j++)
617 {
618 velE2A(enc[j], j, tmp, index);
619 ang[index]=tmp;
620 }
621}
622
623void ControlBoardHelper::velE2A_abs(const double *enc, double *ang)
624{
625 double tmp;
626 int index;
627 for(int j=0;j<mPriv->nj;j++)
628 {
629 velE2A_abs(enc[j], j, tmp, index);
630 ang[index]=tmp;
631 }
632}
633
634void ControlBoardHelper::accA2E(const double *ang, double *enc)
635{
636 double tmp;
637 int index;
638 for(int j=0;j<mPriv->nj;j++)
639 {
640 accA2E(ang[j], j, tmp, index);
641 enc[index]=tmp;
642 }
643}
644
645void ControlBoardHelper::accA2E_abs(const double *ang, double *enc)
646{
647 double tmp;
648 int index;
649 for(int j=0;j<mPriv->nj;j++)
650 {
651 accA2E_abs(ang[j], j, tmp, index);
652 enc[index]=tmp;
653 }
654}
655
656void ControlBoardHelper::accE2A(const double *enc, double *ang)
657{
658 double tmp;
659 int index;
660 for(int j=0;j<mPriv->nj;j++)
661 {
662 accE2A(enc[j], j, tmp, index);
663 ang[index]=tmp;
664 }
665}
666
667void ControlBoardHelper::accE2A_abs(const double *enc, double *ang)
668{
669 double tmp;
670 int index;
671 for(int j=0;j<mPriv->nj;j++)
672 {
673 accE2A_abs(enc[j], j, tmp, index);
674 ang[index]=tmp;
675 }
676}
677
678//***************** current ******************//
679void ControlBoardHelper::ampereA2S(double ampere, int j, double &sens, int &k)
680{
681 sens=ampere* mPriv->ampereToSensors[j];
682 k=toHw(j);
683}
684
685double ControlBoardHelper::ampereA2S(double ampere, int j)
686{
687 return ampere* mPriv->ampereToSensors[j];
688}
689
690//map a vector, convert from ampere to sensors
691void ControlBoardHelper::ampereA2S(const double *ampere, double *sens)
692{
693 double tmp;
694 int index;
695 for(int j=0;j<mPriv->nj;j++)
696 {
697 ampereA2S(ampere[j], j, tmp, index);
698 sens[index]=tmp;
699 }
700}
701
702//map a vector, convert from sensor to ampere
703void ControlBoardHelper::ampereS2A(const double *sens, double *ampere)
704{
705 double tmp;
706 int index;
707 for(int j=0;j<mPriv->nj;j++)
708 {
709 ampereS2A(sens[j], j, tmp, index);
710 ampere[index]=tmp;
711 }
712}
713
714void ControlBoardHelper::ampereS2A(double sens, int j, double &ampere, int &k)
715{
716 k=toUser(j);
717 ampere=(sens/ mPriv->ampereToSensors[k]);
718}
719
720double ControlBoardHelper::ampereS2A(double sens, int j)
721{
722 int k=toUser(j);
723 return sens/ mPriv->ampereToSensors[k];
724}
725// *******************************************//
726
727//***************** voltage ******************//
728void ControlBoardHelper::voltageV2S(double voltage, int j, double &sens, int &k)
729{
730 sens=voltage* mPriv->voltToSensors[j];
731 k=toHw(j);
732}
733
734double ControlBoardHelper::voltageV2S(double voltage, int j)
735{
736 return voltage* mPriv->voltToSensors[j];
737}
738
739//map a vector, convert from voltage to sensors
740void ControlBoardHelper::voltageV2S(const double *voltage, double *sens)
741{
742 double tmp;
743 int index;
744 for(int j=0;j<mPriv->nj;j++)
745 {
746 voltageV2S(voltage[j], j, tmp, index);
747 sens[index]=tmp;
748 }
749}
750
751//map a vector, convert from sensor to newtons
752void ControlBoardHelper::voltageS2V(const double *sens, double *voltage)
753{
754 double tmp;
755 int index;
756 for(int j=0;j<mPriv->nj;j++)
757 {
758 voltageS2V(sens[j], j, tmp, index);
759 voltage[index]=tmp;
760 }
761}
762
763void ControlBoardHelper::voltageS2V(double sens, int j, double &voltage, int &k)
764{
765 k=toUser(j);
766 voltage=(sens/ mPriv->voltToSensors[k]);
767}
768
769double ControlBoardHelper::voltageS2V(double sens, int j)
770{
771 int k=toUser(j);
772 return (sens/ mPriv->voltToSensors[k]);
773}
774// *******************************************//
775
776//***************** dutycycle ******************//
777void ControlBoardHelper::dutycycle2PWM(double dutycycle, int j, double &pwm, int &k)
778{
779 pwm = dutycycle* mPriv->dutycycleToPWMs[j];
780 k = toHw(j);
781}
782
783double ControlBoardHelper::dutycycle2PWM(double dutycycle, int j)
784{
785 return dutycycle* mPriv->dutycycleToPWMs[j];
786}
787
788void ControlBoardHelper::dutycycle2PWM(const double *dutycycle, double *sens)
789{
790 double tmp;
791 int index;
792 for (int j = 0; j<mPriv->nj; j++)
793 {
794 dutycycle2PWM(dutycycle[j], j, tmp, index);
795 sens[index] = tmp;
796 }
797}
798
799void ControlBoardHelper::PWM2dutycycle(const double *pwm, double *dutycycle)
800{
801 double tmp;
802 int index;
803 for (int j = 0; j<mPriv->nj; j++)
804 {
805 PWM2dutycycle(pwm[j], j, tmp, index);
806 dutycycle[index] = tmp;
807 }
808}
809
810void ControlBoardHelper::PWM2dutycycle(double pwm_raw, int k_raw, double &dutycycle, int &j)
811{
812 j = toUser(k_raw);
813 dutycycle = (pwm_raw / mPriv->dutycycleToPWMs[j]);
814}
815
816double ControlBoardHelper::PWM2dutycycle(double pwm_raw, int k_raw)
817{
818 int j = toUser(k_raw);
819 return (pwm_raw / mPriv->dutycycleToPWMs[j]);
820}
821
822// *******************************************//
823void ControlBoardHelper::bemf_user2raw(double bemf_user, int j, double &bemf_raw, int &k)
824{
825 bemf_raw = bemf_user * mPriv->bemfToRaws[j];
826 k = toHw(j);
827}
828
829void ControlBoardHelper::ktau_user2raw(double ktau_user, int j, double &ktau_raw, int &k)
830{
831 ktau_raw = ktau_user * mPriv->ktauToRaws[j];
832 k = toHw(j);
833}
834
835void ControlBoardHelper::viscousPos_user2raw(double viscousPos_user, int j, double &viscousPos_raw, int &k)
836{
837 viscousPos_raw = viscousPos_user * mPriv->viscousPosToRaws[j];
838 k = toHw(j);
839}
840
841void ControlBoardHelper::viscousNeg_user2raw(double viscousNeg_user, int j, double &viscousNeg_raw, int &k)
842{
843 viscousNeg_raw = viscousNeg_user * mPriv->viscousNegToRaws[j];
844 k = toHw(j);
845}
846
847void ControlBoardHelper::coulombPos_user2raw(double coulombPos_user, int j, double &coulombPos_raw, int &k)
848{
849 coulombPos_raw = coulombPos_user * mPriv->coulombPosToRaws[j];
850 k = toHw(j);
851}
852
853void ControlBoardHelper::coulombNeg_user2raw(double coulombNeg_user, int j, double &coulombNeg_raw, int &k)
854{
855 coulombNeg_raw = coulombNeg_user * mPriv->coulombNegToRaws[j];
856 k = toHw(j);
857}
858
859void ControlBoardHelper::velocityThres_user2raw(double velocityThres_user, int j, double &velocityThres_raw, int &k)
860{
861 velocityThres_raw = velocityThres_user * mPriv->velocityThresToRaws[j];
862 k = toHw(j);
863}
864
865void ControlBoardHelper::bemf_raw2user(double bemf_raw, int k_raw, double &bemf_user, int &j_user)
866{
867 j_user = toUser(k_raw);
868 bemf_user = bemf_raw / mPriv->bemfToRaws[j_user];
869}
870
871void ControlBoardHelper::ktau_raw2user(double ktau_raw, int k_raw, double &ktau_user, int &j_user)
872{
873 j_user = toUser(k_raw);
874 ktau_user = ktau_raw / mPriv->ktauToRaws[j_user];
875}
876
877double ControlBoardHelper::bemf_user2raw(double bemf_user, int j)
878{
879 return bemf_user * mPriv->bemfToRaws[j];
880}
881
882double ControlBoardHelper::ktau_user2raw(double ktau_user, int j)
883{
884 return ktau_user * mPriv->ktauToRaws[j];
885}
886
887double ControlBoardHelper::viscousPos_user2raw(double viscousPos_user, int j)
888{
889 return viscousPos_user * mPriv->viscousPosToRaws[j];
890}
891
892double ControlBoardHelper::viscousNeg_user2raw(double viscousNeg_user, int j)
893{
894 return viscousNeg_user * mPriv->viscousNegToRaws[j];
895}
896
897double ControlBoardHelper::coulombPos_user2raw(double coulombPos_user, int j)
898{
899 return coulombPos_user * mPriv->coulombPosToRaws[j];
900}
901
902double ControlBoardHelper::coulombNeg_user2raw(double coulombNeg_user, int j)
903{
904 return coulombNeg_user * mPriv->coulombNegToRaws[j];
905}
906
907double ControlBoardHelper::velocityThres_user2raw(double velocityThres_user, int j)
908{
909 return velocityThres_user * mPriv->velocityThresToRaws[j];
910}
911
912void ControlBoardHelper::viscousPos_raw2user(double viscousPos_raw, int k_raw, double &viscousPos_user, int &j_user)
913{
914 j_user = toUser(k_raw);
915 viscousPos_user = viscousPos_raw / mPriv->viscousPosToRaws[j_user];
916}
917
918void ControlBoardHelper::viscousNeg_raw2user(double viscousNeg_raw, int k_raw, double &viscousNeg_user, int &j_user)
919{
920 j_user = toUser(k_raw);
921 viscousNeg_user = viscousNeg_raw / mPriv->viscousNegToRaws[j_user];
922}
923
924void ControlBoardHelper::coulombPos_raw2user(double coulombPos_raw, int k_raw, double &coulombPos_user, int &j_user)
925{
926 j_user = toUser(k_raw);
927 coulombPos_user = coulombPos_raw / mPriv->coulombPosToRaws[j_user];
928}
929
930void ControlBoardHelper::coulombNeg_raw2user(double coulombNeg_raw, int k_raw, double &coulombNeg_user, int &j_user)
931{
932 j_user = toUser(k_raw);
933 coulombNeg_user = coulombNeg_raw / mPriv->coulombNegToRaws[j_user];
934}
935
936void ControlBoardHelper::velocityThres_raw2user(double velocityThres_raw, int k_raw, double &velocityThres_user, int &j_user)
937{
938 j_user = toUser(k_raw);
939 velocityThres_user = velocityThres_raw / mPriv->velocityThresToRaws[j_user];
940}
941
942
943// *******************************************//
944
946{ return mPriv->nj; }
947
948// *******************************************//
949
951{
952 double output_conversion_factor;
953 switch ((mPriv->pid_units[pidtype])[j].out_units)
954 {
955 case PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT: output_conversion_factor = mPriv->dutycycleToPWMs[j]; break;
956 case PidOutputUnitsEnum::CURRENT_METRIC: output_conversion_factor = mPriv->ampereToSensors[j]; break;
957 case PidOutputUnitsEnum::POSITION_METRIC: output_conversion_factor = mPriv->angleToEncoders[j]; break;
958 case PidOutputUnitsEnum::VELOCITY_METRIC: output_conversion_factor = mPriv->angleToEncoders[j]; break;
959 case PidOutputUnitsEnum::TORQUE_METRIC: output_conversion_factor = mPriv->newtonsToSensors[j]; break;
961 default: output_conversion_factor = mPriv->helper_ones[j]; break;
962 }
963 return output_conversion_factor;
964}
965
967{
968 double feedback_conversion_factor = 0.0;
969 switch (( mPriv->pid_units[pidtype])[j].fbk_units)
970 {
972 switch (pidtype)
973 {
989 feedback_conversion_factor = mPriv->angleToEncoders[j];
990 break;
994 feedback_conversion_factor = mPriv->newtonsToSensors[j];
995 break;
999 feedback_conversion_factor = mPriv->ampereToSensors[j];
1000 break;
1001 }
1002 break;
1004 default: feedback_conversion_factor = mPriv->helper_ones[j]; break;
1005 }
1006 return (1.0 / feedback_conversion_factor);
1007}
1008void ControlBoardHelper::convert_pid_to_user(const yarp::dev::PidControlTypeEnum& pidtype, const Pid &in_raw, int j_raw, Pid &out_usr, int &k_usr)
1009{
1010 ControlBoardHelper* cb_helper = this;
1011 k_usr = cb_helper->toUser(j_raw);
1012 out_usr = in_raw;
1013
1014 //feedback is at the denominator, i.e. kp=[PWM/deg]
1015 double feedback_conversion_units_user2raw = get_pidfeedback_conversion_factor_user2raw(pidtype, k_usr);
1016 out_usr.kp = out_usr.kp / feedback_conversion_units_user2raw;
1017 out_usr.ki = out_usr.ki / feedback_conversion_units_user2raw;
1018 out_usr.kd = out_usr.kd / feedback_conversion_units_user2raw;
1019
1020 //output is at the numerator, i.e. kp=[PWM/deg]
1021 double output_conversion_units_user2raw = get_pidoutput_conversion_factor_user2raw(pidtype, k_usr);
1022 out_usr.kp = out_usr.kp / output_conversion_units_user2raw;
1023 out_usr.ki = out_usr.ki / output_conversion_units_user2raw;
1024 out_usr.kd = out_usr.kd / output_conversion_units_user2raw;
1025 out_usr.max_output = out_usr.max_output / output_conversion_units_user2raw;
1026 out_usr.max_int = out_usr.max_int / output_conversion_units_user2raw;
1027 out_usr.stiction_up_val = out_usr.stiction_up_val / output_conversion_units_user2raw;
1028 out_usr.stiction_down_val = out_usr.stiction_down_val / output_conversion_units_user2raw;
1029 out_usr.offset = out_usr.offset / output_conversion_units_user2raw;
1030}
1031
1033{
1034 Pid tmp;
1035 int tmp_k;
1036 convert_pid_to_machine(pidtype, in_usr, j_usr, tmp, tmp_k);
1037 return tmp;
1038}
1039
1040void ControlBoardHelper::convert_pid_to_machine(const yarp::dev::PidControlTypeEnum& pidtype, const Pid &in_usr, int j_usr, Pid &out_raw, int &k_raw)
1041{
1042 ControlBoardHelper* cb_helper = this;
1043 k_raw = cb_helper->toHw(j_usr);
1044 out_raw = in_usr;
1045
1046 //feedback is at the denominator, i.e. kp=[PWM/deg]
1047 double feedback_conversion_units_user2raw = get_pidfeedback_conversion_factor_user2raw(pidtype, j_usr);
1048 out_raw.kp = out_raw.kp * feedback_conversion_units_user2raw;
1049 out_raw.ki = out_raw.ki * feedback_conversion_units_user2raw;
1050 out_raw.kd = out_raw.kd * feedback_conversion_units_user2raw;
1051
1052 //output is at the numerator, i.e. kp=[PWM/deg]
1053 double output_conversion_units_user2raw = get_pidoutput_conversion_factor_user2raw(pidtype, j_usr);
1054 out_raw.kp = out_raw.kp * output_conversion_units_user2raw;
1055 out_raw.ki = out_raw.ki * output_conversion_units_user2raw;
1056 out_raw.kd = out_raw.kd * output_conversion_units_user2raw;
1057 out_raw.max_output = out_raw.max_output * output_conversion_units_user2raw;
1058 out_raw.max_int = out_raw.max_int * output_conversion_units_user2raw;
1059 out_raw.stiction_up_val = out_raw.stiction_up_val * output_conversion_units_user2raw;
1060 out_raw.stiction_down_val = out_raw.stiction_down_val * output_conversion_units_user2raw;
1061 out_raw.offset = out_raw.offset * output_conversion_units_user2raw;
1062}
1063
1064void ControlBoardHelper::convert_pidunits_to_machine(const yarp::dev::PidControlTypeEnum& pidtype, double userval, int j, double &machineval, int &k)
1065{
1066 ControlBoardHelper* cb_helper = this;
1067 switch (pidtype)
1068 {
1072 //Beware:posA2E employs zeros, not only angleToEncoders
1073 //This is fine if convert_pidunits_to_machine() is called by methods that are aware of this feature, that is intentional.
1074 cb_helper->posA2E(userval, j, machineval, k);
1075 break;
1079 cb_helper->velA2E(userval, j, machineval, k);
1080 break;
1084 cb_helper->trqN2S(userval, j, machineval, k);
1085 break;
1089 cb_helper->ampereA2S(userval, j, machineval, k);
1090 break;
1091 default:
1092 yError() << "convert_units_to_machine: invalid pidtype";
1093 break;
1094 }
1095}
1096
1097void ControlBoardHelper::convert_pidunits_to_machine(const yarp::dev::PidControlTypeEnum& pidtype, const double* userval, double* machineval)
1098{
1099 ControlBoardHelper* cb_helper = this;
1100 switch (pidtype)
1101 {
1105 //Beware:posA2E employs zeros, not only angleToEncoders
1106 //This is fine if convert_pidunits_to_machine() is called by methods that are aware of this feature, that is intentional.
1107 cb_helper->posA2E(userval, machineval);
1108 break;
1112 cb_helper->velA2E(userval, machineval);
1113 break;
1117 cb_helper->trqN2S(userval, machineval);
1118 break;
1122 cb_helper->ampereA2S(userval, machineval);
1123 break;
1124 default:
1125 yError() << "convert_units_to_machine: invalid pidtype";
1126 break;
1127 }
1128}
1129
1130void ControlBoardHelper::convert_pidunits_to_user(const yarp::dev::PidControlTypeEnum& pidtype, const double machineval, double* userval, int k)
1131{
1132 ControlBoardHelper* cb_helper = this;
1133 switch (pidtype)
1134 {
1138 //Beware:posE2A employs zeros, not only angleToEncoders.
1139 //This is fine if convert_pidunits_to_user() is called by methods that are aware of this feature, that is intentional.
1140 *userval = cb_helper->posE2A(machineval, k);
1141 break;
1145 *userval = cb_helper->velE2A(machineval, k);
1146 break;
1150 *userval = cb_helper->trqS2N(machineval, k);
1151 break;
1155 *userval = cb_helper->ampereS2A(machineval, k);
1156 break;
1157 default:
1158 yError() << "convert_units_to_machine: invalid pidtype";
1159 break;
1160 }
1161}
1162
1163void ControlBoardHelper::convert_pidunits_to_user(const yarp::dev::PidControlTypeEnum& pidtype, const double* machineval, double* userval)
1164{
1165 ControlBoardHelper* cb_helper = this;
1166 switch (pidtype)
1167 {
1171 //Beware:posE2A employs zeros, not only angleToEncoders.
1172 //This is fine if convert_pidunits_to_user() is called by methods that are aware of this feature, that is intentional.
1173 cb_helper->posE2A(machineval, userval);
1174 break;
1178 cb_helper->velE2A(machineval, userval);
1179 break;
1183 cb_helper->trqS2N(machineval, userval);
1184 break;
1188 cb_helper->ampereS2A(machineval, userval);
1189 break;
1190 default:
1191 yError() << "convert_units_to_machine: invalid pidtype";
1192 break;
1193 }
1194}
1195
1197{
1198 ControlBoardHelper* cb_helper = this;
1199 int nj = cb_helper->axes();
1200 for (int i = 0; i < nj; i++)
1201 {
1202 mPriv->pid_units[pidtype][i].fbk_units = fbk_conv_units;
1203 mPriv->pid_units[pidtype][i].out_units = out_conv_units;
1204 }
1205}
void checkAndDestroy(T *&p)
#define yError(...)
Definition Log.h:361
#define yAssert(x)
Definition Log.h:388
std::map< PidControlTypeEnum, PidUnits * > pid_units
double get_pidfeedback_conversion_factor_user2raw(const yarp::dev::PidControlTypeEnum &pidtype, int j)
void accE2A(double enc, int j, double &ang, int &k)
void convert_pid_to_user(const yarp::dev::PidControlTypeEnum &pidtype, const Pid &in_raw, int j_raw, Pid &out_usr, int &k_usr)
void viscousNeg_raw2user(double viscousNeg_raw, int k_raw, double &viscousNeg_user, int &j_user)
double viscousNeg_user2raw(double viscousNeg_user, int j)
void ktau_raw2user(double ktau_raw, int k_raw, double &ktau_user, int &j_user)
void trqN2S(double newtons, int j, double &sens, int &k)
void velE2A_abs(double enc, int j, double &ang, int &k)
void convert_pidunits_to_machine(const yarp::dev::PidControlTypeEnum &pidtype, double userval, int j, double &machineval, int &k)
void bemf_raw2user(double bemf_raw, int k_raw, double &bemf_user, int &j_user)
void coulombPos_raw2user(double coulombPos_raw, int k_raw, double &coulombPos_user, int &j_user)
void voltageS2V(const double *sens, double *voltage)
void convert_pid_to_machine(const yarp::dev::PidControlTypeEnum &pidtype, const Pid &in_usr, int j_usr, Pid &out_raw, int &k_raw)
void ampereA2S(double ampere, int j, double &sens, int &k)
void impN2S(double newtons, int j, double &sens, int &k)
double ktau_user2raw(double ktau_user, int j)
double coulombPos_user2raw(double coulombPos_user, int j)
void velE2A(double enc, int j, double &ang, int &k)
double coulombNeg_user2raw(double coulombNeg_user, int j)
double velocityThres_user2raw(double velocityThres_user, int j)
void velocityThres_raw2user(double threshold_raw, int k_raw, double &velocityThres_user, int &j_user)
void velA2E(double ang, int j, double &enc, int &k)
void dutycycle2PWM(double dutycycle, int j, double &pwm, int &k)
void ampereS2A(const double *sens, double *ampere)
double viscousPos_user2raw(double viscousPos_user, int j)
void posE2A(double enc, int j, double &ang, int &k)
void coulombNeg_raw2user(double coulombNeg_raw, int k_raw, double &coulombNeg_user, int &j_user)
void trqS2N(const double *sens, double *newtons)
double get_pidoutput_conversion_factor_user2raw(const yarp::dev::PidControlTypeEnum &pidtype, int j)
void set_pid_conversion_units(const PidControlTypeEnum &pidtype, const PidFeedbackUnitsEnum fbk_conv_units, const PidOutputUnitsEnum out_conv_units)
void voltageV2S(double voltage, int j, double &sens, int &k)
ControlBoardHelper & operator=(const ControlBoardHelper &other)
double bemf_user2raw(double bemf_user, int j)
void accA2E_abs(double ang, int j, double &enc, int &k)
void accA2E(double ang, int j, double &enc, int &k)
void accE2A_abs(double enc, int j, double &ang, int &k)
void velA2E_abs(double ang, int j, double &enc, int &k)
ControlBoardHelper(int n, const int *aMap, const double *angToEncs=nullptr, const double *zs=nullptr, const double *newtons=nullptr, const double *amps=nullptr, const double *volts=nullptr, const double *dutycycles=nullptr, const double *kbemf=nullptr, const double *ktau=nullptr)
void viscousPos_raw2user(double viscousPos_raw, int k_raw, double &viscousPos_user, int &j_user)
void convert_pidunits_to_user(const yarp::dev::PidControlTypeEnum &pidtype, const double machineval, double *userval, int k)
void impS2N(const double *sens, double *newtons)
void PWM2dutycycle(const double *pwm, double *dutycycle)
bool checkAxesIds(const int n_axes, const int *axesList)
void posA2E(double ang, int j, double &enc, int &k)
double ki
integrative gain
double kp
proportional gain
double max_output
max output
double stiction_down_val
down stiction offset added to the pid output
double stiction_up_val
up stiction offset added to the pid output
double kd
derivative gain
double offset
pwm offset added to the pid output
double max_int
saturation threshold for the integrator
Contains the parameters for a PID.
#define yCError(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
PidOutputUnitsEnum
Definition PidEnums.h:64
PidControlTypeEnum
Definition PidEnums.h:15
PidFeedbackUnitsEnum
Definition PidEnums.h:58
PidOutputUnitsEnum out_units
PidFeedbackUnitsEnum fbk_units