YARP
Yet Another Robot Platform
ControlBoardWrapperVelocityControl.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 
8 #include <yarp/os/LogStream.h>
9 
11 
13 {
14  size_t off;
15  try {
16  off = device.lut.at(j).offset;
17  } catch (...) {
18  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
19  return false;
20  }
21  size_t subIndex = device.lut[j].deviceEntry;
22 
23  SubDevice* p = device.getSubdevice(subIndex);
24 
25  if (!p) {
26  return false;
27  }
28 
29  if (p->vel) {
30  return p->vel->velocityMove(static_cast<int>(off + p->base), v);
31  }
32  return false;
33 }
34 
36 {
37  bool ret = true;
38  int j_wrap = 0; // index of the wrapper joint
39 
40  for (size_t subDev_idx = 0; subDev_idx < device.subdevices.size(); subDev_idx++) {
41  SubDevice* p = device.getSubdevice(subDev_idx);
42 
43  if (!p) {
44  return false;
45  }
46 
47  int wrapped_joints = static_cast<int>((p->top - p->base) + 1);
48  int* joints = new int[wrapped_joints];
49 
50  if (p->vel) {
51  // verione comandi su subset di giunti
52  for (int j_dev = 0; j_dev < wrapped_joints; j_dev++) {
53  joints[j_dev] = static_cast<int>(p->base + j_dev);
54  }
55 
56  ret = ret && p->vel->velocityMove(wrapped_joints, joints, &v[j_wrap]);
57  j_wrap += wrapped_joints;
58  } else {
59  ret = false;
60  }
61 
62  if (joints != nullptr) {
63  delete[] joints;
64  joints = nullptr;
65  }
66  }
67 
68  return ret;
69 }
70 
71 bool ControlBoardWrapperVelocityControl::velocityMove(const int n_joints, const int* joints, const double* spds)
72 {
73  bool ret = true;
74 
75  rpcDataMutex.lock();
76  //Reset subdev_jointsVectorLen vector
77  memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum);
78 
79  // Create a map of joints for each subDevice
80  size_t subIndex = 0;
81  for (int j = 0; j < n_joints; j++) {
82  subIndex = device.lut[joints[j]].deviceEntry;
83  rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] =
84  static_cast<int>(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base);
85  rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = spds[j];
86  rpcData.subdev_jointsVectorLen[subIndex]++;
87  }
88 
89  for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) {
90  if (rpcData.subdevices_p[subIndex]->vel) {
91  ret = ret && rpcData.subdevices_p[subIndex]->vel->velocityMove(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]);
92  } else {
93  ret = false;
94  }
95  }
96  rpcDataMutex.unlock();
97  return ret;
98 }
99 
101 {
103 
104  size_t off;
105  try {
106  off = device.lut.at(j).offset;
107  } catch (...) {
108  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
109  return false;
110  }
111  size_t subIndex = device.lut[j].deviceEntry;
112 
113  SubDevice* p = device.getSubdevice(subIndex);
114 
115  if (!p) {
116  return false;
117  }
118 
119  if (p->vel) {
120  bool ret = p->vel->getRefVelocity(static_cast<int>(off + p->base), vel);
121  return ret;
122  }
123  *vel = 0;
124  return false;
125 }
126 
127 
129 {
130  auto* references = new double[device.maxNumOfJointsInDevices];
131  bool ret = true;
132  for (size_t d = 0; d < device.subdevices.size(); d++) {
133  SubDevice* p = device.getSubdevice(d);
134  if (!p) {
135  ret = false;
136  break;
137  }
138 
139  if ((p->vel) && (ret = p->vel->getRefVelocities(references))) {
140  for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) {
141  vels[juser] = references[jdevice];
142  }
143  } else {
144  printError("getRefVelocities", p->id, ret);
145  ret = false;
146  break;
147  }
148  }
149 
150  delete[] references;
151  return ret;
152 }
153 
154 bool ControlBoardWrapperVelocityControl::getRefVelocities(const int n_joints, const int* joints, double* vels)
155 {
157 
158  bool ret = true;
159 
160  rpcDataMutex.lock();
161  //Reset subdev_jointsVectorLen vector
162  memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum);
163 
164  // Create a map of joints for each subDevice
165  size_t subIndex = 0;
166  for (int j = 0; j < n_joints; j++) {
167  subIndex = device.lut[joints[j]].deviceEntry;
168  rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] =
169  static_cast<int>(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base);
170  rpcData.subdev_jointsVectorLen[subIndex]++;
171  }
172 
173  for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) {
174  if (rpcData.subdevices_p[subIndex]->vel) {
176  }
177  }
178 
179  if (ret) {
180  // ReMix values by user expectations
181  for (size_t i = 0; i < rpcData.deviceNum; i++) {
182  rpcData.subdev_jointsVectorLen[i] = 0; // reset tmp index
183  }
184 
185  // fill the output vector
186  for (int j = 0; j < n_joints; j++) {
187  subIndex = device.lut[joints[j]].deviceEntry;
188  vels[j] = rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]];
189  rpcData.subdev_jointsVectorLen[subIndex]++;
190  }
191  } else {
192  for (int j = 0; j < n_joints; j++) {
193  vels[j] = 0;
194  }
195  }
196  rpcDataMutex.unlock();
197  return ret;
198 }
const yarp::os::LogComponent & CONTROLBOARD()
bool ret
void printError(const std::string &func_name, const std::string &info, bool result)
bool getRefVelocity(const int joint, double *vel) override
Get the last reference speed set by velocityMove for single joint.
bool getRefVelocities(double *vels) override
Get the last reference speed set by velocityMove for all joints.
bool velocityMove(int j, double v) override
Start motion at a given speed, single joint.
int * subdev_jointsVectorLen
SubDevice ** subdevices_p
double ** values
size_t base
Definition: SubDevice.h:55
std::string id
Definition: SubDevice.h:54
yarp::dev::IVelocityControl * vel
Definition: SubDevice.h:69
size_t top
Definition: SubDevice.h:56
size_t wbase
Definition: SubDevice.h:57
std::vector< DevicesLutEntry > lut
Definition: SubDevice.h:122
SubDevice * getSubdevice(size_t i)
Definition: SubDevice.h:125
size_t maxNumOfJointsInDevices
Definition: SubDevice.h:123
SubDeviceVector subdevices
Definition: SubDevice.h:121
virtual bool getRefVelocity(const int joint, double *vel)
Get the last reference speed set by velocityMove for single joint.
virtual bool velocityMove(int j, double sp)=0
Start motion at a given speed, single joint.
virtual bool getRefVelocities(double *vels)
Get the last reference speed set by velocityMove for all joints.
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCTrace(component,...)
Definition: LogComponent.h:84