YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ImplementPositionControl.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
6#include <cstdio>
7
11#include <yarp/os/Log.h>
13
14using namespace yarp::dev;
15using namespace yarp::os;
16
17namespace {
18YARP_LOG_COMPONENT(IMPLEMENT_LAYER_COMPONENT, "yarp.ImplementPositionControl")
19}
20
25
26
31
40bool ImplementPositionControl::initialize(int size, const int *amap, const double *enc, const double *zos)
41{
42 if (m_helper != nullptr) {
43 return false;
44 }
45
46 m_helper=(void *)(new ControlBoardHelper(size, amap, enc, zos));
47 yAssert(m_helper != nullptr);
48
49 m_buffer_doubles.resize(size);
50 m_buffer_ints.resize(size);
51
52 return true;
53}
54
60{
61 if(m_helper != nullptr)
62 {
63 delete castToMapper(m_helper);
64 m_helper = nullptr;
65 }
66
67 return true;
68}
69
71{
72 std::lock_guard lock(m_imp_mutex);
74
75 int k;
76 double enc;
78 return m_iraw->positionMoveRaw(k, enc);
79}
80
81ReturnValue ImplementPositionControl::positionMove(const int n_joints, const int *joints, const double *refs)
82{
83 std::lock_guard lock(m_imp_mutex);
87
88 for(int idx=0; idx<n_joints; idx++)
89 {
92 }
93 ReturnValue ret = m_iraw->positionMoveRaw(n_joints, m_buffer_ints.data(), m_buffer_doubles.data());
94
95 return ret;
96}
97
99{
100 std::lock_guard lock(m_imp_mutex);
102
104
106
107 return ret;
108}
109
111{
112 std::lock_guard lock(m_imp_mutex);
114
115 int k;
116 double enc;
117 castToMapper(m_helper)->velA2E(delta, j, enc, k);
118
119 return m_iraw->relativeMoveRaw(k,enc);
120}
121
122ReturnValue ImplementPositionControl::relativeMove(const int n_joints, const int *joints, const double *deltas)
123{
124 std::lock_guard lock(m_imp_mutex);
128
129 for(int idx=0; idx<n_joints; idx++)
130 {
132 m_buffer_doubles[idx] = castToMapper(m_helper)->velA2E(deltas[idx], joints[idx]);
133 }
134 ReturnValue ret = m_iraw->relativeMoveRaw(n_joints, m_buffer_ints.data(), m_buffer_doubles.data());
135
136 return ret;
137}
138
140{
141 std::lock_guard lock(m_imp_mutex);
143
146
147 return ret;
148}
149
151{
152 std::lock_guard lock(m_imp_mutex);
154
155 int k=castToMapper(m_helper)->toHw(j);
156
157 return m_iraw->checkMotionDoneRaw(k,flag);
158}
159
160ReturnValue ImplementPositionControl::checkMotionDone(const std::vector<int>& joints, bool& flag)
161{
162 std::lock_guard lock(m_imp_mutex);
164
165 for(int idx=0; idx<joints.size(); idx++)
166 {
168 }
170
171 return ret;
172}
173
175{
176 std::lock_guard lock(m_imp_mutex);
177
178 return m_iraw->checkMotionDoneRaw(flag);
179}
180
182{
183 std::lock_guard lock(m_imp_mutex);
185
186 int k;
187 double enc;
189 return m_iraw->setTrajSpeedRaw(k, enc);
190}
191
192ReturnValue ImplementPositionControl::setTrajSpeeds(const int n_joints, const int *joints, const double *spds)
193{
194 std::lock_guard lock(m_imp_mutex);
198
199 for(int idx=0; idx<n_joints; idx++)
200 {
202 }
203 ReturnValue ret = m_iraw->setTrajSpeedsRaw(n_joints, m_buffer_ints.data(), m_buffer_doubles.data());
204
205 return ret;
206}
207
209{
210 std::lock_guard lock(m_imp_mutex);
212
215
216 return ret;
217}
218
220{
221 std::lock_guard lock(m_imp_mutex);
223
224 int k;
225 double enc;
226
229}
230
231ReturnValue ImplementPositionControl::setTrajAccelerations(const int n_joints, const int *joints, const double *accs)
232{
233 std::lock_guard lock(m_imp_mutex);
237
238 for(int idx=0; idx<n_joints; idx++)
239 {
241 }
242
244
245 return ret;
246}
247
249{
250 std::lock_guard lock(m_imp_mutex);
252
254
256
257 return ret;
258}
259
261{
262 std::lock_guard lock(m_imp_mutex);
265
266 int k;
267 double enc;
269
271
273
274 return ret;
275}
276
277ReturnValue ImplementPositionControl::getTrajSpeeds(const int n_joints, const int *joints, double *spds)
278{
279 std::lock_guard lock(m_imp_mutex);
283
284 for(int idx=0; idx<n_joints; idx++)
285 {
287 }
288
289 ReturnValue ret = m_iraw->getTrajSpeedsRaw(n_joints, m_buffer_ints.data(), m_buffer_doubles.data());
290
291 for(int idx=0; idx<n_joints; idx++)
292 {
294 }
295
296 return ret;
297}
298
300{
301 std::lock_guard lock(m_imp_mutex);
303
306
307 return ret;
308}
309
320
321ReturnValue ImplementPositionControl::getTrajAccelerations(const int n_joints, const int *joints, double *accs)
322{
323 std::lock_guard lock(m_imp_mutex);
327
328 for(int idx=0; idx<n_joints; idx++)
329 {
331 }
332
334
335 for(int idx=0; idx<n_joints; idx++)
336 {
338 }
339
340 return ret;
341}
342
344{
345 std::lock_guard lock(m_imp_mutex);
348
349 int k;
350 double enc;
353
355
356 return ret;
357}
358
360{
361 std::lock_guard lock(m_imp_mutex);
363
364 int k;
366
367 return m_iraw->stopRaw(k);
368}
369
371{
372 std::lock_guard lock(m_imp_mutex);
374
375 for(int idx=0; idx<n_joint; idx++)
376 {
378 }
379
381
382 return ret;
383}
384
386{
387 std::lock_guard lock(m_imp_mutex);
388
389 return m_iraw->stopRaw();
390}
391
393{
394 std::lock_guard lock(m_imp_mutex);
395
396 (axis)=castToMapper(m_helper)->axes();
397
398 return ReturnValue_ok;
399}
400
401
403{
404 std::lock_guard lock(m_imp_mutex);
407
408 int k;
409 double enc;
412
414
415 return ret;
416}
417
419{
420 std::lock_guard lock(m_imp_mutex);
422
425
426 return ret;
427}
428
429ReturnValue ImplementPositionControl::getTargetPositions(const int n_joints, const int* joints, double* refs)
430{
431 std::lock_guard lock(m_imp_mutex);
435
436 for(int idx=0; idx<n_joints; idx++)
437 {
439 }
441
442 for(int idx=0; idx<n_joints; idx++)
443 {
445 }
446
447 return ret;
448}
yarp::dev::ControlBoardHelper * castToMapper(void *p)
#define yAssert(x)
Definition Log.h:388
#define ReturnValue_ok
Definition ReturnValue.h:82
void velE2A_abs(double enc, int j, double &ang, int &k)
void velA2E(double ang, int j, double &enc, int &k)
void posE2A(double enc, int j, double &ang, int &k)
void accA2E_abs(double ang, int j, double &enc, int &k)
void accE2A_abs(double enc, int j, double &ang, int &k)
void velA2E_abs(double ang, int j, double &enc, int &k)
void posA2E(double ang, int j, double &enc, int &k)
Interface for a generic control board device implementing position control in encoder coordinates.
virtual yarp::dev::ReturnValue getTrajAccelerationsRaw(double *accs)=0
Get reference acceleration of all joints.
virtual yarp::dev::ReturnValue getTargetPositionRaw(const int joint, double *ref)=0
Get the last position reference for the specified axis.
virtual yarp::dev::ReturnValue getTrajSpeedRaw(int j, double *ref)=0
Get reference speed for a joint.
virtual yarp::dev::ReturnValue stopRaw(int j)=0
Stop motion, single joint.
virtual yarp::dev::ReturnValue setTrajSpeedsRaw(const double *spds)=0
Set reference speed on all joints.
virtual yarp::dev::ReturnValue positionMoveRaw(int j, double ref)=0
Set new reference point for a single axis.
virtual yarp::dev::ReturnValue getTrajAccelerationRaw(int j, double *acc)=0
Get reference acceleration for a joint.
virtual yarp::dev::ReturnValue setTrajSpeedRaw(int j, double sp)=0
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
virtual yarp::dev::ReturnValue checkMotionDoneRaw(int j, bool &flag)=0
Check if the current trajectory is terminated.
virtual yarp::dev::ReturnValue setTrajAccelerationRaw(int j, double acc)=0
Set reference acceleration for a joint.
virtual yarp::dev::ReturnValue getTargetPositionsRaw(double *refs)=0
Get the last position reference for all axes.
virtual yarp::dev::ReturnValue setTrajAccelerationsRaw(const double *accs)=0
Set reference acceleration on all joints.
virtual yarp::dev::ReturnValue getTrajSpeedsRaw(double *spds)=0
Get reference speed of all joints.
virtual yarp::dev::ReturnValue relativeMoveRaw(int j, double delta)=0
Set relative position.
yarp::dev::ReturnValue getTrajAcceleration(int j, double *acc) override
Get reference acceleration for a joint.
yarp::dev::ReturnValue stop() override
Stop motion, multiple joints.
yarp::dev::ReturnValue setTrajSpeeds(const int n_joint, const int *joints, const double *spds) override
Set reference speed on all joints.
yarp::dev::ReturnValue getTargetPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue setTrajAcceleration(int j, double acc) override
Set reference acceleration for a joint.
yarp::dev::ReturnValue relativeMove(int j, double delta) override
Set relative position.
yarp::dev::ReturnValue getAxes(size_t &axis) override
Get the number of controlled axes.
yarp::dev::ReturnValue checkMotionDone(bool &flag) override
Check if the current trajectory is terminated.
yarp::dev::ReturnValue getTrajSpeed(int j, double *ref) override
Get reference speed for a joint.
ImplementPositionControl(yarp::dev::IPositionControlRaw *y)
Constructor.
yarp::dev::ReturnValue getTrajAccelerations(const int n_joint, const int *joints, double *accs) override
Get reference acceleration for a joint.
yarp::dev::ReturnValue positionMove(int j, double ref) override
Set new reference point for a single axis.
yarp::dev::ReturnValue setTrajSpeed(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
yarp::dev::ReturnValue setTrajAccelerations(const int n_joint, const int *joints, const double *accs) override
Set reference acceleration on all joints.
yarp::dev::ReturnValue getTrajSpeeds(const int n_joint, const int *joints, double *spds) override
Get reference speed of all joints.
yarp::dev::ReturnValue getTargetPositions(double *refs) override
Get the last position reference for all axes.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
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.
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13