YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ImplementPidControl.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
9#include <yarp/os/LogStream.h>
11
12#include <cmath>
13
14using namespace yarp::dev;
15using namespace yarp::os;
16
17namespace {
18YARP_LOG_COMPONENT(IMPLEMENT_LAYER_COMPONENT, "yarp.ImplementPidControl")
19}
20
23m_helper(nullptr)
24{
25 m_iraw= dynamic_cast<IPidControlRaw *> (y);
26}
27
32
33bool ImplementPidControl:: initialize (int size, const int *amap, const double *enc, const double *zos, const double* newtons, const double* amps, const double* dutys)
34{
35 if (m_helper != nullptr) {
36 return false;
37 }
38
39 m_helper=(void *)(new ControlBoardHelper(size, amap, enc, zos,newtons,amps,nullptr,dutys));
40 yAssert (m_helper != nullptr);
41
42 m_buffer_ints.resize (size);
43 m_buffer_doubles.resize(size);
44 m_buffer_pids.resize(size);
45
46 return true;
47}
48
54{
55 if (m_helper!=nullptr)
56 {
57 delete castToMapper(m_helper);
58 m_helper=nullptr;
59 }
60
61 return true;
62}
63
64ReturnValue ImplementPidControl::getAvailablePids(int j, std::vector<PidControlTypeEnum>& avail)
65{
66 std::lock_guard lock(m_imp_mutex);
68
70 int k=castToMapper(m_helper)->toHw(j);
72 return m_iraw->getAvailablePidsRaw(k, avail);
73}
74
76{
77 std::lock_guard lock(m_imp_mutex);
79
81 int k;
83 cb_helper->convert_pid_to_machine(pidtype, pid, j, pid_machine, k);
84 return m_iraw->setPidRaw(pidtype, k, pid_machine);
85}
86
88{
89 std::lock_guard lock(m_imp_mutex);
91
93 int nj= cb_helper->axes();
94
95 for(int j=0;j<nj;j++)
96 {
98 int k;
99 cb_helper->convert_pid_to_machine(pidtype, pids[j], j, pid_machine, k);
101 }
102
103 auto ret = m_iraw->setPidsRaw(pidtype, m_buffer_pids.data());
104
105 return ret;
106}
107
109{
110 std::lock_guard lock(m_imp_mutex);
112
113 int k=0;
114 double raw;
116 cb_helper->convert_pidunits_to_machine(pidtype,ref,j,raw,k);
117 return m_iraw->setPidReferenceRaw(pidtype, k, raw);
118}
119
121{
122 std::lock_guard lock(m_imp_mutex);
124
126
127 cb_helper->convert_pidunits_to_machine(pidtype,refs,m_buffer_doubles.data());
129
130 return ret;
131}
132
134{
135 std::lock_guard lock(m_imp_mutex);
137
138 int k;
139 double raw;
141 cb_helper->convert_pidunits_to_machine(pidtype,limit,j,raw,k);
142 return m_iraw->setPidErrorLimitRaw(pidtype, k, raw);
143}
144
146{
147 std::lock_guard lock(m_imp_mutex);
149
151
152 cb_helper->convert_pidunits_to_machine(pidtype,limits,m_buffer_doubles.data());
154
155 return ret;
156}
157
159{
160 std::lock_guard lock(m_imp_mutex);
163
164 int k;
165 double raw;
168
169 ReturnValue ret=m_iraw->getPidErrorRaw(pidtype, k, &raw);
170
171 cb_helper->convert_pidunits_to_user(pidtype,raw,err,k);
172 return ret;
173}
174
176{
177 std::lock_guard lock(m_imp_mutex);
179
180 ReturnValue ret;
182
183 ret=m_iraw->getPidErrorsRaw(pidtype, m_buffer_doubles.data());
184 cb_helper->convert_pidunits_to_user(pidtype,m_buffer_doubles.data(),errs);
185
186 return ret;
187}
188
190{
191 std::lock_guard lock(m_imp_mutex);
194
195 ReturnValue ret;
196 int k_raw;
197 double raw;
199 ret = m_iraw->getPidOutputRaw(pidtype, k_raw, &raw);
200 if (ret)
201 {
203 double output_conversion_units_user2raw = cb_helper->get_pidoutput_conversion_factor_user2raw(pidtype, j);
205 return ret;
206 }
207 return ret;
208}
209
211{
212 std::lock_guard lock(m_imp_mutex);
214
216 int nj = cb_helper->axes();
217
218 ReturnValue ret = m_iraw->getPidOutputsRaw(pidtype, m_buffer_doubles.data());
219 if (ret)
220 {
222 for (int j = 0; j < nj; j++)
223 {
224 double output_conversion_units_user2raw = cb_helper->get_pidoutput_conversion_factor_user2raw(pidtype, j);
225 outs[j] = outs[j] / output_conversion_units_user2raw;
226 }
227 }
228
229 return ret;
230}
231
233{
234 std::lock_guard lock(m_imp_mutex);
237
239 int k_raw;
240 k_raw=cb_helper->toHw(j);
241 Pid rawPid;
242 auto ret = m_iraw->getPidRaw(pidtype, k_raw, &rawPid);
243 if (ret)
244 {
245 cb_helper->convert_pid_to_user(pidtype, rawPid, k_raw, *pid, j);
246 return ret;
247 }
248 return ret;
249}
250
252{
253 std::lock_guard lock(m_imp_mutex);
255
256 auto ret = m_iraw->getPidsRaw(pidtype, m_buffer_pids.data());
257 if(!ret)
258 {
259 return ret;
260 }
261
263 int nj=cb_helper->axes();
264
265 for (int k_raw = 0; k_raw < nj; k_raw++)
266 {
267 int j_usr;
268 Pid outpid;
269 cb_helper->convert_pid_to_user(pidtype, m_buffer_pids[k_raw], k_raw, outpid, j_usr);
270 pids[j_usr] = outpid;
271 }
272
273 return ret;
274}
275
277{
278 std::lock_guard lock(m_imp_mutex);
281
282 ReturnValue ret;
283 int k;
284 double raw;
287
288 ret=m_iraw->getPidReferenceRaw(pidtype, k, &raw);
289
290 cb_helper->convert_pidunits_to_user(pidtype,raw,ref,k);
291 return ret;
292}
293
295{
296 std::lock_guard lock(m_imp_mutex);
298
299 ReturnValue ret;
301
302 ret=m_iraw->getPidReferencesRaw(pidtype, m_buffer_doubles.data());
303
304 cb_helper->convert_pidunits_to_user(pidtype,m_buffer_doubles.data(),refs);
305
306 return ret;
307}
308
310{
311 std::lock_guard lock(m_imp_mutex);
314
315 ReturnValue ret;
316 int k;
317 double raw;
320
321 ret=m_iraw->getPidErrorLimitRaw(pidtype, k, &raw);
322
323 cb_helper->convert_pidunits_to_user(pidtype,raw,ref,k);
324 return ret;
325}
326
328{
329 std::lock_guard lock(m_imp_mutex);
331
332 ReturnValue ret;
334
335 ret=m_iraw->getPidErrorLimitsRaw(pidtype, m_buffer_doubles.data());
336
337 cb_helper->convert_pidunits_to_user(pidtype,m_buffer_doubles.data(),refs);
338 return ret;
339}
340
342{
343 std::lock_guard lock(m_imp_mutex);
345
346 int k=0;
348
349 return m_iraw->resetPidRaw(pidtype, k);
350}
351
353{
354 std::lock_guard lock(m_imp_mutex);
356
357 int k=0;
359
360 return m_iraw->enablePidRaw(pidtype, k);
361}
362
364{
365 std::lock_guard lock(m_imp_mutex);
367
368 int k=0;
370
371 return m_iraw->disablePidRaw(pidtype, k);
372}
373
375{
376 std::lock_guard lock(m_imp_mutex);
378
379 int k = 0;
381 double rawoff;
383 double output_conversion_units_user2raw = cb_helper->get_pidoutput_conversion_factor_user2raw(pidtype,j);
385 return m_iraw->setPidOffsetRaw(pidtype, k, rawoff);
386}
387
389{
390 std::lock_guard lock(m_imp_mutex);
392
393 int k = 0;
395 double rawoff;
397 double output_conversion_units_user2raw = cb_helper->get_pidoutput_conversion_factor_user2raw(pidtype,j);
399 return m_iraw->setPidFeedforwardRaw(pidtype, k, rawoff);
400}
401
403{
404 std::lock_guard lock(m_imp_mutex);
406
407 ReturnValue ret;
408 int k;
409 double raw;
412
413 ret=m_iraw->getPidOffsetRaw(pidtype, k, raw);
414
415 cb_helper->convert_pidunits_to_user(pidtype,raw,&off,k);
416 return ret;
417}
418
420{
421 std::lock_guard lock(m_imp_mutex);
423
424 ReturnValue ret;
425 int k;
426 double raw;
429
430 ret=m_iraw->getPidFeedforwardRaw(pidtype, k, raw);
431
432 cb_helper->convert_pidunits_to_user(pidtype,raw,&ffd,k);
433 return ret;
434}
435
437{
438 std::lock_guard lock(m_imp_mutex);
440
441 int k=0;
443
444 return m_iraw->isPidEnabledRaw(pidtype, k, enabled);
445}
446
454
456{
457 std::lock_guard lock(m_imp_mutex);
459
460 int k=0;
462 return m_iraw->getPidExtraInfoRaw(pidtype, k, units);
463}
464
465yarp::dev::ReturnValue ImplementPidControl::getPidExtraInfos(const PidControlTypeEnum& pidtype, std::vector<yarp::dev::PidExtraInfo>& units)
466{
467 std::lock_guard lock(m_imp_mutex);
469
470 ReturnValue ret;
471 std::vector<yarp::dev::PidExtraInfo> tmp_infos;
472 tmp_infos.resize(units.size());
473 ret = m_iraw->getPidExtraInfosRaw(pidtype, tmp_infos);
474
475 for (size_t i = 0; i < tmp_infos.size(); i++)
476 {
477 int o = castToMapper(m_helper)->toHw(i);
478 units[o] = tmp_infos[i];
479 }
480
481 return ret;
482}
yarp::dev::ControlBoardHelper * castToMapper(void *p)
#define yAssert(x)
Definition Log.h:388
void set_pid_conversion_units(const PidControlTypeEnum &pidtype, const PidFeedbackUnitsEnum fbk_conv_units, const PidOutputUnitsEnum out_conv_units)
Interface for a generic control board device implementing a PID controller.
Definition IPidControl.h:31
virtual yarp::dev::ReturnValue getPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, double *limits)=0
Get the error limit for all controllers.
virtual yarp::dev::ReturnValue getPidRaw(const PidControlTypeEnum &pidtype, int j, Pid *pid)=0
Get current PID value for a specific joint.
virtual yarp::dev::ReturnValue getPidReferencesRaw(const PidControlTypeEnum &pidtype, double *refs)=0
Get the current reference of all PID controllers.
virtual yarp::dev::ReturnValue setPidRaw(const PidControlTypeEnum &pidtype, int j, const Pid &pid)=0
Set new PID value for a joint axis.
virtual yarp::dev::ReturnValue setPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double ref)=0
Set the controller reference for a given axis.
virtual yarp::dev::ReturnValue getPidOutputsRaw(const PidControlTypeEnum &pidtype, double *outs)=0
Get the output of the controllers (e.g.
virtual yarp::dev::ReturnValue getPidOutputRaw(const PidControlTypeEnum &pidtype, int j, double *out)=0
Get the output of the controller (e.g.
virtual yarp::dev::ReturnValue getPidExtraInfosRaw(const PidControlTypeEnum &pidtype, std::vector< yarp::dev::PidExtraInfo > &info)=0
Get PID extra info and measurement units for a specific joint.
virtual yarp::dev::ReturnValue isPidEnabledRaw(const PidControlTypeEnum &pidtype, int j, bool &enabled)=0
Get the current status (enabled/disabled) of the PID controller.
virtual yarp::dev::ReturnValue getPidExtraInfoRaw(const PidControlTypeEnum &pidtype, int j, yarp::dev::PidExtraInfo &info)=0
Get PID extra info and measurement units for a specific joint.
virtual yarp::dev::ReturnValue getAvailablePidsRaw(int j, std::vector< PidControlTypeEnum > &avail)=0
Return a list of available pid types mode for the given joint.
virtual yarp::dev::ReturnValue enablePidRaw(const PidControlTypeEnum &pidtype, int j)=0
Enable the PID computation for a joint.
virtual yarp::dev::ReturnValue setPidReferencesRaw(const PidControlTypeEnum &pidtype, const double *refs)=0
Set the controller reference, multiple axes.
virtual yarp::dev::ReturnValue setPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, const double *limits)=0
Get the error limit for the controller on all joints.
virtual yarp::dev::ReturnValue getPidErrorRaw(const PidControlTypeEnum &pidtype, int j, double *err)=0
Get the current error for a joint.
virtual yarp::dev::ReturnValue getPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double *ref)=0
Get the current reference of the PID controller for a specific joint.
virtual yarp::dev::ReturnValue getPidFeedforwardRaw(const PidControlTypeEnum &pidtype, int j, double &v)=0
Get the currently set feedforward value for the specified PID controller.
virtual yarp::dev::ReturnValue getPidsRaw(const PidControlTypeEnum &pidtype, Pid *pids)=0
Get current PID value for a specific joint.
virtual yarp::dev::ReturnValue getPidOffsetRaw(const PidControlTypeEnum &pidtype, int j, double &v)=0
Get the currently set offset value for the specified PID controller.
virtual yarp::dev::ReturnValue setPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double limit)=0
Set the error limit for the controller on a specific joint.
virtual yarp::dev::ReturnValue disablePidRaw(const PidControlTypeEnum &pidtype, int j)=0
Disable the PID computation for a joint.
virtual yarp::dev::ReturnValue resetPidRaw(const PidControlTypeEnum &pidtype, int j)=0
Reset the controller of a given joint, usually sets the current status of the joint as the reference ...
virtual yarp::dev::ReturnValue getPidErrorsRaw(const PidControlTypeEnum &pidtype, double *errs)=0
Get the error of all joints.
virtual yarp::dev::ReturnValue setPidFeedforwardRaw(const PidControlTypeEnum &pidtype, int j, double v)=0
Set the feedforward value for the specified PID controller.
virtual yarp::dev::ReturnValue getPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double *limit)=0
Get the error limit for the controller on a specific joint.
virtual yarp::dev::ReturnValue setPidOffsetRaw(const PidControlTypeEnum &pidtype, int j, double v)=0
Set an offset value for a given controller.
virtual yarp::dev::ReturnValue setPidsRaw(const PidControlTypeEnum &pidtype, const Pid *pids)=0
Set new PID value on multiple axes.
yarp::dev::ReturnValue setPidErrorLimit(const PidControlTypeEnum &pidtype, int j, double limit) override
Set the error limit for the controller on a specific joint.
yarp::dev::ReturnValue setPidOffset(const PidControlTypeEnum &pidtype, int j, double v) override
Set an offset value for a given controller.
yarp::dev::ReturnValue getPidErrorLimit(const PidControlTypeEnum &pidtype, int j, double *ref) override
Get the error limit for the controller on a specific joint.
yarp::dev::ReturnValue setPid(const PidControlTypeEnum &pidtype, int j, const Pid &pid) override
Set new PID value for a joint axis.
yarp::dev::ReturnValue setPidErrorLimits(const PidControlTypeEnum &pidtype, const double *limits) override
Get the error limit for the controller on all joints.
std::vector< double > m_buffer_doubles
bool uninitialize()
Clean up internal data and memory.
yarp::dev::ReturnValue getPidOutput(const PidControlTypeEnum &pidtype, int j, double *out) override
Get the output of the controller (e.g.
yarp::dev::ReturnValue getPidErrors(const PidControlTypeEnum &pidtype, double *errs) override
Get the error of all joints.
yarp::dev::ReturnValue setPidReferences(const PidControlTypeEnum &pidtype, const double *refs) override
Set the controller reference, multiple axes.
yarp::dev::ReturnValue setPids(const PidControlTypeEnum &pidtype, const Pid *pids) override
Set new PID value on multiple axes.
bool initialize(int size, const int *amap, const double *enc, const double *zos, const double *newtons, const double *amps, const double *dutys)
Initialize the internal data and alloc memory.
yarp::dev::ReturnValue getPidError(const PidControlTypeEnum &pidtype, int j, double *err) override
Get the current error for a joint.
ImplementPidControl(yarp::dev::IPidControlRaw *y)
yarp::dev::ReturnValue getPids(const PidControlTypeEnum &pidtype, Pid *pids) override
Get current PID value for a specific joint.
yarp::dev::ReturnValue isPidEnabled(const PidControlTypeEnum &pidtype, int j, bool &enabled) override
Get the current status (enabled/disabled) of the PID.
yarp::dev::ReturnValue getPidReferences(const PidControlTypeEnum &pidtype, double *refs) override
Get the current reference of all PID controllers.
yarp::dev::ReturnValue getPidExtraInfos(const PidControlTypeEnum &pidtype, std::vector< yarp::dev::PidExtraInfo > &units) override
Get PID extra info and measurement units for a specific joint.
yarp::dev::ReturnValue setPidReference(const PidControlTypeEnum &pidtype, int j, double ref) override
Set the controller reference for a given axis.
yarp::dev::ReturnValue disablePid(const PidControlTypeEnum &pidtype, int j) override
Disable the PID computation for a joint.
yarp::dev::ReturnValue getPidOutputs(const PidControlTypeEnum &pidtype, double *outs) override
Get the output of the controllers (e.g.
yarp::dev::ReturnValue getPidErrorLimits(const PidControlTypeEnum &pidtype, double *refs) override
Get the error limit for all controllers.
yarp::dev::ReturnValue setPidFeedforward(const PidControlTypeEnum &pidtype, int j, double v) override
Set the feedforward value for the specified PID controller.
yarp::dev::ReturnValue getPidOffset(const PidControlTypeEnum &pidtype, int j, double &v) override
Get the currently set offset value for the specified PID controller.
yarp::dev::ReturnValue getPid(const PidControlTypeEnum &pidtype, int j, Pid *pid) override
Get current PID value for a specific joint.
yarp::dev::ReturnValue getAvailablePids(int j, std::vector< PidControlTypeEnum > &avail) override
Return a list of available pid types mode for the given joint.
yarp::dev::ReturnValue getPidExtraInfo(const PidControlTypeEnum &pidtype, int j, yarp::dev::PidExtraInfo &units) override
Get PID extra info and measurement units for a specific joint.
yarp::dev::ReturnValue resetPid(const PidControlTypeEnum &pidtype, int j) override
Reset the controller of a given joint, usually sets the current status of the joint as the reference ...
yarp::dev::ReturnValue enablePid(const PidControlTypeEnum &pidtype, int j) override
Enable the PID computation for a joint.
yarp::dev::ReturnValue getPidReference(const PidControlTypeEnum &pidtype, int j, double *ref) override
Get the current reference of the PID controller for a specific joint.
yarp::dev::ReturnValue getPidFeedforward(const PidControlTypeEnum &pidtype, int j, double &v) override
Get the currently set feedforward value for the specified PID controller.
bool setConversionUnits(const PidControlTypeEnum &pidtype, const PidFeedbackUnitsEnum fbk_conv_units, const PidOutputUnitsEnum out_conv_units)
Contains the parameters for a PID.
A mini-server for performing network communication in the background.
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
PidOutputUnitsEnum
Definition PidEnums.h:64
PidControlTypeEnum
Definition PidEnums.h:15
PidFeedbackUnitsEnum
Definition PidEnums.h:58
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13