yarp-devices
Loading...
Searching...
No Matches
CanBusBroker.hpp
1// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2
3#ifndef __CAN_BUS_BROKER_HPP__
4#define __CAN_BUS_BROKER_HPP__
5
6#include <vector>
7
8#include <yarp/dev/ControlBoardInterfaces.h>
9#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
10
11#include "DeviceMapper.hpp"
12#include "SingleBusBroker.hpp"
13#include "SyncPeriodicThread.hpp"
14
15#define CHECK_JOINT(j) do { int n = deviceMapper.getControlledAxes(); if ((j) < 0 || (j) > n - 1) return false; } while (0)
16
17namespace roboticslab
18{
19
40class CanBusBroker : public yarp::dev::DeviceDriver,
41 // control board interfaces
42 public yarp::dev::IAmplifierControl,
43 public yarp::dev::IAxisInfo,
44 public yarp::dev::IControlCalibration,
45 public yarp::dev::IControlLimits,
46 public yarp::dev::IControlMode,
47 public yarp::dev::ICurrentControl,
48 public yarp::dev::IEncodersTimed,
49 public yarp::dev::IImpedanceControl,
50 public yarp::dev::IInteractionMode,
51 public yarp::dev::IJointFault,
52 public yarp::dev::IMotor,
53 public yarp::dev::IMotorEncoders,
54 public yarp::dev::IPidControl,
55 public yarp::dev::IPositionControl,
56 public yarp::dev::IPositionDirect,
57 public yarp::dev::IPWMControl,
58 public yarp::dev::IRemoteVariables,
59 public yarp::dev::ITorqueControl,
60 public yarp::dev::IVelocityControl,
61 // multiple analog sensors interfaces
62 public yarp::dev::IContactLoadCellArrays,
63 public yarp::dev::IEncoderArrays,
64 public yarp::dev::IOrientationSensors,
65 public yarp::dev::IPositionSensors,
66 public yarp::dev::ISixAxisForceTorqueSensors,
67 public yarp::dev::ISkinPatches,
68 public yarp::dev::ITemperatureSensors,
69 public yarp::dev::IThreeAxisGyroscopes,
70 public yarp::dev::IThreeAxisLinearAccelerometers,
71 public yarp::dev::IThreeAxisMagnetometers
72{
73public:
74 ~CanBusBroker() override
75 { close(); }
76
77 // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------
78
79 bool open(yarp::os::Searchable & config) override;
80 bool close() override;
81
82 // ---------- CONTROL BOARD INTERFACES ----------
83
84 // --------- IAmplifierControl declarations. Implementation in IAmplifierControlImpl.cpp ---------
85
86 bool enableAmp(int j) override;
87 bool disableAmp(int j) override;
88 bool getAmpStatus(int j, int * v) override;
89 bool getAmpStatus(int * st) override;
90 //bool getCurrent(int j, double * val) override;
91 //bool getCurrents(double * vals) override;
92 bool getMaxCurrent(int j, double * v) override;
93 bool setMaxCurrent(int j, double v) override;
94 bool getNominalCurrent(int m, double * val) override;
95 bool setNominalCurrent(int m, double val) override;
96 bool getPeakCurrent(int m, double * val) override;
97 bool setPeakCurrent(int m, double val) override;
98 bool getPWM(int j, double * val) override;
99 bool getPWMLimit(int j, double * val) override;
100 bool setPWMLimit(int j, double val) override;
101 bool getPowerSupplyVoltage(int j, double * val) override;
102
103 // --------- IAxisInfo declarations. Implementation in IAxisInfoImpl.cpp ---------
104
105 bool getAxisName(int axis, std::string & name) override;
106 bool getJointType(int axis, yarp::dev::JointTypeEnum & type) override;
107
108 // --------- IControlCalibration declarations. Implementation in IControlCalibrationImpl.cpp ---------
109
110 bool calibrateAxisWithParams(int axis, unsigned int type, double p1, double p2, double p3) override;
111 bool setCalibrationParameters(int axis, const yarp::dev::CalibrationParameters & params) override;
112 bool calibrationDone(int j) override;
113
114 // --------- IControlLimits declarations. Implementation in IControlLimitsImpl.cpp ---------
115
116 bool setLimits(int axis, double min, double max) override;
117 bool getLimits(int axis, double * min, double * max) override;
118 bool setVelLimits(int axis, double min, double max) override;
119 bool getVelLimits(int axis, double * min, double * max) override;
120
121 // --------- IControlMode declarations. Implementation in IControlModeImpl.cpp ---------
122
123 bool getControlMode(int j, int * mode) override;
124 bool getControlModes(int * modes) override;
125 bool getControlModes(int n_joint, const int * joints, int * modes) override;
126 bool setControlMode(int j, const int mode) override;
127 bool setControlModes(int n_joint, const int * joints, int * modes) override;
128 bool setControlModes(int * modes) override;
129
130 // --------- ICurrentControl declarations. Implementation in ICurrentControlImpl.cpp ---------
131
132 //bool getNumberOfMotors(int * ax) override;
133 bool getCurrent(int m, double * curr) override;
134 bool getCurrents(double * currs) override;
135 bool getCurrentRange(int m, double * min, double * max) override;
136 bool getCurrentRanges(double * mins, double * maxs) override;
137 bool setRefCurrent(int m, double curr) override;
138 bool setRefCurrents(const double * currs) override;
139 bool setRefCurrents(int n_motor, const int * motors, const double * currs) override;
140 bool getRefCurrent(int m, double * curr) override;
141 bool getRefCurrents(double * currs) override;
142
143 // ---------- IEncoders declarations. Implementation in IEncodersImpl.cpp ----------
144
145 bool getAxes(int * ax) override;
146 bool resetEncoder(int j) override;
147 bool resetEncoders() override;
148 bool setEncoder(int j, double val) override;
149 bool setEncoders(const double * vals) override;
150 bool getEncoder(int j, double * v) override;
151 bool getEncoders(double * encs) override;
152 bool getEncoderSpeed(int j, double * spd) override;
153 bool getEncoderSpeeds(double * spds) override;
154 bool getEncoderAcceleration(int j, double * spds) override;
155 bool getEncoderAccelerations(double * accs) override;
156
157 // ---------- IEncodersTimed declarations. Implementation in IEncodersImpl.cpp ----------
158
159 bool getEncoderTimed(int j, double * encs, double * time) override;
160 bool getEncodersTimed(double * encs, double * times) override;
161
162 // --------- IImpedanceControl declarations. Implementation in IImpedanceControlImpl.cpp ---------
163
164 //bool getAxes(int * ax) override;
165 bool getImpedance(int j, double * stiffness, double * damping) override;
166 bool setImpedance(int j, double stiffness, double damping) override;
167 bool setImpedanceOffset(int j, double offset) override;
168 bool getImpedanceOffset(int j, double * offset) override;
169 bool getCurrentImpedanceLimit(int j, double * min_stiff, double * max_stiff, double * min_damp, double * max_damp) override;
170
171 // -----------IInteractionMode declarations. Implementation in IInteractionModeImpl.cpp --------------
172
173 bool getInteractionMode(int axis, yarp::dev::InteractionModeEnum * mode) override;
174 bool getInteractionModes(yarp::dev::InteractionModeEnum * modes) override;
175 bool getInteractionModes(int n_joints, int * joints, yarp::dev::InteractionModeEnum * modes) override;
176 bool setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode) override;
177 bool setInteractionModes(yarp::dev::InteractionModeEnum * modes) override;
178 bool setInteractionModes(int n_joints, int * joints, yarp::dev::InteractionModeEnum * modes) override;
179
180 // --------- IJointFault declarations. Implementation in IJointFaultImpl.cpp ---------
181
182 bool getLastJointFault(int j, int & fault, std::string & message) override;
183
184 // --------- IMotor declarations. Implementation in IMotorImpl.cpp ---------
185
186 bool getNumberOfMotors(int * num) override;
187 bool getTemperature(int m, double * val) override;
188 bool getTemperatures(double * vals) override;
189 bool getTemperatureLimit(int m, double * temp) override;
190 bool setTemperatureLimit(int m, double temp) override;
191 bool getGearboxRatio(int m, double * val) override;
192 bool setGearboxRatio(int m, double val) override;
193
194 // --------- IMotorEncoders declarations. Implementation in IMotorEncodersImpl.cpp ---------
195
196 bool getNumberOfMotorEncoders(int * num) override;
197 bool resetMotorEncoder(int m) override;
198 bool resetMotorEncoders() override;
199 bool setMotorEncoderCountsPerRevolution(int m, double cpr) override;
200 bool getMotorEncoderCountsPerRevolution(int m, double * cpr) override;
201 bool setMotorEncoder(int m, double val) override;
202 bool setMotorEncoders(const double * vals) override;
203 bool getMotorEncoder(int m, double * v) override;
204 bool getMotorEncoders(double * encs) override;
205 bool getMotorEncoderTimed(int m, double * enc, double * stamp) override;
206 bool getMotorEncodersTimed(double * encs, double * stamps) override;
207 bool getMotorEncoderSpeed(int m, double * sp) override;
208 bool getMotorEncoderSpeeds(double *spds) override;
209 bool getMotorEncoderAcceleration(int m, double * acc) override;
210 bool getMotorEncoderAccelerations(double * accs) override;
211
212 // --------- IPidControl declarations. Implementation in IPidControlImpl.cpp ---------
213
214 bool setPid(const yarp::dev::PidControlTypeEnum & pidtype, int j, const yarp::dev::Pid & pid) override;
215 bool setPids(const yarp::dev::PidControlTypeEnum & pidtype, const yarp::dev::Pid * pids) override;
216 bool setPidReference(const yarp::dev::PidControlTypeEnum & pidtype, int j, double ref) override;
217 bool setPidReferences(const yarp::dev::PidControlTypeEnum & pidtype, const double * refs) override;
218 bool setPidErrorLimit(const yarp::dev::PidControlTypeEnum & pidtype, int j, double limit) override;
219 bool setPidErrorLimits(const yarp::dev::PidControlTypeEnum & pidtype, const double * limits) override;
220 bool getPidError(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * err) override;
221 bool getPidErrors(const yarp::dev::PidControlTypeEnum & pidtype, double * errs) override;
222 bool getPidOutput(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * out) override;
223 bool getPidOutputs(const yarp::dev::PidControlTypeEnum & pidtype, double * outs) override;
224 bool getPid(const yarp::dev::PidControlTypeEnum & pidtype, int j, yarp::dev::Pid * pid) override;
225 bool getPids(const yarp::dev::PidControlTypeEnum & pidtype, yarp::dev::Pid * pids) override;
226 bool getPidReference(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * ref) override;
227 bool getPidReferences(const yarp::dev::PidControlTypeEnum & pidtype, double * refs) override;
228 bool getPidErrorLimit(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * limit) override;
229 bool getPidErrorLimits(const yarp::dev::PidControlTypeEnum & pidtype, double * limits) override;
230 bool resetPid(const yarp::dev::PidControlTypeEnum & pidtype, int j) override;
231 bool disablePid(const yarp::dev::PidControlTypeEnum & pidtype, int j) override;
232 bool enablePid(const yarp::dev::PidControlTypeEnum & pidtype, int j) override;
233 bool setPidOffset(const yarp::dev::PidControlTypeEnum & pidtype, int j, double v) override;
234 bool isPidEnabled(const yarp::dev::PidControlTypeEnum & pidtype, int j, bool * enabled) override;
235
236 // ------- IPositionControl declarations. Implementation in IPositionControlImpl.cpp -------
237
238 //bool getAxes(int * ax) override;
239 bool positionMove(int j, double ref) override;
240 bool positionMove(const double * refs) override;
241 bool positionMove(int n_joint, const int * joints, const double * refs) override;
242 bool relativeMove(int j, double delta) override;
243 bool relativeMove(const double * deltas) override;
244 bool relativeMove(int n_joint, const int * joints, const double * deltas) override;
245 bool checkMotionDone(int j, bool * flag) override;
246 bool checkMotionDone(bool * flag) override;
247 bool checkMotionDone(int n_joint, const int * joints, bool * flag) override;
248 bool setRefSpeed(int j, double sp) override;
249 bool setRefSpeeds(const double * spds) override;
250 bool setRefSpeeds(int n_joint, const int * joints, const double * spds) override;
251 bool setRefAcceleration(int j, double acc) override;
252 bool setRefAccelerations(const double * accs) override;
253 bool setRefAccelerations(int n_joint, const int * joints, const double * accs) override;
254 bool getRefSpeed(int j, double * spd) override;
255 bool getRefSpeeds(double * spds) override;
256 bool getRefSpeeds(int n_joint, const int * joints, double * spds) override;
257 bool getRefAcceleration(int j, double * acc) override;
258 bool getRefAccelerations(double * accs) override;
259 bool getRefAccelerations(int n_joint, const int * joints, double * accs) override;
260 bool stop(int j) override;
261 bool stop() override;
262 bool stop(int n_joint, const int *joints) override;
263 bool getTargetPosition(int joint, double * ref) override;
264 bool getTargetPositions(double * refs) override;
265 bool getTargetPositions(int n_joint, const int * joints, double * refs) override;
266
267 // ------- IPositionDirect declarations. Implementation in IPositionDirectImpl.cpp -------
268
269 //bool getAxes(int * ax) override;
270 bool setPosition(int j, double ref) override;
271 bool setPositions(const double * refs) override;
272 bool setPositions(int n_joint, const int * joints, const double * refs) override;
273 bool getRefPosition(int joint, double * ref) override;
274 bool getRefPositions(double * refs) override;
275 bool getRefPositions(int n_joint, const int * joints, double * refs) override;
276
277 // --------- IPWMControl declarations. Implementation in IPWMControlImpl.cpp ---------
278
279 //bool getNumberOfMotors(int * number) override;
280 bool setRefDutyCycle(int m, double ref) override;
281 bool setRefDutyCycles(const double * refs) override;
282 bool getRefDutyCycle(int m, double * ref) override;
283 bool getRefDutyCycles(double * refs) override;
284 bool getDutyCycle(int m, double * val) override;
285 bool getDutyCycles(double * vals) override;
286
287 // ----------- IRemoteVariables declarations. Implementation in IRemoteVariablesImpl.cpp --------------
288
289 bool getRemoteVariable(std::string key, yarp::os::Bottle & val) override;
290 bool setRemoteVariable(std::string key, const yarp::os::Bottle & val) override;
291 bool getRemoteVariablesList(yarp::os::Bottle * listOfKeys) override;
292
293 // -------- ITorqueControl declarations. Implementation in ITorqueControlImpl.cpp --------
294
295 //bool getAxes(int * ax) override;
296 bool getRefTorque(int j, double * t) override;
297 bool getRefTorques(double * t) override;
298 bool setRefTorque(int j, double t) override;
299 bool setRefTorques(const double * t) override;
300 bool setRefTorques(int n_joint, const int * joints, const double * t) override;
301 bool getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters * params) override;
302 bool setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params) override;
303 bool getTorque(int j, double * t) override;
304 bool getTorques(double * t) override;
305 bool getTorqueRange(int j, double * min, double * max) override;
306 bool getTorqueRanges(double * min, double * max) override;
307
308 // --------- IVelocityControl declarations. Implementation in IVelocityControlImpl.cpp ---------
309
310 //bool getAxes(int * ax) override;
311 bool velocityMove(int j, double spd) override;
312 bool velocityMove(const double * spds) override;
313 bool velocityMove(int n_joint, const int * joints, const double * spds) override;
314 bool getRefVelocity(int joint, double * vel) override;
315 bool getRefVelocities(double * vels) override;
316 bool getRefVelocities(int n_joint, const int * joints, double * vels) override;
317 //bool setRefAcceleration(int j, double acc) override;
318 //bool setRefAccelerations(const double * accs) override;
319 //bool setRefAccelerations(int n_joint, const int * joints, const double * accs) override;
320 //bool getRefAcceleration(int j, double * acc) override;
321 //bool getRefAccelerations(double * accs) override;
322 //bool getRefAccelerations(int n_joint, const int * joints, double * accs) override;
323 //bool stop(int j) override;
324 //bool stop() override;
325 //bool stop(int n_joint, const int *joints) override;
326
327 // ---------- MULTIPLE ANALOG SENSORS INTERFACES ----------
328
329 // --------- IContactLoadCellArrays declarations. Implementation in IContactLoadCellArraysImpl.cpp ---------
330
331 std::size_t getNrOfContactLoadCellArrays() const override;
332 yarp::dev::MAS_status getContactLoadCellArrayStatus(std::size_t sens_index) const override;
333 bool getContactLoadCellArrayName(std::size_t sens_index, std::string & name) const override;
334 bool getContactLoadCellArrayMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
335 std::size_t getContactLoadCellArraySize(std::size_t sens_index) const override;
336
337 // --------- IEncoderArrays declarations. Implementation in IEncoderArraysImpl.cpp ---------
338
339 std::size_t getNrOfEncoderArrays() const override;
340 yarp::dev::MAS_status getEncoderArrayStatus(std::size_t sens_index) const override;
341 bool getEncoderArrayName(std::size_t sens_index, std::string & name) const override;
342 bool getEncoderArrayMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
343 std::size_t getEncoderArraySize(std::size_t sens_index) const override;
344
345 // --------- IOrientationSensors declarations. Implementation in IOrientationSensorsImpl.cpp ---------
346
347 std::size_t getNrOfOrientationSensors() const override;
348 yarp::dev::MAS_status getOrientationSensorStatus(std::size_t sens_index) const override;
349 bool getOrientationSensorName(std::size_t sens_index, std::string & name) const override;
350 bool getOrientationSensorFrameName(std::size_t sens_index, std::string & frameName) const override;
351 bool getOrientationSensorMeasureAsRollPitchYaw(std::size_t sens_index, yarp::sig::Vector & rpy, double & timestamp) const override;
352
353 // --------- IPositionSensors declarations. Implementation in IPositionSensorsImpl.cpp ---------
354
355 std::size_t getNrOfPositionSensors() const override;
356 yarp::dev::MAS_status getPositionSensorStatus(std::size_t sens_index) const override;
357 bool getPositionSensorName(std::size_t sens_index, std::string & name) const override;
358 bool getPositionSensorFrameName(std::size_t sens_index, std::string & frameName) const override;
359 bool getPositionSensorMeasure(std::size_t sens_index, yarp::sig::Vector & xyz, double & timestamp) const override;
360
361 // --------- ISixAxisForceTorqueSensors declarations. Implementation in ISixAxisForceTorqueSensorsImpl.cpp ---------
362
363 std::size_t getNrOfSixAxisForceTorqueSensors() const override;
364 yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(std::size_t sens_index) const override;
365 bool getSixAxisForceTorqueSensorName(std::size_t sens_index, std::string & name) const override;
366 bool getSixAxisForceTorqueSensorFrameName(std::size_t sens_index, std::string & frameName) const override;
367 bool getSixAxisForceTorqueSensorMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
368
369 // --------- ISkinPatches declarations. Implementation in ISkinPatchesImpl.cpp ---------
370
371 std::size_t getNrOfSkinPatches() const override;
372 yarp::dev::MAS_status getSkinPatchStatus(std::size_t sens_index) const override;
373 bool getSkinPatchName(std::size_t sens_index, std::string & name) const override;
374 bool getSkinPatchMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
375 std::size_t getSkinPatchSize(std::size_t sens_index) const override;
376
377 // --------- ITemperatureSensors declarations. Implementation in ITemperatureSensorsImpl.cpp ---------
378
379 std::size_t getNrOfTemperatureSensors() const override;
380 yarp::dev::MAS_status getTemperatureSensorStatus(std::size_t sens_index) const override;
381 bool getTemperatureSensorName(std::size_t sens_index, std::string & name) const override;
382 bool getTemperatureSensorFrameName(std::size_t sens_index, std::string & frameName) const override;
383 bool getTemperatureSensorMeasure(std::size_t sens_index, double & out, double & timestamp) const override;
384 bool getTemperatureSensorMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
385
386 // --------- IThreeAxisGyroscopes declarations. Implementation in IThreeAxisGyroscopesImpl.cpp ---------
387
388 std::size_t getNrOfThreeAxisGyroscopes() const override;
389 yarp::dev::MAS_status getThreeAxisGyroscopeStatus(std::size_t sens_index) const override;
390 bool getThreeAxisGyroscopeName(std::size_t sens_index, std::string & name) const override;
391 bool getThreeAxisGyroscopeFrameName(std::size_t sens_index, std::string & frameName) const override;
392 bool getThreeAxisGyroscopeMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
393
394 // --------- IThreeAxisLinearAccelerometers declarations. Implementation in IThreeAxisLinearAccelerometersImpl.cpp ---------
395
396 std::size_t getNrOfThreeAxisLinearAccelerometers() const override;
397 yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(std::size_t sens_index) const override;
398 bool getThreeAxisLinearAccelerometerName(std::size_t sens_index, std::string & name) const override;
399 bool getThreeAxisLinearAccelerometerFrameName(std::size_t sens_index, std::string & frameName) const override;
400 bool getThreeAxisLinearAccelerometerMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
401
402 // --------- IThreeAxisMagnetometers declarations. Implementation in IThreeAxisMagnetometersImpl.cpp ---------
403
404 std::size_t getNrOfThreeAxisMagnetometers() const override;
405 yarp::dev::MAS_status getThreeAxisMagnetometerStatus(std::size_t sens_index) const override;
406 bool getThreeAxisMagnetometerName(std::size_t sens_index, std::string & name) const override;
407 bool getThreeAxisMagnetometerFrameName(std::size_t sens_index, std::string & frameName) const override;
408 bool getThreeAxisMagnetometerMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
409
410private:
411 DeviceMapper deviceMapper;
412
413 std::vector<yarp::dev::PolyDriver *> busDevices;
414 std::vector<yarp::dev::PolyDriver *> nodeDevices;
415 std::vector<SingleBusBroker *> brokers;
416
417 SyncPeriodicThread * syncThread {nullptr};
418};
419
420} // namespace roboticslab
421
422#endif // __CAN_BUS_BROKER_HPP__
roboticslab::CanBusBroker
CAN-oriented control board that implements all YARP motor interfaces.
Definition CanBusBroker.hpp:72
roboticslab::DeviceMapper
Exposes raw subdevice interface handles on a per-axis manner.
Definition DeviceMapper.hpp:65
roboticslab::SyncPeriodicThread
Periodic SYNC signal emitter.
Definition SyncPeriodicThread.hpp:29
roboticslab
The main, catch-all namespace for Robotics Lab UC3M.
Definition groups.dox:6
Generated by doxygen 1.9.8