YARP
Yet Another Robot Platform
ControlBoardWrapperCommon.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
9
10
12{
13 *ax = static_cast<int>(controlledJoints);
14 return true;
15}
16
17
19{
20 size_t off;
21 try {
22 off = device.lut.at(j).offset;
23 } catch (...) {
24 yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
25 return false;
26 }
27 size_t subIndex = device.lut[j].deviceEntry;
28
29 SubDevice* p = device.getSubdevice(subIndex);
30
31 if (!p) {
32 return false;
33 }
34
35 if (p->pos) {
36 return p->pos->setRefAcceleration(static_cast<int>(off + p->base), acc);
37 }
38 return false;
39}
40
41
43{
44 bool ret = true;
45 int j_wrap = 0; // index of the joint from the wrapper side (useful if wrapper joins 2 subdevices)
46
47 // for all subdevices
48 for (size_t subDev_idx = 0; subDev_idx < device.subdevices.size(); subDev_idx++) {
49 SubDevice* p = device.getSubdevice(subDev_idx);
50
51 if (!p) {
52 return false;
53 }
54
55 int wrapped_joints = static_cast<int>((p->top - p->base) + 1);
56 int* joints = new int[wrapped_joints]; // to be defined once and for all?
57
58 if (p->pos) {
59 // verione comandi su subset di giunti
60 for (int j_dev = 0; j_dev < wrapped_joints; j_dev++) {
61 joints[j_dev] = static_cast<int>(p->base + j_dev);
62 }
63
64 ret = ret && p->pos->setRefAccelerations(wrapped_joints, joints, &accs[j_wrap]);
65 j_wrap += wrapped_joints;
66 } else {
67 ret = false;
68 }
69
70 if (joints != nullptr) {
71 delete[] joints;
72 joints = nullptr;
73 }
74 }
75
76 return ret;
77}
78
79
80bool ControlBoardWrapperCommon::setRefAccelerations(const int n_joints, const int* joints, const double* accs)
81{
82 bool ret = true;
83
84 rpcDataMutex.lock();
85 //Reset subdev_jointsVectorLen vector
86 memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum);
87
88 // Create a map of joints for each subDevice
89 size_t subIndex = 0;
90 for (int j = 0; j < n_joints; j++) {
91 subIndex = device.lut[joints[j]].deviceEntry;
93 static_cast<int>(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base);
94 rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = accs[j];
96 }
97
98 for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) {
99 if (rpcData.subdevices_p[subIndex]->pos) {
101 } else {
102 ret = false;
103 }
104 }
105 rpcDataMutex.unlock();
106 return ret;
107}
108
109
111{
112 size_t off;
113 try {
114 off = device.lut.at(j).offset;
115 } catch (...) {
116 yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
117 return false;
118 }
119 size_t subIndex = device.lut[j].deviceEntry;
120
121 SubDevice* p = device.getSubdevice(subIndex);
122
123 if (!p) {
124 return false;
125 }
126
127 if (p->pos) {
128 return p->pos->getRefAcceleration(static_cast<int>(off + p->base), acc);
129 }
130 *acc = 0;
131 return false;
132}
133
134
136{
137 auto* references = new double[device.maxNumOfJointsInDevices];
138 bool ret = true;
139 for (size_t d = 0; d < device.subdevices.size(); d++) {
141 if (!p) {
142 ret = false;
143 break;
144 }
145
146 if ((p->pos) && (ret = p->pos->getRefAccelerations(references))) {
147 for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) {
148 accs[juser] = references[jdevice];
149 }
150 } else {
151 printError("getRefAccelerations", p->id, ret);
152 ret = false;
153 break;
154 }
155 }
156
157 delete[] references;
158 return ret;
159}
160
161
162bool ControlBoardWrapperCommon::getRefAccelerations(const int n_joints, const int* joints, double* accs)
163{
164 bool ret = true;
165
166 rpcDataMutex.lock();
167 //Reset subdev_jointsVectorLen vector
168 memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum);
169
170 // Create a map of joints for each subDevice
171 size_t subIndex = 0;
172 for (int j = 0; j < n_joints; j++) {
173 subIndex = device.lut[joints[j]].deviceEntry;
175 static_cast<int>(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base);
177 }
178
179 for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) {
180 if (rpcData.subdevices_p[subIndex]->pos) {
182 } else {
183 ret = false;
184 }
185 }
186
187 if (ret) {
188 // ReMix values by user expectations
189 for (size_t i = 0; i < rpcData.deviceNum; i++) {
190 rpcData.subdev_jointsVectorLen[i] = 0; // reset tmp index
191 }
192
193 // fill the output vector
194 for (int j = 0; j < n_joints; j++) {
195 subIndex = device.lut[joints[j]].deviceEntry;
196 accs[j] = rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]];
198 }
199 } else {
200 for (int j = 0; j < n_joints; j++) {
201 accs[j] = 0;
202 }
203 }
204 rpcDataMutex.unlock();
205 return ret;
206}
207
208
210{
211 size_t off;
212 try {
213 off = device.lut.at(j).offset;
214 } catch (...) {
215 yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
216 return false;
217 }
218 size_t subIndex = device.lut[j].deviceEntry;
219
220 SubDevice* p = device.getSubdevice(subIndex);
221
222 if (!p) {
223 return false;
224 }
225
226 if (p->pos) {
227 return p->pos->stop(static_cast<int>(off + p->base));
228 }
229 return false;
230}
231
232
234{
235 bool ret = true;
236
237 for (size_t l = 0; l < controlledJoints; l++) {
238 size_t off = device.lut[l].offset;
239 size_t subIndex = device.lut[l].deviceEntry;
240
241 SubDevice* p = device.getSubdevice(subIndex);
242
243 if (!p) {
244 return false;
245 }
246
247 if (p->pos) {
248 ret = ret && p->pos->stop(static_cast<int>(off + p->base));
249 } else {
250 ret = false;
251 }
252 }
253 return ret;
254}
255
256
257bool ControlBoardWrapperCommon::stop(const int n_joints, const int* joints)
258{
259 bool ret = true;
260
261 rpcDataMutex.lock();
262 //Reset subdev_jointsVectorLen vector
263 memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum);
264
265 // Create a map of joints for each subDevice
266 size_t subIndex = 0;
267 for (int j = 0; j < n_joints; j++) {
268 subIndex = device.lut[joints[j]].deviceEntry;
270 static_cast<int>(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base);
272 }
273
274 for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) {
275 if (rpcData.subdevices_p[subIndex]->pos) {
276 ret = ret && rpcData.subdevices_p[subIndex]->pos->stop(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex]);
277 } else {
278 ret = false;
279 }
280 }
281 rpcDataMutex.unlock();
282 return ret;
283}
284
285
287{
288 *num = static_cast<int>(controlledJoints);
289 return true;
290}
291
292
294{
295 auto* currs = new double[device.maxNumOfJointsInDevices];
296 bool ret = true;
297 for (size_t d = 0; d < device.subdevices.size(); d++) {
298 ret = false;
300 if (!p) {
301 break;
302 }
303
304 if (p->iCurr) {
305 ret = p->iCurr->getCurrents(currs);
306 } else if (p->amp) {
307 ret = p->amp->getCurrents(currs);
308 }
309
310 if (ret) {
311 for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) {
312 vals[juser] = currs[jdevice];
313 }
314 } else {
315 printError("getCurrents", p->id, ret);
316 break;
317 }
318 }
319 delete[] currs;
320 return ret;
321}
322
323
325{
326 size_t off;
327 try {
328 off = device.lut.at(j).offset;
329 } catch (...) {
330 yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
331 return false;
332 }
333 size_t subIndex = device.lut[j].deviceEntry;
334
335 SubDevice* p = device.getSubdevice(subIndex);
336 if (!p) {
337 return false;
338 }
339
340 if (p->iCurr) {
341 return p->iCurr->getCurrent(static_cast<int>(off + p->base), val);
342 }
343
344 if (p->amp) {
345 return p->amp->getCurrent(static_cast<int>(off + p->base), val);
346 }
347 *val = 0.0;
348 return false;
349}
const yarp::os::LogComponent & CONTROLBOARD()
bool ret
bool getCurrent(int m, double *curr)
bool getRefAcceleration(int j, double *acc)
bool setRefAcceleration(int j, double acc)
bool setRefAccelerations(const double *accs)
void printError(const std::string &func_name, const std::string &info, bool result)
int * subdev_jointsVectorLen
SubDevice ** subdevices_p
double ** values
size_t base
Definition: SubDevice.h:55
std::string id
Definition: SubDevice.h:54
yarp::dev::IAmplifierControl * amp
Definition: SubDevice.h:72
yarp::dev::IPositionControl * pos
Definition: SubDevice.h:68
size_t top
Definition: SubDevice.h:56
size_t wbase
Definition: SubDevice.h:57
yarp::dev::ICurrentControl * iCurr
Definition: SubDevice.h:85
std::vector< DevicesLutEntry > lut
Definition: SubDevice.h:122
size_t maxNumOfJointsInDevices
Definition: SubDevice.h:123
SubDeviceVector subdevices
Definition: SubDevice.h:121
SubDevice * getSubdevice(size_t i)
Definition: SubDevice.h:125
virtual bool getCurrents(double *vals)=0
virtual bool getCurrent(int j, double *val)=0
virtual bool getCurrents(double *currs)=0
Get the instantaneous current measurement for all motors.
virtual bool getCurrent(int m, double *curr)=0
Get the instantaneous current measurement for a single motor.
virtual bool getRefAcceleration(int j, double *acc)=0
Get reference acceleration for a joint.
virtual bool setRefAcceleration(int j, double acc)=0
Set reference acceleration for a joint.
virtual bool stop(int j)=0
Stop motion, single joint.
virtual bool setRefAccelerations(const double *accs)=0
Set reference acceleration on all joints.
virtual bool getRefAccelerations(double *accs)=0
Get reference acceleration of all joints.
#define yCError(component,...)
Definition: LogComponent.h:213