YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ControlBoardRemapperHelpers.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
8
9#include <yarp/os/LogStream.h>
10
11
12using namespace yarp::os;
13using namespace yarp::dev;
14using namespace yarp::sig;
15
16
18{
19 id = "";
20 attachedF=false;
21 _subDevVerbose = false;
22}
23
24
26{
27 subdevice=nullptr;
28
29 pid=nullptr;
30 pos=nullptr;
31 posDir=nullptr;
32 vel=nullptr;
33 amp = nullptr;
34 iJntEnc=nullptr;
35 iMotEnc=nullptr;
36 lim=nullptr;
37 calib=nullptr;
38 info=nullptr;
39 iTorque=nullptr;
40 iImpedance=nullptr;
41 iMode=nullptr;
42 iTimed=nullptr;
43 iInteract=nullptr;
44 imotor=nullptr;
45 iVar = nullptr;
46 iPwm = nullptr;
47 iCurr = nullptr;
48 iBrake = nullptr;
49 iVelDir = nullptr;
50
51 attachedF=false;
52}
53
55{
56 if (id!=k)
57 {
58 yCError(CONTROLBOARDREMAPPER) << "Wrong device" << k.c_str();
59 return false;
60 }
61
62 if (d==nullptr)
63 {
64 yCError(CONTROLBOARDREMAPPER) << "Invalid device (null pointer)";
65 return false;
66 }
67
68 subdevice=d;
69
70 if (subdevice->isValid())
71 {
95 }
96 else
97 {
98 yCError(CONTROLBOARDREMAPPER) << "Invalid device" << k << "(isValid() returned false)";
99 return false;
100 }
101
102 if ((iTorque==nullptr) && (_subDevVerbose))
103 {
104 yCWarning(CONTROLBOARDREMAPPER) << "iTorque not valid interface";
105 }
106
107 if ((iImpedance==nullptr) && (_subDevVerbose))
108 {
109 yCWarning(CONTROLBOARDREMAPPER) << "iImpedance not valid interface";
110 }
111
112 if ((iInteract==nullptr) && (_subDevVerbose))
113 {
114 yCWarning(CONTROLBOARDREMAPPER) << "iInteractionMode not valid interface";
115 }
116
117 if ((iMotEnc==nullptr) && (_subDevVerbose))
118 {
119 yCWarning(CONTROLBOARDREMAPPER) << "IMotorEncoders not valid interface";
120 }
121
122 if ((imotor==nullptr) && (_subDevVerbose))
123 {
124 yCWarning(CONTROLBOARDREMAPPER) << "IMotor not valid interface";
125 }
126
127 if ((iVar == nullptr) && (_subDevVerbose))
128 {
129 yCWarning(CONTROLBOARDREMAPPER) << "IRemoveVariables not valid interface";
130 }
131
132 if ((info == nullptr) && (_subDevVerbose))
133 {
134 yCWarning(CONTROLBOARDREMAPPER) << "IAxisInfo not valid interface";
135 }
136
137 if ((iPwm == nullptr) && (_subDevVerbose))
138 {
139 yCWarning(CONTROLBOARDREMAPPER) << "IPWMControl not valid interface";
140 }
141
142 if ((iCurr == nullptr) && (_subDevVerbose))
143 {
144 yCWarning(CONTROLBOARDREMAPPER) << "ICurrentControl not valid interface";
145 }
146
147 if ((iFault == nullptr) && (_subDevVerbose))
148 {
149 yCWarning(CONTROLBOARDREMAPPER) << "IJointFault not valid interface";
150 }
151
152 if ((iBrake == nullptr) && (_subDevVerbose)) {
153 yCWarning(CONTROLBOARDREMAPPER) << "IJointBrake not valid interface";
154 }
155
156 if ((iVelDir == nullptr) && (_subDevVerbose))
157 {
158 yCWarning(CONTROLBOARDREMAPPER) << "IVelocityDirect not valid interface";
159 }
160
161 // checking minimum set of interfaces required
162 if( !(pos) )
163 {
164 yCWarning(CONTROLBOARDREMAPPER, "IPositionControl interface was not found in subdevice");
165 }
166
167 if( ! (vel) )
168 {
169 yCWarning(CONTROLBOARDREMAPPER, "IVelocityControl interface was not found in subdevice");
170 }
171
172 if(!iJntEnc)
173 {
174 yCWarning(CONTROLBOARDREMAPPER, "IEncoderTimed interface was not found in subdevice");
175 }
176
177 if(!iMode)
178 {
179 yCWarning(CONTROLBOARDREMAPPER, "IControlMode interface was not found in subdevice");
180 }
181
182 size_t deviceJoints=0;
183 if (pos!=nullptr)
184 {
185 if (!pos->getAxes(deviceJoints))
186 {
187 yCError(CONTROLBOARDREMAPPER) << "failed to get axes number for subdevice" << k.c_str();
188 return false;
189 }
190 if(deviceJoints <= 0)
191 {
192 yCError(CONTROLBOARDREMAPPER, "attached device has an invalid number of joints (%d)", deviceJoints);
193 return false;
194 }
195 }
196 else if(info!=nullptr)
197 {
199 {
200 yCError(CONTROLBOARDREMAPPER) << "failed to get axes number for subdevice" << k.c_str();
201 return false;
202 }
203 if(deviceJoints <= 0)
204 {
205 yCError(CONTROLBOARDREMAPPER, "attached device has an invalid number of joints (%d)", deviceJoints);
206 return false;
207 }
208 }
209 else
210 {
211 yCError(CONTROLBOARDREMAPPER, "attached device has no IPositionControl nor IAxisInfo interface");
212 return false;
213 }
214
215 attachedF=true;
216 return true;
217}
218
220{
221 // Resize buffers
223
224 size_t nrOfSubControlBoards = remappedControlBoards.getNrOfSubControlBoards();
225
228
233
235
236 for(size_t ctrlBrd=0; ctrlBrd < nrOfSubControlBoards; ctrlBrd++)
237 {
244 }
245
246 // Fill buffers
247 for(size_t j=0; j < remappedControlBoards.getNrOfRemappedAxes(); j++)
248 {
249 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
250 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
251
253 m_jointsInSubControlBoard[subIndex].push_back(off);
254 }
255
256 // Allocate enough space in buffers
257 for(size_t ctrlBrd=0; ctrlBrd < nrOfSubControlBoards; ctrlBrd++)
258 {
263
265 }
266
267 return true;
268}
269
270
271
273{
274 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
275 {
277 }
278
279 for(int j=0; j < m_nrOfControlledAxesInRemappedCtrlBrd; j++)
280 {
281 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
282
284 }
285}
286
288{
289 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
290 {
292 }
293
294 for(int j=0; j < m_nrOfControlledAxesInRemappedCtrlBrd; j++)
295 {
296 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
299 }
300}
301
303{
304 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
305 {
307 }
308
309 for(int j=0; j < m_nrOfControlledAxesInRemappedCtrlBrd; j++)
310 {
311 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
312
314 }
315}
316
331
333{
334 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
335 {
337 }
338
339 for(int j=0; j < m_nrOfControlledAxesInRemappedCtrlBrd; j++)
340 {
341 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
342
344 }
345}
346
348{
349 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
350 {
352 }
353
354 for(int j=0; j < m_nrOfControlledAxesInRemappedCtrlBrd; j++)
355 {
356 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
359 }
360}
361
363{
364 // Resize buffers
365 size_t nrOfSubControlBoards = remappedControlBoards.getNrOfSubControlBoards();
366
373
375
376 for(size_t ctrlBrd=0; ctrlBrd < nrOfSubControlBoards; ctrlBrd++)
377 {
383
384 }
385
386 // Count the maximum number of joints
387 for(size_t j=0; j < remappedControlBoards.getNrOfRemappedAxes(); j++)
388 {
389 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
390
392 }
393
394 // Allocate enough space in buffers
395 for(size_t ctrlBrd=0; ctrlBrd < nrOfSubControlBoards; ctrlBrd++)
396 {
401
404 }
405
406 return true;
407}
408
409
410
412 const int n_joints,
413 const int *joints,
414 const RemappedControlBoards& remappedControlBoards)
415{
416 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
417 {
419 }
420
421 for(int j=0; j < n_joints; j++)
422 {
423 size_t subIndex=remappedControlBoards.lut[joints[j]].subControlBoardIndex;
426 }
427}
428
429
431 const int n_joints,
432 const int *joints,
433 const RemappedControlBoards& remappedControlBoards)
434{
435 this->createListOfJointsDecomposition(n_joints,joints,remappedControlBoards);
436
437 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
438 {
440 }
441
442 for(int j=0; j < n_joints; j++)
443 {
444 size_t subIndex=remappedControlBoards.lut[joints[j]].subControlBoardIndex;
446 }
447}
448
449
451 const int n_joints,
452 const int *joints,
453 const RemappedControlBoards& remappedControlBoards)
454{
455 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
456 {
458 }
459
460 for(int j=0; j < n_joints; j++)
461 {
462 size_t subIndex=remappedControlBoards.lut[joints[j]].subControlBoardIndex;
465 }
466}
467
468
470 const int n_joints,
471 const int *joints,
472 const RemappedControlBoards& remappedControlBoards)
473{
474 this->createListOfJointsDecomposition(n_joints,joints,remappedControlBoards);
475
476 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
477 {
479 }
480
481 for(int j=0; j < n_joints; j++)
482 {
483 size_t subIndex=remappedControlBoards.lut[joints[j]].subControlBoardIndex;
485 }
486}
487
488
490 const int n_joints,
491 const int *joints,
492 const RemappedControlBoards& remappedControlBoards)
493{
494 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
495 {
497 }
498
499 for(int j=0; j < n_joints; j++)
500 {
501 size_t subIndex=remappedControlBoards.lut[joints[j]].subControlBoardIndex;
504 }
505}
506
507
509 const int n_joints,
510 const int *joints,
511 const RemappedControlBoards& remappedControlBoards)
512{
513 this->createListOfJointsDecomposition(n_joints,joints,remappedControlBoards);
514
515 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
516 {
518 }
519
520 for(int j=0; j < n_joints; j++)
521 {
522 size_t subIndex=remappedControlBoards.lut[joints[j]].subControlBoardIndex;
524 }
525}
526
527
528void ControlBoardArbitraryAxesDecomposition::createListOfJointsDecomposition(const int n_joints, const int* joints, const RemappedControlBoards & remappedControlBoards)
529{
530 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
531 {
534 }
535
536 // Fill buffers
537 for(int j=0; j < n_joints; j++)
538 {
539 int off=(int)remappedControlBoards.lut[joints[j]].axisIndexInSubControlBoard;
540 size_t subIndex=remappedControlBoards.lut[joints[j]].subControlBoardIndex;
541
543 m_jointsInSubControlBoard[subIndex].push_back(off);
544 }
545
546}
547
548void ControlBoardArbitraryAxesDecomposition::resizeSubControlBoardBuffers(const int n_joints, const int *joints, const RemappedControlBoards & remappedControlBoards)
549{
550 // Properly populate the m_nJointsInSubControlBoard and m_jointsInSubControlBoard methods
551 createListOfJointsDecomposition(n_joints,joints,remappedControlBoards);
552
553 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
554 {
557 }
558}
const yarp::os::LogComponent & CONTROLBOARDREMAPPER()
bool configure(const RemappedControlBoards &remappedControlBoards)
Resize the buffers using the information in the RemappedControlBoards.
std::vector< std::vector< double > > m_bufferForSubControlBoard
std::vector< std::vector< yarp::dev::SelectableControlModeEnum > > m_bufferForSubControlBoardSelectableControlModesEnum
std::vector< std::vector< yarp::dev::InteractionModeEnum > > m_bufferForSubControlBoardInteractionModes
std::vector< std::vector< yarp::dev::ControlModeEnum > > m_bufferForSubControlBoardControlModesEnum
void fillSubControlBoardBuffersFromArbitraryJointVector(const double *arbitraryVec, const int n_joints, const int *joints, const RemappedControlBoards &remappedControlBoards)
Fill buffers for the SubControlBoard from a vector of joints of the RemappedControlBoards.
std::vector< std::vector< int > > m_jointsInSubControlBoard
void resizeSubControlBoardBuffers(const int n_joints, const int *joints, const RemappedControlBoards &remappedControlBoards)
Resize buffers to have the dimension of specified by the method (used for multi joint methods that re...
void fillArbitraryJointVectorFromSubControlBoardBuffers(double *arbitraryVec, const int n_joints, const int *joints, const RemappedControlBoards &remappedControlBoards)
Fill a vector of joints of the ControlBoardRemapper from the buffers of the SubControlBoard .
bool configure(const RemappedControlBoards &remappedControlBoards)
Resize the buffers using the information in the RemappedControlBoards.
std::vector< std::vector< int > > m_jointsInSubControlBoard
void fillCompleteJointVectorFromSubControlBoardBuffers(double *full, const RemappedControlBoards &remappedControlBoards)
Fill a vector of joints of the ControlBoardRemapper from the buffers of the SubControlBoard .
void fillSubControlBoardBuffersFromCompleteJointVector(const double *full, const RemappedControlBoards &remappedControlBoards)
Fill buffers for the SubControlBoard from a vector of joints of the RemappedControlBoards.
std::vector< std::vector< double > > m_bufferForSubControlBoard
std::vector< std::vector< yarp::dev::ControlModeEnum > > m_bufferForSubControlBoardControlModesEnum
std::vector< std::vector< yarp::dev::SelectableControlModeEnum > > m_bufferForSubControlBoardSelectableControlModesEnum
std::vector< std::vector< yarp::dev::InteractionModeEnum > > m_bufferForSubControlBoardInteractionModes
std::vector< RemappedAxis > lut
Vector of dimension getNrOfRemappedAxes .
yarp::dev::IPositionControl * pos
yarp::dev::IImpedanceControl * iImpedance
yarp::dev::IAmplifierControl * amp
yarp::dev::IEncodersTimed * iJntEnc
yarp::dev::IInteractionMode * iInteract
yarp::dev::IRemoteVariables * iVar
yarp::dev::ICurrentControl * iCurr
yarp::dev::ITorqueControl * iTorque
yarp::dev::IVelocityControl * vel
yarp::dev::IControlMode * iMode
yarp::dev::IJointFault * iFault
yarp::dev::IVelocityDirect * iVelDir
yarp::dev::IPositionDirect * posDir
yarp::dev::IRemoteCalibrator * remcalib
bool attach(yarp::dev::PolyDriver *d, const std::string &id)
yarp::dev::IControlLimits * lim
yarp::dev::IJointBrake * iBrake
yarp::dev::IControlCalibration * calib
yarp::dev::PolyDriver * subdevice
yarp::dev::IMotorEncoders * iMotEnc
yarp::dev::IPreciselyTimed * iTimed
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
A container for a device driver.
Definition PolyDriver.h:23
bool isValid() const
Check if device is valid.
A mini-server for performing network communication in the background.
#define yCError(component,...)
#define yCAssert(component, x)
#define yCWarning(component,...)
For streams capable of holding different kinds of content, check what they actually have.
SelectableControlModeEnum
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13