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
160 b = ipid->getPidFeedforward(pp,0,pidffd);
161 CHECK(b);
162 CHECK(pidffd == 43);
163 }
164
165 //PidReference
166 {
167 double getpidref=0;
168 b = ipid->getPidReference(pp, 0, &getpidref);
169 CHECK(b);
170 CHECK(getpidref == 0.0);
171 b = ipid->setPidReference(pp, 0, 10.5);
172 CHECK(b);
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 b = ipid->getPidReferences(vv, pidrefs.data());
188 CHECK(b);
190 }
191
192 //Error limit
193 {
194 double errlim=0;
195 b = ipid->getPidErrorLimit(pp, 0, &errlim);
196 CHECK(b);
197 b = ipid->setPidErrorLimit(pp, 0, 5.0);
198 CHECK(b);
199 b = ipid->getPidErrorLimit(pp, 0, &errlim);
200 CHECK(b);
201 CHECK(errlim == 5.0);
202 }
203
204 //Error limits
205 {
206 auto seterrlims = std::vector<double>(ax);
207 auto errlims = std::vector<double>(ax);
209 b = ipid->getPidErrorLimits(vv, errlims.data());
210 CHECK(b);
211 b = ipid->setPidErrorLimits(vv, seterrlims.data());
212 CHECK(b);
213 b = ipid->getPidErrorLimits(vv, errlims.data());
214 CHECK(b);
216 }
217 }
218
220 {
221 //test one single pid
222 {
223 for (auto pp : pidenums)
224 {
226 INFO("Testing PidControlTypeEnum: " << dv);
227
228 bool b = false;
232 yarp::dev::Pid testpid(0, 1, 2, 3, 4, 5, 6, 7, 8);
233
234 b=ipid->setPid(pp,0, emptypid);
235 CHECK(b);
236
237 b=ipid->getPid(pp,0, &retpid);
238 CHECK(b);
240
241 b=ipid->setPid(pp,0, testpid);
242 CHECK(b);
243
244 b=ipid->getPid(pp,0, &retpid);
245 CHECK(b);
246 CHECK(retpid == testpid);
247
248 b = ipid->getPidExtraInfo(pp,0, infopid);
249 CHECK(b);
250 }
251 }
252
253 //test multiple pids
254 {
255 for (auto pp : pidenums)
256 {
258 INFO("Testing PidControlTypeEnum: " << dv);
259
260 bool b = false;
261 size_t ax=0;
262 b = iaxis->getAxes(ax);
263 CHECK(b);
264 REQUIRE(ax > 0);
265
266 std::vector<yarp::dev::Pid> retpids(ax);
267 std::vector<yarp::dev::Pid> emptypids(ax);
268 std::vector<yarp::dev::Pid> testpids(ax);
269 std::vector<yarp::dev::PidExtraInfo> infopids(ax);
270 testpids[0] = yarp::dev::Pid(0, 1, 2, 3, 4, 5, 6, 7, 8);
271 testpids[1] = yarp::dev::Pid(10, 11, 12, 13, 14, 15, 16, 17, 18);
272
273 b=ipid->setPids(pp, emptypids.data());
274 CHECK(b);
275
276 b=ipid->getPids(pp, retpids.data());
277 CHECK(b);
279
280 b=ipid->setPids(pp, testpids.data());
281 CHECK(b);
282
283 b=ipid->getPids(pp, retpids.data());
284 CHECK(b);
286
287 b = ipid->getPidExtraInfos(pp, infopids);
288 CHECK(b);
289 }
290 }
291 }
292
293
294}
295
296#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.
Definition OSTestMsg.cpp:13
std::string INFO
Definition ymanager.cpp:54