YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ImplementPositionDirect.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.ImplementPositionDirect")
19}
20
22 m_iraw(y),
23 m_helper(nullptr)
24{;}
25
26
31
32bool ImplementPositionDirect::initialize(int size, const int *amap, const double *enc, const double *zos)
33{
34 if (m_helper != nullptr) {
35 return false;
36 }
37
38 m_helper=(void *)(new ControlBoardHelper(size, amap, enc, zos));
39 yAssert(m_helper != nullptr);
40
41 m_buffer_ints.resize (size);
42 m_buffer_doubles.resize(size);
43
44 return true;
45}
46
48{
49 if(m_helper!=nullptr)
50 {
51 delete castToMapper(m_helper);
52 m_helper=nullptr;
53 }
54
55 return true;
56}
57
59{
60 std::lock_guard lock(m_imp_mutex);
61
62 (axes)=castToMapper(m_helper)->axes();
63 return ReturnValue_ok;
64}
65
67{
68 std::lock_guard lock(m_imp_mutex);
70
71 int k;
72 double enc;
73 castToMapper(m_helper)->posA2E(ref, j, enc, k);
74 return m_iraw->setPositionRaw(k, enc);
75}
76
77ReturnValue ImplementPositionDirect::setPositions(const int n_joints, const int *joints, const double *refs)
78{
79 std::lock_guard lock(m_imp_mutex);
83
84 for(int idx=0; idx<n_joints; idx++)
85 {
88 }
89
90 ReturnValue ret = m_iraw->setPositionsRaw(n_joints, m_buffer_ints.data(), m_buffer_doubles.data());
91
92 return ret;
93}
94
96{
97 std::lock_guard lock(m_imp_mutex);
99
102
103 return ret;
104}
105
107{
108 std::lock_guard lock(m_imp_mutex);
111
112 int k;
113 double tmp;
115
117
118 *ref=(castToMapper(m_helper)->posE2A(tmp, k));
119 return ret;
120}
121
122ReturnValue ImplementPositionDirect::getRefPositions(const int n_joints, const int* joints, double* refs)
123{
124 std::lock_guard lock(m_imp_mutex);
128
129 for(int idx=0; idx<n_joints; idx++)
130 {
132 }
133
134 ReturnValue ret = m_iraw->getRefPositionsRaw(n_joints, m_buffer_ints.data(), m_buffer_doubles.data());
135
136 for(int idx=0; idx<n_joints; idx++)
137 {
139 }
140
141 return ret;
142}
143
145{
146 std::lock_guard lock(m_imp_mutex);
148
151
152 return ret;
153}
yarp::dev::ControlBoardHelper * castToMapper(void *p)
#define yAssert(x)
Definition Log.h:388
#define ReturnValue_ok
Definition ReturnValue.h:82
void posE2A(double enc, int j, double &ang, 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 setPositionRaw(int j, double ref)=0
Set new position for a single axis.
virtual yarp::dev::ReturnValue getRefPositionsRaw(double *refs)=0
Get the last position reference for all axes.
virtual yarp::dev::ReturnValue setPositionsRaw(const int n_joint, const int *joints, const double *refs)=0
Set new reference point for all axes.
virtual yarp::dev::ReturnValue getRefPositionRaw(const int joint, double *ref)=0
Get the last position reference for the specified axis.
ImplementPositionDirect(yarp::dev::IPositionDirectRaw *y)
Constructor.
bool uninitialize()
Clean up internal data and memory.
yarp::dev::ReturnValue getRefPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue getRefPositions(double *refs) override
Get the last position reference for all axes.
yarp::dev::ReturnValue setPositions(const int n_joint, const int *joints, const double *refs) override
Set new reference point for all axes.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
yarp::dev::ReturnValue getAxes(size_t &axes) override
Get the number of controlled axes.
yarp::dev::ReturnValue setPosition(int j, double ref) override
Set new position for a single axis.
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