YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ControlBoardCouplingHandler.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
8
9#include <yarp/os/Log.h>
10#include <yarp/os/LogStream.h>
11
12#include <algorithm>
13#include <iostream>
14#include <map>
15#include <mutex>
16#include <cassert>
17
18using namespace yarp::os;
19using namespace yarp::dev;
20using namespace yarp::sig;
21
23{
24 return detach();
25}
26
28{
29
30 if(!parseParams(config))
31 {
32 yCError(CONTROLBOARDCOUPLINGHANDLER) << "Error in parsing parameters";
33 return false;
34 }
35
37 joint_coupling_config.fromString(config.toString());
38 joint_coupling_config.unput("device"); // remove the device parameter from the config otherwise we have recursion
40 if(!jointCouplingHandler.open(joint_coupling_config)) {
41 yCError(CONTROLBOARDCOUPLINGHANDLER) << "Error in opening jointCouplingHandler device";
42 return false;
43 }
44
45 if(!jointCouplingHandler.view(iJntCoupling)) {
46 yCError(CONTROLBOARDCOUPLINGHANDLER) << "Error viewing the IJointCoupling interface";
47 return false;
48 }
49
50 configureBuffers();
51
52 return true;
53}
54
56{
57 // For both cases, now configure everything that need
58 // all the attribute to be correctly configured
59 bool ok {false};
60
61 if (!poly->isValid()) {
62 yCError(CONTROLBOARDCOUPLINGHANDLER) << "Device " << poly << " to attach to is not valid ... cannot proceed";
63 return false;
64 }
65
66 ok = poly->view(iJntEnc);
67 ok = ok && poly->view(iAxInfo);
68
69 if (!ok)
70 {
71 yCError(CONTROLBOARDCOUPLINGHANDLER, "attachAll failed some subdevice was not found or its attach failed");
72 return false;
73 }
74
75 return ok;
76}
77
78void ControlBoardCouplingHandler::configureBuffers() {
79 if (!iJntCoupling) {
80 yCError(CONTROLBOARDCOUPLINGHANDLER) << "IJointCoupling interface not available";
81 return;
82 }
83 size_t nrOfPhysicalJoints;
84 size_t nrOfActuatedAxes;
85 auto ok = iJntCoupling->getNrOfPhysicalJoints(nrOfPhysicalJoints);
86 ok = ok && iJntCoupling->getNrOfActuatedAxes(nrOfActuatedAxes);
87 if(!ok)
88 {
89 yCError(CONTROLBOARDCOUPLINGHANDLER) << "Error in getting the number of physical joints or actuated axes";
90 return;
91 }
92 physJointsPos.resize(nrOfPhysicalJoints);
93 physJointsVel.resize(nrOfPhysicalJoints);
94 physJointsAcc.resize(nrOfPhysicalJoints);
95 physJointsTime.resize(nrOfPhysicalJoints);
96 actAxesPos.resize(nrOfActuatedAxes);
97 actAxesVel.resize(nrOfActuatedAxes);
98 actAxesAcc.resize(nrOfActuatedAxes);
99 actAxesTime.resize(nrOfActuatedAxes);
100 return;
101}
102
104{
105 iJntEnc = nullptr;
106 iAxInfo = nullptr;
107 iJntCoupling = nullptr;
108 jointCouplingHandler.close();
109 return true;
110}
111
112
116
117
118/* IEncoders */
123
128
133
138
140{
141 ReturnValue ok = ReturnValue::return_code::return_value_error_generic;
142 if (iJntEnc && iJntCoupling) {
143 ok = iJntEnc->getEncoders(actAxesPos.data());
144 ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsPos(actAxesPos, physJointsPos);
145 if (ok) {
146 *v = physJointsPos[j];
147 return ok;
148 }
149 }
150 return ok;
151}
152
154{
155 ReturnValue ok = ReturnValue::return_code::return_value_error_generic;
156 if (iJntEnc && iJntCoupling) {
157 ok = iJntEnc->getEncoders(actAxesPos.data());
158 ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsPos(actAxesPos, physJointsPos);
159 if (ok) {
160 std::copy(physJointsPos.begin(), physJointsPos.end(), encs);
161 return ok;
162 }
163 }
164 return ok;
165}
166
168{
169 ReturnValue ok = ReturnValue::return_code::return_value_error_generic;
170 if (iJntEnc && iJntCoupling) {
171 // TODO t has to be probably resized
172 ok = iJntEnc->getEncodersTimed(actAxesPos.data(), actAxesTime.data());
173 ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsPos(actAxesPos, physJointsPos);
174 if (ok) {
175 std::copy(physJointsPos.begin(), physJointsPos.end(), encs);
176 for(size_t i = 0; i < physJointsTime.size(); i++)
177 {
178 //TODO check if this is the correct way to take the time
179 t[i] = actAxesTime[0];
180 }
181 return ok;
182 }
183 }
184 return ok;
185}
186
188{
189 ReturnValue ok = ReturnValue::return_code::return_value_error_generic;
190 if (iJntEnc && iJntCoupling) {
191 ok = iJntEnc->getEncodersTimed(actAxesPos.data(), actAxesTime.data());
192 ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsPos(actAxesPos, physJointsPos);
193 if (ok) {
194 *v = physJointsPos[j];
195 //TODO check if this is the correct way to take the time
196 *t = actAxesTime[0];
197 return ok;
198 }
199 }
200 return ok;
201}
202
204{
205 ReturnValue ok = ReturnValue::return_code::return_value_error_generic;
206 if (iJntEnc && iJntCoupling) {
207 ok = iJntEnc->getEncoders(actAxesPos.data());
208 ok = ok && iJntEnc->getEncoderSpeeds(actAxesVel.data());
209 ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsVel(actAxesPos, actAxesVel, physJointsVel);
210 if (ok) {
211 *sp = physJointsVel[j];
212 return ok;
213 }
214 }
215 return ok;
216}
217
219{
220 ReturnValue ok = ReturnValue::return_code::return_value_error_generic;
221 if (iJntEnc && iJntCoupling) {
222 ok = iJntEnc->getEncoders(actAxesPos.data());
223 ok = ok && iJntEnc->getEncoderSpeeds(actAxesVel.data());
224 ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsVel(actAxesPos, actAxesVel, physJointsVel);
225 if (ok) {
226 std::copy(physJointsVel.begin(), physJointsVel.end(), spds);
227 return ok;
228 }
229 }
230 return ok;
231}
232
234{
235 ReturnValue ok = ReturnValue::return_code::return_value_error_generic;
236 if (iJntEnc && iJntCoupling) {
237 ok = iJntEnc->getEncoders(actAxesPos.data());
238 ok = ok && iJntEnc->getEncoderSpeeds(actAxesVel.data());
239 ok = ok && iJntEnc->getEncoderAccelerations(actAxesAcc.data());
240 ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsAcc(actAxesPos, actAxesVel, actAxesAcc, physJointsAcc);
241 if (ok) {
242 *acc = physJointsAcc[j];
243 return ok;
244 }
245 }
246 return ok;
247}
248
250{
251 ReturnValue ok = ReturnValue::return_code::return_value_error_generic;
252 if (iJntEnc && iJntCoupling) {
253 ok = iJntEnc->getEncoders(actAxesPos.data());
254 ok = ok && iJntEnc->getEncoderSpeeds(actAxesVel.data());
255 ok = ok && iJntEnc->getEncoderAccelerations(actAxesAcc.data());
256 ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsAcc(actAxesPos, actAxesVel, actAxesAcc, physJointsAcc);
257 if (ok) {
258 std::copy(physJointsAcc.begin(), physJointsAcc.end(), accs);
259 return ok;
260 }
261 }
262 return ok;
263}
264
265/* IAxisInfo */
266
268{
269 ReturnValue ok{ReturnValue::return_code::return_value_error_generic};
270 if (iJntCoupling) {
271 size_t nrOfPhysicalJoints {0};
272 ok = iJntCoupling->getNrOfPhysicalJoints(nrOfPhysicalJoints);
273 if (ok) {
275 return ok;
276 }
277 }
278 return ok;
279}
280
282{
283 ReturnValue ok{ReturnValue::return_code::return_value_error_generic};
284 if (iJntCoupling) {
285 ok = iJntCoupling->getPhysicalJointName(j, name);
286 return ok;
287 }
288 return ok;
289}
290
292{
293 ReturnValue ok{ReturnValue::return_code::return_value_error_generic};
294 // TODO I am not sure how to handle this function
295 type = JointTypeEnum::VOCAB_JOINTTYPE_REVOLUTE;
296 return ReturnValue_ok;
297}
const yarp::os::LogComponent & CONTROLBOARDCOUPLINGHANDLER()
#define ReturnValue_ok
Definition ReturnValue.h:82
#define YARP_METHOD_NOT_YET_IMPLEMENTED()
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
yarp::dev::ReturnValue getEncoderTimed(int j, double *v, double *t) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getEncoders(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue getEncoder(int j, double *v) override
Read the value of an encoder.
yarp::dev::ReturnValue getJointType(int j, yarp::dev::JointTypeEnum &type) override
yarp::dev::ReturnValue setEncoders(const double *vals) override
Set the value of all encoders.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
bool close() override
Close the device driver by deallocating all resources and closing ports.
yarp::dev::ReturnValue getEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
yarp::dev::ReturnValue getEncodersTimed(double *encs, double *t) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of an axis.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue resetEncoders() override
Reset encoders.
bool detach() override
Detach the object (you must have first called attach).
yarp::dev::ReturnValue resetEncoder(int j) override
ControlBoard methods.
yarp::dev::ReturnValue getAxisName(int j, std::string &name) override
yarp::dev::ReturnValue getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
yarp::dev::ReturnValue getEncoderAcceleration(int j, double *acc) override
Read the instantaneous acceleration of an axis.
bool open(yarp::os::Searchable &prop) override
Open the device driver.
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getEncodersTimed(double *encs, double *time)=0
Read the instantaneous acceleration of all axes.
virtual yarp::dev::ReturnValue getEncoderAccelerations(double *accs)=0
Read the instantaneous acceleration of all axes.
virtual yarp::dev::ReturnValue getEncoderSpeeds(double *spds)=0
Read the instantaneous speed of all axes.
virtual yarp::dev::ReturnValue getEncoders(double *encs)=0
Read the position of all axes.
A container for a device driver.
Definition PolyDriver.h:23
bool close() override
Close the DeviceDriver.
bool isValid() const
Check if device is valid.
bool open(const std::string &txt)
Construct and configure a device by its common name.
A mini-server for performing network communication in the background.
A class for storing options and configuration information.
Definition Property.h:33
A base class for nested structures that can be searched.
Definition Searchable.h:31
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
void resize(size_t size) override
Resize the vector.
Definition Vector.h:231
size_t size() const
Definition Vector.h:351
iterator begin() noexcept
Returns an iterator to the beginning of the VectorOf.
Definition Vector.h:487
T * data()
Return a pointer to the first element of the vector.
Definition Vector.h:216
iterator end() noexcept
Returns an iterator to the end of the VectorOf.
Definition Vector.h:494
#define yCError(component,...)
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