YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ImplementVelocityControl.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.ImplementVelocityControl")
19}
20
22 m_iraw(y),
23 m_helper(nullptr)
24{;}
25
30
31bool ImplementVelocityControl::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
62
64{
65 std::lock_guard lock(m_imp_mutex);
67
68 int k;
69 double enc;
70 castToMapper(m_helper)->velA2E(sp, j, enc, k);
71 return m_iraw->velocityMoveRaw(k, enc);
72}
73
74ReturnValue ImplementVelocityControl::velocityMove(const int n_joints, const int *joints, const double *spds)
75{
76 std::lock_guard lock(m_imp_mutex);
80
81 std::vector<int> vectorInt_tmp(n_joints);
82 std::vector<double> vectorDouble_tmp(n_joints);
83
84 for(int idx=0; idx<n_joints; idx++)
85 {
88 }
89 ReturnValue ret = m_iraw->velocityMoveRaw(n_joints, vectorInt_tmp.data(), vectorDouble_tmp.data());
90
91 return ret;
92}
93
95{
96 std::lock_guard lock(m_imp_mutex);
98
101 return ret;
102}
103
105{
106 std::lock_guard lock(m_imp_mutex);
109
110 int k;
111 double tmp;
115 return ret;
116}
117
127
128ReturnValue ImplementVelocityControl::getTargetVelocities(const int n_joints, const int *joints, double *vels)
129{
130 std::lock_guard lock(m_imp_mutex);
134
135 std::vector<int> vectorInt_tmp(n_joints);
136 std::vector<double> vectorDouble_tmp(n_joints);
137
138 for(int idx=0; idx<n_joints; idx++)
139 {
141 }
142
144
145 for(int idx=0; idx<n_joints; idx++)
146 {
148 }
149
150 return ret;
151}
152
154{
155 std::lock_guard lock(m_imp_mutex);
157
158 int k;
159 double enc;
162}
163
164ReturnValue ImplementVelocityControl::setTrajAccelerations(const int n_joints, const int *joints, const double *accs)
165{
166 std::lock_guard lock(m_imp_mutex);
170
171 std::vector<int> vectorInt_tmp(n_joints);
172 std::vector<double> vectorDouble_tmp(n_joints);
173
174 for(int idx=0; idx<n_joints; idx++)
175 {
177 }
179
180 return ret;
181}
182
184{
185 std::lock_guard lock(m_imp_mutex);
187
190 return ret;
191}
192
194{
195 std::lock_guard lock(m_imp_mutex);
198
199 int k;
200 double enc;
204 return ret;
205}
206
207ReturnValue ImplementVelocityControl::getTrajAccelerations(const int n_joints, const int *joints, double *accs)
208{
209 std::lock_guard lock(m_imp_mutex);
213
214 std::vector<int> vectorInt_tmp(n_joints);
215 std::vector<double> vectorDouble_tmp(n_joints);
216
217 for(int idx=0; idx<n_joints; idx++)
218 {
220 }
221
223
224 for(int idx=0; idx<n_joints; idx++)
225 {
227 }
228
229 return ret;
230}
231
232
242
243
245{
246 std::lock_guard lock(m_imp_mutex);
248
249 int k;
251 return m_iraw->stopRaw(k);
252}
253
254
255ReturnValue ImplementVelocityControl::stop(const int n_joints, const int *joints)
256{
257 std::lock_guard lock(m_imp_mutex);
260
261 for(int idx=0; idx<n_joints; idx++)
262 {
264 }
265 ReturnValue ret = m_iraw->stopRaw(n_joints, m_buffer_ints.data());
266 return ret;
267}
268
269
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)
void accA2E_abs(double ang, int j, double &enc, int &k)
void accE2A_abs(double enc, int j, double &ang, int &k)
Interface for control boards implementing velocity control in encoder coordinates.
virtual yarp::dev::ReturnValue setTrajAccelerationsRaw(const double *accs)=0
Set reference acceleration on all joints.
virtual yarp::dev::ReturnValue stopRaw(int j)=0
Stop motion, single joint.
virtual yarp::dev::ReturnValue setTrajAccelerationRaw(int j, double acc)=0
Set reference acceleration for a joint.
virtual yarp::dev::ReturnValue getTargetVelocityRaw(const int joint, double *vel)=0
Get the last reference speed set by velocityMove for single joint.
virtual yarp::dev::ReturnValue getTargetVelocitiesRaw(double *vels)=0
Get the last reference speed set by velocityMove for all joints.
virtual yarp::dev::ReturnValue getTrajAccelerationsRaw(double *accs)=0
Get reference acceleration of all joints.
virtual yarp::dev::ReturnValue getTrajAccelerationRaw(int j, double *acc)=0
Get reference acceleration for a joint.
virtual yarp::dev::ReturnValue velocityMoveRaw(int j, double sp)=0
Start motion at a given speed, single joint.
yarp::dev::ReturnValue getTargetVelocity(const int joint, double *vel) override
Get the last reference speed set by velocityMove for single joint.
ImplementVelocityControl(yarp::dev::IVelocityControlRaw *y)
Constructor.
yarp::dev::ReturnValue stop() override
Stop motion, multiple joints.
yarp::dev::ReturnValue getTrajAcceleration(int j, double *acc) override
Get reference acceleration for a joint.
bool uninitialize()
Clean up internal data and memory.
yarp::dev::ReturnValue velocityMove(int j, double sp) override
Start motion at a given speed, 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.
yarp::dev::ReturnValue getTargetVelocities(double *vels) override
Get the last reference speed set by velocityMove for all joints.
yarp::dev::ReturnValue setTrajAcceleration(int j, double acc) override
Set reference acceleration for a joint.
yarp::dev::ReturnValue setTrajAccelerations(const double *accs) override
Set reference acceleration on all joints.
yarp::dev::ReturnValue getTrajAccelerations(double *accs) override
Get reference acceleration of all joints.
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.