YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ImplementVelocityDirect.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2025-2025 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.ImplementVelocityDirect")
19}
20
21
25
30
31bool ImplementVelocityDirect::initialize(int size, const int* axis_map, const double* enc, const double* zeros)
32{
33 if (m_helper != nullptr) {
34 return false;
35 }
36
37 m_helper = (void*)(new ControlBoardHelper(size, axis_map, enc, zeros));
38 yAssert(m_helper != nullptr);
39
40 m_buffer_ints.resize(size);
41 m_buffer_doubles.resize(size);
42
43 return true;
44}
45
47{
48 if (m_helper != nullptr)
49 {
50 delete castToMapper(m_helper);
51 m_helper = nullptr;
52 }
53
54 return true;
55}
56
58{
59 std::lock_guard lock(m_imp_mutex);
60
61 axes = castToMapper(m_helper)->axes();
62 return ReturnValue_ok;
63}
64
66{
67 std::lock_guard lock(m_imp_mutex);
69
70 int k;
71 double enc;
72 castToMapper(m_helper)->velA2E(sp, j, enc, k);
74}
75
85
86yarp::dev::ReturnValue ImplementVelocityDirect::setRefVelocity(const std::vector<int>& joints, const std::vector<double>& vels)
87{
88 std::lock_guard lock(m_imp_mutex);
90
91 std::vector<int> vectorInt_tmp(joints.size());
92 std::vector<double> vectorDouble_tmp(joints.size());
93
94 for (size_t idx = 0; idx < joints.size(); idx++) {
97 }
98
100 return ret;
101}
102
104{
105 std::lock_guard lock(m_imp_mutex);
107
108 int k = castToMapper(m_helper)->toHw(j);
109 double tmp;
111 vel = castToMapper(m_helper)->velE2A(tmp, k);
112 return ret;
113}
114
116{
117 std::lock_guard lock(m_imp_mutex);
119
120 size_t axes = castToMapper(m_helper)->axes();
123 return ret;
124}
125
126yarp::dev::ReturnValue ImplementVelocityDirect::getRefVelocity(const std::vector<int>& joints, std::vector<double>& vels)
127{
128 std::lock_guard lock(m_imp_mutex);
131
132 std::vector<int> vectorInt_tmp(joints.size());
133 std::vector<double> vectorDouble_tmp(joints.size());
134
135 for (size_t idx = 0; idx < joints.size(); idx++) {
137 }
138
140
141 for (size_t idx = 0; idx < joints.size(); idx++) {
143 }
144
145 return ret;
146}
yarp::dev::ControlBoardHelper * castToMapper(void *p)
#define yAssert(x)
Definition Log.h:388
#define ReturnValue_ok
Definition ReturnValue.h:82
void velE2A(double enc, int j, double &ang, int &k)
void velA2E(double ang, int j, double &enc, int &k)
Interface for control boards implementing direct velocity control.
virtual yarp::dev::ReturnValue getRefVelocityRaw(const int jnt, double &vel)=0
Get the last reference velocity set by setRefVelocity() for a single joint.
virtual yarp::dev::ReturnValue setRefVelocityRaw(int jnt, double vel)=0
Set the velocity of single joint.
bool uninitialize()
Clean up internal data and memory.
yarp::dev::ReturnValue setRefVelocity(int jnt, double vel) override
Set the velocity of single joint.
ImplementVelocityDirect(yarp::dev::IVelocityDirectRaw *y)
Constructor.
yarp::dev::ReturnValue getRefVelocity(const int jnt, double &vel) override
Get the last reference velocity set by setRefVelocity() for a single joint.
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.
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