YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
IPidControlTest.h
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2023-2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#ifndef IPIDCONTROLTEST_H
7#define IPIDCONTROLTEST_H
8
9#include <yarp/os/Vocab32.h>
10#include <yarp/os/Time.h>
11
14#include <yarp/dev/IAxisInfo.h>
15#include <catch2/catch_amalgamated.hpp>
16
17#include "TestUtils.h"
18
19using namespace yarp::dev;
20using namespace yarp::os;
21
22std::vector<yarp::dev::PidControlTypeEnum> pidenums =
23{
49};
50
51namespace yarp::dev::tests
52{
54 {
55 REQUIRE(ipid != nullptr);
56 REQUIRE(iaxis != nullptr);
57
58 size_t ax=0;
59 bool b = false;
60
61 b = iaxis->getAxes(ax);
62 CHECK(b);
63 REQUIRE(ax > 0);
64
67
68 // get available pids
69 {
70 bool b;
71 std::vector<yarp::dev::PidControlTypeEnum> avail;
72 b = ipid->getAvailablePids(0, avail);
73 CHECK(b);
74 CHECK(avail.size() == 7 * 3); // position, velocity, torque, current, position direct, velocity direct, mixed
75 }
76
77 // Pid enable/disable
78 {
79 bool enabled;
80 b = ipid->disablePid(pp,0);
81 CHECK(b);
82 b = ipid->isPidEnabled(pp, 0, enabled);
83 CHECK(b);
84 CHECK(!enabled);
85
86 b = ipid->enablePid(pp,0);
87 CHECK(b);
88 b = ipid->isPidEnabled(pp, 0, enabled);
89 CHECK(b);
90 CHECK(enabled);
91
92 b = ipid->disablePid(pp,0);
93 CHECK(b);
94 b = ipid->isPidEnabled(pp, 0, enabled);
95 CHECK(b);
96 CHECK(!enabled);
97 }
98
99 // Pid set/get
100 {
103
104 b=ipid->getPid(pp,0, &retpid);
105 CHECK(b);
106
107 b = ipid->setPid(pp, 0, setpid);
108 CHECK(b);
109
110 b=ipid->getPid(pp,0, &retpid);
111 CHECK(b);
112 CHECK(retpid == setpid);
113 }
114
115 // Pids set/get
116 {
117 auto retpids = std::vector<yarp::dev::Pid>(ax);
118 auto setpids = std::vector<yarp::dev::Pid>(ax);
119 setpids[0] = yarp::dev::Pid(0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0);
120 setpids[1] = yarp::dev::Pid(0.1, 1.1, 2.1, 3.1, 4.1, 5.1, 6.1, 7.1, 8.1);
121
122 b= ipid->getPids(pp, retpids.data());
123 CHECK(b);
124
125 b = ipid->setPids(pp, setpids.data());
126 CHECK(b);
127
128 b= ipid->getPids(pp, retpids.data());
129 CHECK(b);
131 }
132
133 b = ipid->resetPid(pp,0);
134 CHECK(b);
135
136 //PidOffset
137 {
138 double pidoff=0;
139 b = ipid->getPidOffset(pp,0,pidoff);
140 CHECK(b);
141 CHECK(pidoff == 0);
142 b = ipid->setPidOffset(pp,0,42);
143 CHECK(b);
144 wait_safe(); // Allow some time for the command to take effect
145 b = ipid->getPidOffset(pp,0,pidoff);
146 CHECK(b);
147 CHECK(pidoff == 42);
148 }
149
150 //PidFeedforward
151 {
152 double pidffd=0;
153 b = ipid->getPidFeedforward(pp,0,pidffd);
154 CHECK(b);
155 CHECK(pidffd == 0);
156 b = ipid->setPidFeedforward(pp,0,43);
157 CHECK(b);
158 wait_safe(); // Allow some time for the command to take effect
159 b = ipid->getPidFeedforward(pp,0,pidffd);
160 CHECK(b);
161 CHECK(pidffd == 43);
162 }
163
164 //PidReference
165 {
166 double getpidref=0;
167 b = ipid->getPidReference(pp, 0, &getpidref);
168 CHECK(b);
169 CHECK(getpidref == 0.0);
170 b = ipid->setPidReference(pp, 0, 10.5);
171 CHECK(b);
172 wait_safe(); // Allow some time for the command to take effect
173 b = ipid->getPidReference(pp, 0, &getpidref);
174 CHECK(b);
175 CHECK(getpidref == 10.5);
176 }
177
178 //PidReferences
179 {
180 auto setpidrefs = std::vector<double>(ax);
182 auto pidrefs = std::vector<double>(ax);
183 b = ipid->getPidReferences(pp, pidrefs.data());
184 CHECK(b);
185 b = ipid->setPidReferences(vv, setpidrefs.data());
186 CHECK(b);
187 wait_safe(); // Allow some time for the command to take effect
188 b = ipid->getPidReferences(vv, pidrefs.data());
189 CHECK(b);
191 }
192
193 //PidOutput
194 //There is no setPidOutput(): the fake device returns the PID offset as PID output,
195 //so the offset is set first and then read back through getPidOutput().
196 //This keeps the test independent from any joint remapping.
197 auto setpidouts = std::vector<double>(ax);
199 {
200 for (size_t j=0; j<ax; j++)
201 {
202 b = ipid->setPidOffset(pp, j, setpidouts[j]);
203 CHECK(b);
204 }
205 wait_safe(); // Allow some time for the command to take effect
206 double getpidout=0;
207 for (size_t j=0; j<ax; j++)
208 {
209 b = ipid->getPidOutput(pp, j, &getpidout);
210 CHECK(b);
212 }
213 }
214
215 //PidOutputs
216 {
217 auto pidouts = std::vector<double>(ax);
218 b = ipid->getPidOutputs(pp, pidouts.data());
219 CHECK(b);
221 }
222
223 // PidReference
224 {
225 auto setpidrefs = std::vector<double>(ax);
227 auto pidrefs = std::vector<double>(ax);
228 b = ipid->getPidReferences(pp, pidrefs.data());
229 CHECK(b);
230 b = ipid->setPidReferences(vv, setpidrefs.data());
231 CHECK(b);
232 b = ipid->getPidReferences(vv, pidrefs.data());
233 CHECK(b);
235 }
236
237 //Error limit
238 {
239 double errlim=0;
240 b = ipid->getPidErrorLimit(pp, 0, &errlim);
241 CHECK(b);
242 b = ipid->setPidErrorLimit(pp, 0, 5.0);
243 CHECK(b);
244 b = ipid->getPidErrorLimit(pp, 0, &errlim);
245 CHECK(b);
246 CHECK(errlim == 5.0);
247 }
248
249 //Error limits
250 {
251 auto seterrlims = std::vector<double>(ax);
252 auto errlims = std::vector<double>(ax);
254 b = ipid->getPidErrorLimits(vv, errlims.data());
255 CHECK(b);
256 b = ipid->setPidErrorLimits(vv, seterrlims.data());
257 CHECK(b);
258 b = ipid->getPidErrorLimits(vv, errlims.data());
259 CHECK(b);
261 }
262 }
263
265 {
266 //test one single pid
267 {
268 for (auto pp : pidenums)
269 {
271 INFO("Testing PidControlTypeEnum: " << dv);
272
273 bool b = false;
277 yarp::dev::Pid testpid(0, 1, 2, 3, 4, 5, 6, 7, 8);
278
279 b=ipid->setPid(pp,0, emptypid);
280 CHECK(b);
281
282 b=ipid->getPid(pp,0, &retpid);
283 CHECK(b);
285
286 b=ipid->setPid(pp,0, testpid);
287 CHECK(b);
288
289 b=ipid->getPid(pp,0, &retpid);
290 CHECK(b);
291 CHECK(retpid == testpid);
292
293 b = ipid->getPidExtraInfo(pp,0, infopid);
294 CHECK(b);
295 }
296 }
297
298 //test multiple pids
299 {
300 for (auto pp : pidenums)
301 {
303 INFO("Testing PidControlTypeEnum: " << dv);
304
305 bool b = false;
306 size_t ax=0;
307 b = iaxis->getAxes(ax);
308 CHECK(b);
309 REQUIRE(ax > 0);
310
311 std::vector<yarp::dev::Pid> retpids(ax);
312 std::vector<yarp::dev::Pid> emptypids(ax);
313 std::vector<yarp::dev::Pid> testpids(ax);
314 std::vector<yarp::dev::PidExtraInfo> infopids(ax);
315 testpids[0] = yarp::dev::Pid(0, 1, 2, 3, 4, 5, 6, 7, 8);
316 testpids[1] = yarp::dev::Pid(10, 11, 12, 13, 14, 15, 16, 17, 18);
317
318 b=ipid->setPids(pp, emptypids.data());
319 CHECK(b);
320
321 b=ipid->getPids(pp, retpids.data());
322 CHECK(b);
324
325 b=ipid->setPids(pp, testpids.data());
326 CHECK(b);
327
328 b=ipid->getPids(pp, retpids.data());
329 CHECK(b);
331
332 b = ipid->getPidExtraInfos(pp, infopids);
333 CHECK(b);
334 }
335 }
336 }
337
338
339}
340
341#endif
define control board standard interfaces
std::vector< yarp::dev::PidControlTypeEnum > pidenums
Interface for getting information about specific axes, if available.
Definition IAxisInfo.h:37
Interface for a generic control board device implementing a PID controller, with scaled arguments.
Contains the parameters for a PID.
A mini-server for performing network communication in the background.
std::int32_t vocab32_t
Definition numeric.h:78
void set_vector_crescent(std::vector< double > &a, double val)
Definition TestUtils.cpp:65
void exec_iPidControl_test_2(IPidControl *ipid, IAxisInfo *iaxis)
void exec_iPidControl_test_1(IPidControl *ipid, IAxisInfo *iaxis)
void wait_safe(const double seconds)
Definition TestUtils.cpp:17
For streams capable of holding different kinds of content, check what they actually have.
PidControlTypeEnum
Definition PidEnums.h:15
std::string decode(NetInt32 code)
Convert a vocabulary identifier into a string.
Definition Vocab32.cpp:33
An interface to the operating system, including Port based communication.
std::string INFO
Definition ymanager.cpp:54