YARP
Yet Another Robot Platform
ControlBoardWrapper.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
9 #ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPER_H
10 #define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPER_H
11 
12 
13 // ControlBoardWrapper
14 // A modified version of the remote control board class
15 // which remaps joints, it can also merge networks into a single part.
16 //
17 
18 #include <yarp/os/PortablePair.h>
19 #include <yarp/os/BufferedPort.h>
20 #include <yarp/os/Time.h>
21 #include <yarp/os/Network.h>
22 #include <yarp/os/PeriodicThread.h>
23 #include <yarp/os/Stamp.h>
24 #include <yarp/os/Vocab.h>
25 
27 #include <yarp/dev/PolyDriver.h>
30 #include <yarp/sig/Vector.h>
33 
34 #include <mutex>
35 #include <string>
36 #include <vector>
37 
38 #include <yarp/dev/impl/jointData.h> // struct for YARP extended port
39 
40 #include "SubDevice.h"
42 #include "RPCMessagesParser.h"
43 
44 // ROS state publisher
45 #include <yarp/os/Node.h>
46 #include <yarp/os/Publisher.h>
49 
50 #ifdef MSVC
51  #pragma warning(disable:4355)
52 #endif
53 
54 #define PROTOCOL_VERSION_MAJOR 1
55 #define PROTOCOL_VERSION_MINOR 9
56 #define PROTOCOL_VERSION_TWEAK 0
57 
58 /*
59  * To optimize memory allocation, for group of joints we can have one mem reserver for rpc port
60  * and on e for streaming. The size could be numOfSubDevices*maxNumOfjointForSubdevice.
61  * (we could also use the actual joint number for each subdevice using a for loop). TODO
62  */
63 
64 class CommandsHelper;
65 class SubDevice;
66 class WrappedDevice;
67 
69 {
70 public:
71  int deviceNum{0};
73 
74  int *subdev_jointsVectorLen{nullptr}; // number of joints belonging to each subdevice
75  int **jointNumbers{nullptr};
76  int **modes{nullptr};
77  double **values{nullptr};
78  SubDevice **subdevices_p{nullptr};
79 
80  MultiJointData() = default;
81 
82  void resize(int _deviceNum, int _maxJointsNumForDevice, WrappedDevice *_device)
83  {
84  deviceNum = _deviceNum;
85  maxJointsNumForDevice = _maxJointsNumForDevice;
87  jointNumbers = new int *[deviceNum]; // alloc a vector of pointers
88  jointNumbers[0] = new int[deviceNum * _maxJointsNumForDevice]; // alloc real memory for data
89 
90  modes = new int *[deviceNum]; // alloc a vector of pointers
91  modes[0] = new int[deviceNum * _maxJointsNumForDevice]; // alloc real memory for data
92 
93  values = new double *[deviceNum]; // alloc a vector of pointers
94  values[0] = new double[deviceNum * _maxJointsNumForDevice]; // alloc real memory for data
95 
97  subdevices_p[0] = _device->getSubdevice(0);
98 
99  for (int i = 1; i < deviceNum; i++)
100  {
101  jointNumbers[i] = jointNumbers[i-1] + _maxJointsNumForDevice; // set pointer to correct location
102  values [i] = values[i-1] + _maxJointsNumForDevice; // set pointer to correct location
103  modes [i] = modes[i-1] + _maxJointsNumForDevice; // set pointer to correct location
104  subdevices_p[i] = _device->getSubdevice(i);
105  }
106  }
107 
108  void destroy()
109  {
110  // release matrix memory
111  delete[] jointNumbers[0];
112  delete[] values[0];
113  delete[] modes[0];
114 
115  // release vector of pointers
116  delete[] jointNumbers;
117  delete[] values;
118  delete[] modes;
119 
120  // delete other vectors
121  delete[] subdev_jointsVectorLen;
122  delete[] subdevices_p;
123  }
124 };
125 
228  public yarp::dev::IPidControl,
233  public yarp::dev::IMotor,
243  public yarp::dev::IAxisInfo,
247  public yarp::dev::IPWMControl,
249 {
250 private:
251  std::string rootName;
252  WrappedDevice device;
253 
254  bool checkPortName(yarp::os::Searchable &params);
255 
257 
258  yarp::os::BufferedPort<yarp::sig::Vector> outputPositionStatePort; // Port /state:o streaming out the encoder positions
259  yarp::os::BufferedPort<CommandMessage> inputStreamingPort; // Input streaming port for high frequency commands
260  yarp::os::Port inputRPCPort; // Input RPC port for set/get remote calls
261  yarp::os::Stamp time; // envelope to attach to the state port
262  yarp::sig::Vector times; // time for each joint
263  std::mutex timeMutex;
264 
265  // Buffer associated to the extendedOutputStatePort port; in this case we will use the type generated
266  // from the YARP .thrift file
268  yarp::os::Port extendedOutputStatePort; // Port /stateExt:o streaming out the struct with the robot data
269 
270  // ROS state publisher
271  ROSTopicUsageType useROS; // decide if open ROS topic or not
272  std::vector<std::string> jointNames; // name of the joints
273  std::string rosNodeName; // name of the rosNode
274  std::string rosTopicName; // name of the rosTopic
275  yarp::os::Node *rosNode; // add a ROS node
276  yarp::os::NetUint32 rosMsgCounter; // incremental counter in the ROS message
277  yarp::os::PortWriterBuffer<yarp::rosmsg::sensor_msgs::JointState> rosOutputState_buffer; // Buffer associated to the ROS topic
278  yarp::os::Publisher<yarp::rosmsg::sensor_msgs::JointState> rosPublisherPort; // Dedicated ROS topic publisher
279 
280  yarp::os::PortReaderBuffer<yarp::os::Bottle> inputRPC_buffer; // Buffer associated to the inputRPCPort port
281  RPCMessagesParser RPC_parser; // Message parser associated to the inputRPCPort port
282  StreamingMessagesParser streaming_parser; // Message parser associated to the inputStreamingPort port
283 
284 
285  // RPC calls are concurrent from multiple clients, data used inside the calls has to be protected
286  std::mutex rpcDataMutex; // mutex to avoid concurrency between more clients using rppc port
287  MultiJointData rpcData; // Structure used to re-arrange data from "multiple_joints" calls.
288 
289  std::string partName; // to open ports and print more detailed debug messages
290 
291  int controlledJoints;
292  int base; // to be removed
293  int top; // to be removed
294  double period; // thread rate for publishing data
295  bool _verb; // make it work and propagate to subdevice if --subdevice option is used
296 
297  yarp::os::Bottle getOptions();
298  bool updateAxisName();
299  bool checkROSParams(yarp::os::Searchable &config);
300  bool initialize_ROS();
301  bool initialize_YARP(yarp::os::Searchable &prop);
302  void cleanup_yarpPorts();
303 
304  // Default usage
305  // Open the wrapper only, the attach method needs to be called before using it
306  bool openDeferredAttach(yarp::os::Property& prop);
307 
308  // For the simulator, if a subdevice parameter is given to the wrapper, it will
309  // open it and attach to it immediately.
310  yarp::dev::PolyDriver *subDeviceOwned;
311  bool openAndAttachSubDevice(yarp::os::Property& prop);
312 
313  bool ownDevices;
314  inline void printError(std::string func_name, std::string info, bool result)
315  {
316  //If result is false, this means that en error occurred in function named func_name, otherwise means that the device doesn't implement the interface to witch func_name belongs to.
317  // if(false == result)
318  // yError() << "CBW(" << partName << "): " << func_name.c_str() << " on device" << info.c_str() << " returns false";
319  //Commented in order to maintain the old behaviour (none message appear if device desn't implement the interface)
320  //else
321  // yError() << "CBW(" << partName << "): " << func_name.c_str() << " on device" << info.c_str() << ": the interface is not available.";
322  }
323 
324  void calculateMaxNumOfJointsInDevices();
325 
326 public:
328  ControlBoardWrapper(const ControlBoardWrapper&) = delete;
332  ~ControlBoardWrapper() override;
333 
338  bool verbose() const
339  {
340  return _verb;
341  }
342 
343  /* Return id of this device */
344  std::string getId()
345  {
346  return partName;
347  }
348 
353  bool close() override;
354 
355 
366  bool open(yarp::os::Searchable& prop) override;
367 
368  bool detachAll() override;
369 
370  bool attachAll(const yarp::dev::PolyDriverList &l) override;
371 
375  void run() override;
376 
377  /* IPidControl
378  These methods are documented by Doxygen in IPidControl.h*/
379  bool setPid(const yarp::dev::PidControlTypeEnum& pidtype, int j, const yarp::dev::Pid &p) override;
380  bool setPids(const yarp::dev::PidControlTypeEnum& pidtype, const yarp::dev::Pid *ps) override;
381  bool setPidReference(const yarp::dev::PidControlTypeEnum& pidtype, int j, double ref) override;
382  bool setPidReferences(const yarp::dev::PidControlTypeEnum& pidtype, const double *refs) override;
383  bool setPidErrorLimit(const yarp::dev::PidControlTypeEnum& pidtype, int j, double limit) override;
384  bool setPidErrorLimits(const yarp::dev::PidControlTypeEnum& pidtype, const double *limits) override;
385  bool getPidError(const yarp::dev::PidControlTypeEnum& pidtype, int j, double *err) override;
386  bool getPidErrors(const yarp::dev::PidControlTypeEnum& pidtype, double *errs) override;
387  bool getPidOutput(const yarp::dev::PidControlTypeEnum& pidtype, int j, double *out) override;
388  bool getPidOutputs(const yarp::dev::PidControlTypeEnum& pidtype, double *outs) override;
389  bool setPidOffset(const yarp::dev::PidControlTypeEnum& pidtype, int j, double v) override;
390  bool getPid(const yarp::dev::PidControlTypeEnum& pidtype, int j, yarp::dev::Pid *p) override;
391  bool getPids(const yarp::dev::PidControlTypeEnum& pidtype, yarp::dev::Pid *pids) override;
392  bool getPidReference(const yarp::dev::PidControlTypeEnum& pidtype, int j, double *ref) override;
393  bool getPidReferences(const yarp::dev::PidControlTypeEnum& pidtype, double *refs) override;
394  bool getPidErrorLimit(const yarp::dev::PidControlTypeEnum& pidtype, int j, double *limit) override;
395  bool getPidErrorLimits(const yarp::dev::PidControlTypeEnum& pidtype, double *limits) override;
396  bool resetPid(const yarp::dev::PidControlTypeEnum& pidtype, int j) override;
397  bool disablePid(const yarp::dev::PidControlTypeEnum& pidtype, int j) override;
398  bool enablePid(const yarp::dev::PidControlTypeEnum& pidtype, int j) override;
399  bool isPidEnabled(const yarp::dev::PidControlTypeEnum& pidtype, int j, bool* enabled) override;
400 
401  /* IPositionControl */
402 
409  bool getAxes(int *ax) override;
410 
417  bool positionMove(int j, double ref) override;
418 
423  bool positionMove(const double *refs) override;
424 
430  bool positionMove(const int n_joints, const int *joints, const double *refs) override;
431 
441  bool getTargetPosition(const int joint, double *ref) override;
442 
452  bool getTargetPositions(double *refs) override;
453 
463  bool getTargetPositions(const int n_joint, const int *joints, double *refs) override;
464 
471  bool relativeMove(int j, double delta) override;
472 
477  bool relativeMove(const double *deltas) override;
478 
484  bool relativeMove(const int n_joints, const int *joints, const double *deltas) override;
485 
492  bool checkMotionDone(int j, bool *flag) override;
499  bool checkMotionDone(bool *flag) override;
500 
507  bool checkMotionDone(const int n_joints, const int *joints, bool *flags) override;
508 
515  bool setRefSpeed(int j, double sp) override;
516 
522  bool setRefSpeeds(const double *spds) override;
523 
530  bool setRefSpeeds(const int n_joints, const int *joints, const double *spds) override;
531 
538  bool setRefAcceleration(int j, double acc) override;
539 
545  bool setRefAccelerations(const double *accs) override;
546 
553  bool setRefAccelerations(const int n_joints, const int *joints, const double *accs) override;
554 
561  bool getRefSpeed(int j, double *ref) override;
562 
568  bool getRefSpeeds(double *spds) override;
569 
576  bool getRefSpeeds(const int n_joints, const int *joints, double *spds) override;
577 
584  bool getRefAcceleration(int j, double *acc) override;
585 
591  bool getRefAccelerations(double *accs) override;
592 
599  bool getRefAccelerations(const int n_joints, const int *joints, double *accs) override;
600 
605  bool stop(int j) override;
606 
611  bool stop() override;
612 
613 
618  bool stop(const int n_joints, const int *joints) override;
619 
620  /* IVelocityControl */
621 
628  bool velocityMove(int j, double v) override;
629 
635  bool velocityMove(const double *v) override;
636 
637  /* IEncoders */
638 
644  bool resetEncoder(int j) override;
645 
650  bool resetEncoders() override;
651 
658  bool setEncoder(int j, double val) override;
659 
665  bool setEncoders(const double *vals) override;
666 
673  bool getEncoder(int j, double *v) override;
674 
680  bool getEncoders(double *encs) override;
681 
682  bool getEncodersTimed(double *encs, double *t) override;
683 
684  bool getEncoderTimed(int j, double *v, double *t) override;
685 
692  bool getEncoderSpeed(int j, double *sp) override;
693 
699  bool getEncoderSpeeds(double *spds) override;
700 
706  bool getEncoderAcceleration(int j, double *acc) override;
712  bool getEncoderAccelerations(double *accs) override;
713 
714  /* IMotorEncoders */
715 
721  bool getNumberOfMotorEncoders(int *num) override;
722 
728  bool resetMotorEncoder(int m) override;
729 
734  bool resetMotorEncoders() override;
735 
742  bool setMotorEncoderCountsPerRevolution(int m, const double cpr) override;
743 
750  bool getMotorEncoderCountsPerRevolution(int m, double *cpr) override;
751 
758  bool setMotorEncoder(int m, const double val) override;
759 
765  bool setMotorEncoders(const double *vals) override;
766 
773  bool getMotorEncoder(int m, double *v) override;
774 
780  bool getMotorEncoders(double *encs) override;
781 
782  bool getMotorEncodersTimed(double *encs, double *t) override;
783 
784  bool getMotorEncoderTimed(int m, double *v, double *t) override;
785 
792  bool getMotorEncoderSpeed(int m, double *sp) override;
793 
799  bool getMotorEncoderSpeeds(double *spds) override;
800 
806  bool getMotorEncoderAcceleration(int m, double *acc) override;
812  bool getMotorEncoderAccelerations(double *accs) override;
813 
814  /* IAmplifierControl */
815 
822  bool enableAmp(int j) override;
823 
829  bool disableAmp(int j) override;
830 
838  bool getAmpStatus(int *st) override;
839 
840  bool getAmpStatus(int j, int *v) override;
841 
847  bool getCurrents(double *vals) override;
848 
855  bool getCurrent(int j, double *val) override;
856 
865  bool setMaxCurrent(int j, double v) override;
866 
875  bool getMaxCurrent(int j, double *v) override;
876 
877  /* Get the the nominal current which can be kept for an indefinite amount of time
878  * without harming the motor. This value is specific for each motor and it is typically
879  * found in its datasheet. The units are Ampere.
880  * This value and the peak current may be used by the firmware to configure
881  * an I2T filter.
882  * @param m motor number
883  * @param val storage for return value. [Ampere]
884  * @return true/false success failure.
885  */
886  bool getNominalCurrent(int m, double *val) override;
887 
888  /* Set the the nominal current which can be kept for an indefinite amount of time
889  * without harming the motor. This value is specific for each motor and it is typically
890  * found in its datasheet. The units are Ampere.
891  * This value and the peak current may be used by the firmware to configure
892  * an I2T filter.
893  * @param m motor number
894  * @param val storage for return value. [Ampere]
895  * @return true/false success failure.
896  */
897  bool setNominalCurrent(int m, const double val) override;
898 
899  /* Get the the peak current which causes damage to the motor if maintained
900  * for a long amount of time.
901  * The value is often found in the motor datasheet, units are Ampere.
902  * This value and the nominal current may be used by the firmware to configure
903  * an I2T filter.
904  * @param m motor number
905  * @param val storage for return value. [Ampere]
906  * @return true/false success failure.
907  */
908  bool getPeakCurrent(int m, double *val) override;
909 
910  /* Set the the peak current. This value which causes damage to the motor if maintained
911  * for a long amount of time.
912  * The value is often found in the motor datasheet, units are Ampere.
913  * This value and the nominal current may be used by the firmware to configure
914  * an I2T filter.
915  * @param m motor number
916  * @param val storage for return value. [Ampere]
917  * @return true/false success failure.
918  */
919  bool setPeakCurrent(int m, const double val) override;
920 
921  /* Get the the current PWM value used to control the motor.
922  * The units are firmware dependent, either machine units or percentage.
923  * @param m motor number
924  * @param val filled with PWM value.
925  * @return true/false success failure.
926  */
927  bool getPWM(int m, double* val) override;
928 
929  /* Get the PWM limit for the given motor.
930  * The units are firmware dependent, either machine units or percentage.
931  * @param m motor number
932  * @param val filled with PWM limit value.
933  * @return true/false success failure.
934  */
935  bool getPWMLimit(int m, double* val) override;
936 
937  /* Set the PWM limit for the given motor.
938  * The units are firmware dependent, either machine units or percentage.
939  * @param m motor number
940  * @param val new value for the PWM limit.
941  * @return true/false success failure.
942  */
943  bool setPWMLimit(int m, const double val) override;
944 
945  /* Get the power source voltage for the given motor in Volt.
946  * @param m motor number
947  * @param val filled with return value.
948  * @return true/false success failure.
949  */
950  bool getPowerSupplyVoltage(int m, double* val) override;
951 
952  /* IControlLimits */
953 
962  bool setLimits(int j, double min, double max) override;
963 
971  bool getLimits(int j, double *min, double *max) override;
972 
981  bool setVelLimits(int j, double min, double max) override;
982 
990  bool getVelLimits(int j, double *min, double *max) override;
991 
992  /* IRemoteVariables */
993 
994  bool getRemoteVariable(std::string key, yarp::os::Bottle& val) override;
995 
996  bool setRemoteVariable(std::string key, const yarp::os::Bottle& val) override;
997 
998  bool getRemoteVariablesList(yarp::os::Bottle* listOfKeys) override;
999 
1000  /* IRemoteCalibrator */
1001 
1002  bool isCalibratorDevicePresent(bool *isCalib) override;
1003 
1009 
1015  bool calibrateSingleJoint(int j) override;
1016 
1021  bool calibrateWholePart() override;
1022 
1028  bool homingSingleJoint(int j) override;
1029 
1034  bool homingWholePart() override;
1035 
1040  bool parkSingleJoint(int j, bool _wait=true) override;
1041 
1046  bool parkWholePart() override;
1047 
1052  bool quitCalibrate() override;
1053 
1058  bool quitPark() override;
1059 
1060  /* IControlCalibration */
1061  bool calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) override;
1062 
1063  bool setCalibrationParameters(int j, const yarp::dev::CalibrationParameters& params) override;
1064 
1070  bool calibrationDone(int j) override;
1071 
1072  bool abortPark() override;
1073 
1074  bool abortCalibration() override;
1075 
1076  /* IMotor */
1077  bool getNumberOfMotors (int *num) override;
1078 
1079  bool getTemperature (int m, double* val) override;
1080 
1081  bool getTemperatures (double *vals) override;
1082 
1083  bool getTemperatureLimit (int m, double* val) override;
1084 
1085  bool setTemperatureLimit (int m, const double val) override;
1086 
1087  bool getGearboxRatio(int m, double* val) override;
1088 
1089  bool setGearboxRatio(int m, const double val) override;
1090 
1091  /* IAxisInfo */
1092  bool getAxisName(int j, std::string& name) override;
1093 
1094  bool getJointType(int j, yarp::dev::JointTypeEnum& type) override;
1095 
1096  bool getRefTorques(double *refs) override;
1097 
1098  bool getRefTorque(int j, double *t) override;
1099 
1100  bool setRefTorques(const double *t) override;
1101 
1102  bool setRefTorque(int j, double t) override;
1103 
1104  bool setRefTorques(const int n_joint, const int *joints, const double *t) override;
1105 
1106  bool getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters *params) override;
1107 
1108  bool setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params) override;
1109 
1110  bool setImpedance(int j, double stiff, double damp) override;
1111 
1112  bool setImpedanceOffset(int j, double offset) override;
1113 
1114  bool getTorque(int j, double *t) override;
1115 
1116  bool getTorques(double *t) override;
1117 
1118  bool getTorqueRange(int j, double *min, double *max) override;
1119 
1120  bool getTorqueRanges(double *min, double *max) override;
1121 
1122  bool getImpedance(int j, double* stiff, double* damp) override;
1123 
1124  bool getImpedanceOffset(int j, double* offset) override;
1125 
1126  bool getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override;
1127 
1128  bool getControlMode(int j, int *mode) override;
1129 
1130  bool getControlModes(int *modes) override;
1131 
1132  // iControlMode2
1133  bool getControlModes(const int n_joint, const int *joints, int *modes) override;
1134 
1135  bool setControlMode(const int j, const int mode) override;
1136 
1137  bool setControlModes(const int n_joints, const int *joints, int *modes) override;
1138 
1139  bool setControlModes(int *modes) override;
1140 
1141  // IPositionDirect
1142 
1143  bool setPosition(int j, double ref) override;
1144 
1145  bool setPositions(const int n_joints, const int *joints, const double *dpos) override;
1146 
1147  bool setPositions(const double *refs) override;
1148 
1158  bool getRefPosition(const int joint, double *ref) override;
1159 
1169  bool getRefPositions(double *refs) override;
1170 
1180  bool getRefPositions(const int n_joint, const int *joints, double *refs) override;
1181 
1183 
1184  //
1185  // IVelocityControl2 Interface
1186  //
1187  bool velocityMove(const int n_joints, const int *joints, const double *spds) override;
1188 
1189  bool getRefVelocity(const int joint, double* vel) override;
1190 
1191  bool getRefVelocities(double* vels) override;
1192 
1193  bool getRefVelocities(const int n_joint, const int* joints, double* vels) override;
1194 
1195  bool getInteractionMode(int j, yarp::dev::InteractionModeEnum* mode) override;
1196 
1197  bool getInteractionModes(int n_joints, int *joints, yarp::dev::InteractionModeEnum* modes) override;
1198 
1200 
1201  bool setInteractionMode(int j, yarp::dev::InteractionModeEnum mode) override;
1202 
1203  bool setInteractionModes(int n_joints, int *joints, yarp::dev::InteractionModeEnum* modes) override;
1204 
1206 
1207  //
1208  // IPWMControl Interface
1209  //
1210 
1211  bool setRefDutyCycle(int j, double v) override;
1212  bool setRefDutyCycles(const double *v) override;
1213  bool getRefDutyCycle(int j, double *v) override;
1214  bool getRefDutyCycles(double *v) override;
1215  bool getDutyCycle(int j, double *v) override;
1216  bool getDutyCycles(double *v) override;
1217 
1218  //
1219  // ICurrentControl Interface
1220  //
1221 
1222  //bool getAxes(int *ax) override;
1223  //bool getCurrent(int j, double *t) override;
1224  //bool getCurrents(double *t) override;
1225  bool getCurrentRange(int j, double *min, double *max) override;
1226  bool getCurrentRanges(double *min, double *max) override;
1227  bool setRefCurrents(const double *t) override;
1228  bool setRefCurrent(int j, double t) override;
1229  bool setRefCurrents(const int n_joint, const int *joints, const double *t) override;
1230  bool getRefCurrents(double *t) override;
1231  bool getRefCurrent(int j, double *t) override;
1232 };
1233 
1234 
1235 #endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPER_H
ControlBoardWrapper::setRefAccelerations
bool setRefAccelerations(const double *accs) override
Set reference acceleration on all joints.
Definition: ControlBoardWrapper.cpp:1985
ControlBoardWrapper::setMotorEncoder
bool setMotorEncoder(int m, const double val) override
Set the value of the encoder for a given joint.
Definition: ControlBoardWrapper.cpp:2952
ControlBoardWrapper::getEncoderSpeed
bool getEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of an axis.
Definition: ControlBoardWrapper.cpp:2636
ControlBoardWrapper::enablePid
bool enablePid(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Enable the pid computation for a joint.
Definition: ControlBoardWrapper.cpp:1379
SubDevice.h
ControlBoardWrapper::enableAmp
bool enableAmp(int j) override
Enable the amplifier on a specific joint.
Definition: ControlBoardWrapper.cpp:3225
yarp::os::Bottle
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:72
ControlBoardWrapper::getTorqueRanges
bool getTorqueRanges(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
Definition: ControlBoardWrapper.cpp:4005
yarp::dev::IRemoteVariables
Definition: IRemoteVariables.h:50
ControlBoardWrapper::getTorque
bool getTorque(int j, double *t) override
Get the value of the torque on a given joint (this is the feedback if you have a torque sensor).
Definition: ControlBoardWrapper.cpp:3938
Network.h
JointState.h
ControlBoardWrapper::getMotorEncoder
bool getMotorEncoder(int m, double *v) override
Read the value of an encoder.
Definition: ControlBoardWrapper.cpp:3020
ControlBoardWrapper::operator=
ControlBoardWrapper & operator=(const ControlBoardWrapper &)=delete
ControlBoardWrapper::setPid
bool setPid(const yarp::dev::PidControlTypeEnum &pidtype, int j, const yarp::dev::Pid &p) override
Set new pid value for a joint axis.
Definition: ControlBoardWrapper.cpp:976
WrappedDevice
Definition: SubDevice.h:122
yarp::os::Searchable
A base class for nested structures that can be searched.
Definition: Searchable.h:68
ControlBoardWrapper::getMotorEncoderTimed
bool getMotorEncoderTimed(int m, double *v, double *t) override
Read the instantaneous position of a motor encoder.
Definition: ControlBoardWrapper.cpp:3104
ControlBoardWrapper::resetPid
bool resetPid(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Reset the controller of a given joint, usually sets the current status of the joint as the reference ...
Definition: ControlBoardWrapper.cpp:1349
ControlBoardWrapper::setPosition
bool setPosition(int j, double ref) override
Set new position for a single axis.
Definition: ControlBoardWrapper.cpp:4256
ControlBoardWrapper::getRemoteVariablesList
bool getRemoteVariablesList(yarp::os::Bottle *listOfKeys) override
Definition: ControlBoardWrapper.cpp:2897
MultiJointData::values
double ** values
Definition: ControlBoardWrapper.h:77
ControlBoardWrapper::setControlMode
bool setControlMode(const int j, const int mode) override
Set the current control mode.
Definition: ControlBoardWrapper.cpp:4165
Vector.h
t
float t
Definition: FfmpegWriter.cpp:69
yarp::dev::IImpedanceControl
Definition: IImpedanceControl.h:74
MultiJointData::jointNumbers
int ** jointNumbers
Definition: ControlBoardWrapper.h:75
ControlBoardWrapper::getCurrents
bool getCurrents(double *vals) override
Read the electric current going to all motors.
Definition: ControlBoardWrapper.cpp:4921
ControlBoardWrapper::setRefTorque
bool setRefTorque(int j, double t) override
Set the reference value of the torque for a given joint.
Definition: ControlBoardWrapper.cpp:3821
MultiJointData::maxJointsNumForDevice
int maxJointsNumForDevice
Definition: ControlBoardWrapper.h:72
ControlBoardWrapper::getPidReference
bool getPidReference(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *ref) override
Get the current reference of the pid controller for a specific joint.
Definition: ControlBoardWrapper.cpp:1256
MultiJointData::resize
void resize(int _deviceNum, int _maxJointsNumForDevice, WrappedDevice *_device)
Definition: ControlBoardWrapper.h:82
ControlBoardWrapper::setRefDutyCycle
bool setRefDutyCycle(int j, double v) override
Sets the reference dutycycle to a single motor.
Definition: ControlBoardWrapper.cpp:4774
ControlBoardWrapper::setMaxCurrent
bool setMaxCurrent(int j, double v) override
Set the maximum electric current going to a given motor.
Definition: ControlBoardWrapper.cpp:3312
yarp::dev::MotorTorqueParameters
Definition: ITorqueControl.h:23
ControlBoardWrapper::getRefDutyCycle
bool getRefDutyCycle(int j, double *v) override
Gets the last reference sent using the setRefDutyCycle function.
Definition: ControlBoardWrapper.cpp:4814
ControlBoardWrapper::setEncoder
bool setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
Definition: ControlBoardWrapper.cpp:2499
ControlBoardWrapper::getRemoteVariable
bool getRemoteVariable(std::string key, yarp::os::Bottle &val) override
Definition: ControlBoardWrapper.cpp:2849
IPreciselyTimed.h
yarp::dev::DeviceDriver
Definition: DeviceDriver.h:37
ControlBoardWrapper::setPidReference
bool setPidReference(const yarp::dev::PidControlTypeEnum &pidtype, int j, double ref) override
Set the controller reference for a given axis.
Definition: ControlBoardWrapper.cpp:1015
ControlBoardWrapper::getTemperatureLimit
bool getTemperatureLimit(int m, double *val) override
Retreives the current temperature limit for a specific motor.
Definition: ControlBoardWrapper.cpp:2786
ControlBoardWrapper::resetMotorEncoder
bool resetMotorEncoder(int m) override
Reset encoder, single joint.
Definition: ControlBoardWrapper.cpp:2915
ControlBoardWrapper::getMotorEncoderCountsPerRevolution
bool getMotorEncoderCountsPerRevolution(int m, double *cpr) override
gets number of counts per revolution for motor encoder m.
Definition: ControlBoardWrapper.cpp:3004
ControlBoardWrapper::getCurrentRanges
bool getCurrentRanges(double *min, double *max) override
Get the full scale of the current measurements for all motors motor (e.g.
Definition: ControlBoardWrapper.cpp:4999
ControlBoardWrapper::calibrateAxisWithParams
bool calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) override
Start calibration, this method is very often platform specific.
Definition: ControlBoardWrapper.cpp:3661
ControlBoardWrapper::getCurrentImpedanceLimit
bool getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
Get the current impedandance limits for a specific joint.
Definition: ControlBoardWrapper.cpp:4075
ControlBoardWrapper::stop
bool stop() override
Stop motion, multiple joints.
Definition: ControlBoardWrapper.cpp:2341
ControlBoardWrapper::getEncoders
bool getEncoders(double *encs) override
Read the position of all axes.
Definition: ControlBoardWrapper.cpp:2552
ControlBoardHelpers.h
ControlBoardWrapper::getPidErrorLimit
bool getPidErrorLimit(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *limit) override
Get the error limit for the controller on a specific joint.
Definition: ControlBoardWrapper.cpp:1302
ControlBoardWrapper::getJointType
bool getJointType(int j, yarp::dev::JointTypeEnum &type) override
Definition: ControlBoardWrapper.cpp:3733
yarp::os::PortReaderBuffer< yarp::os::Bottle >
ControlBoardWrapper::getRefCurrents
bool getRefCurrents(double *t) override
Get the reference value of the currents for all motors.
Definition: ControlBoardWrapper.cpp:5110
yarp::os::PortWriterBuffer< yarp::dev::impl::jointData >
ControlBoardWrapper::getRefTorque
bool getRefTorque(int j, double *t) override
Get the reference value of the torque for a given joint.
Definition: ControlBoardWrapper.cpp:3781
ControlBoardWrapper::getRefAcceleration
bool getRefAcceleration(int j, double *acc) override
Get reference acceleration for a joint.
Definition: ControlBoardWrapper.cpp:2198
ControlBoardWrapper::getInteractionModes
bool getInteractionModes(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
Get the current interaction mode of the robot for a set of joints, values can be stiff or compliant.
Definition: ControlBoardWrapper.cpp:4611
ControlBoardWrapper::getTargetPositions
bool getTargetPositions(double *refs) override
Get the last position reference for all axes.
Definition: ControlBoardWrapper.cpp:1559
ControlBoardWrapper::getControlModes
bool getControlModes(int *modes) override
Get the current control mode (multiple joints).
Definition: ControlBoardWrapper.cpp:4108
ControlBoardWrapper::getPidErrors
bool getPidErrors(const yarp::dev::PidControlTypeEnum &pidtype, double *errs) override
Get the error of all joints.
Definition: ControlBoardWrapper.cpp:1110
ControlBoardWrapper::getLimits
bool getLimits(int j, double *min, double *max) override
Get the software limits for a particular axis.
Definition: ControlBoardWrapper.cpp:3516
ControlBoardWrapper::setMotorTorqueParams
bool setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params) override
Set a subset of motor parameters (bemf, ktau etc) useful for torque control.
Definition: ControlBoardWrapper.cpp:3888
ControlBoardWrapper::homingSingleJoint
bool homingSingleJoint(int j) override
homingSingleJoint: call the homing procedure for a single joint
Definition: ControlBoardWrapper.cpp:3605
ControlBoardWrapper::setPWMLimit
bool setPWMLimit(int m, const double val) override
Definition: ControlBoardWrapper.cpp:3461
yarp::os::Publisher< yarp::rosmsg::sensor_msgs::JointState >
yarp::dev::PolyDriverList
Definition: PolyDriverList.h:21
ControlBoardInterfaces.h
ControlBoardWrapper::setGearboxRatio
bool setGearboxRatio(int m, const double val) override
Set the gearbox ratio for a specific motor.
Definition: ControlBoardWrapper.cpp:2833
ControlBoardWrapper::close
bool close() override
Close the device driver by deallocating all resources and closing ports.
Definition: ControlBoardWrapper.cpp:73
ControlBoardWrapper::setImpedance
bool setImpedance(int j, double stiff, double damp) override
Set current impedance gains (stiffness,damping) for a specific joint.
Definition: ControlBoardWrapper.cpp:3904
ControlBoardWrapper::setRefCurrents
bool setRefCurrents(const double *t) override
Set the reference value of the currents for all motors.
Definition: ControlBoardWrapper.cpp:5035
ControlBoardWrapper::setRefDutyCycles
bool setRefDutyCycles(const double *v) override
Sets the reference dutycycle for all the motors.
Definition: ControlBoardWrapper.cpp:4791
ControlBoardWrapper::run
void run() override
The thread main loop deals with writing on ports here.
Definition: ControlBoardWrapper.cpp:876
MultiJointData::subdev_jointsVectorLen
int * subdev_jointsVectorLen
Definition: ControlBoardWrapper.h:74
ControlBoardWrapper::getNumberOfMotorEncoders
bool getNumberOfMotorEncoders(int *num) override
Get the number of available motor encoders.
Definition: ControlBoardWrapper.cpp:3218
ControlBoardWrapper::getPidOutputs
bool getPidOutputs(const yarp::dev::PidControlTypeEnum &pidtype, double *outs) override
Get the output of the controllers (e.g.
Definition: ControlBoardWrapper.cpp:1159
PortablePair.h
yarpRosHelper.h
ControlBoardWrapper::getRefSpeed
bool getRefSpeed(int j, double *ref) override
Get reference speed for a joint.
Definition: ControlBoardWrapper.cpp:2074
ControlBoardWrapper::abortCalibration
bool abortCalibration() override
Definition: ControlBoardWrapper.cpp:3709
ControlBoardWrapper::getRefPosition
bool getRefPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
Definition: ControlBoardWrapper.cpp:4340
ControlBoardWrapper::getDutyCycles
bool getDutyCycles(double *v) override
Gets the current dutycycle of the output of the amplifier (i.e.
Definition: ControlBoardWrapper.cpp:4881
ControlBoardWrapper::getImpedanceOffset
bool getImpedanceOffset(int j, double *offset) override
Get current force Offset for a specific joint.
Definition: ControlBoardWrapper.cpp:4058
ControlBoardWrapper::setPidReferences
bool setPidReferences(const yarp::dev::PidControlTypeEnum &pidtype, const double *refs) override
Set the controller reference, multiple axes.
Definition: ControlBoardWrapper.cpp:1031
ControlBoardWrapper::getEncoderSpeeds
bool getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
Definition: ControlBoardWrapper.cpp:2652
ControlBoardWrapper::getNominalCurrent
bool getNominalCurrent(int m, double *val) override
Definition: ControlBoardWrapper.cpp:3348
ControlBoardWrapper::getMotorTorqueParams
bool getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters *params) override
Get a subset of motor parameters (bemf, ktau etc) useful for torque control.
Definition: ControlBoardWrapper.cpp:3872
ControlBoardWrapper::getId
std::string getId()
Definition: ControlBoardWrapper.h:344
yarp::sig::VectorOf< double >
yarp::os::Port
Definition: Port.h:49
yarp::os::BufferedPort< yarp::sig::Vector >
ControlBoardWrapper::setInteractionModes
bool setInteractionModes(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
Set the interaction mode of the robot for a set of joints, values can be stiff or compliant.
Definition: ControlBoardWrapper.cpp:4716
yarp::dev::IInteractionMode
Definition: IInteractionMode.h:42
ControlBoardWrapper::setInteractionMode
bool setInteractionMode(int j, yarp::dev::InteractionModeEnum mode) override
Set the interaction mode of the robot, values can be stiff or compliant.
Definition: ControlBoardWrapper.cpp:4700
ControlBoardWrapper::setCalibrationParameters
bool setCalibrationParameters(int j, const yarp::dev::CalibrationParameters &params) override
Start calibration, this method is very often platform specific.
Definition: ControlBoardWrapper.cpp:3674
ControlBoardWrapper::getPidOutput
bool getPidOutput(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *out) override
Get the output of the controller (e.g.
Definition: ControlBoardWrapper.cpp:1142
ControlBoardWrapper::parkSingleJoint
bool parkSingleJoint(int j, bool _wait=true) override
parkSingleJoint(): start the parking procedure for the single joint
Definition: ControlBoardWrapper.cpp:3623
ControlBoardWrapper::resetEncoder
bool resetEncoder(int j) override
Reset encoder, single joint.
Definition: ControlBoardWrapper.cpp:2462
ControlBoardWrapper::setImpedanceOffset
bool setImpedanceOffset(int j, double offset) override
Set current force Offset for a specific joint.
Definition: ControlBoardWrapper.cpp:3921
ControlBoardWrapper::getPidErrorLimits
bool getPidErrorLimits(const yarp::dev::PidControlTypeEnum &pidtype, double *limits) override
Get the error limit for all controllers.
Definition: ControlBoardWrapper.cpp:1317
ControlBoardWrapper::getPids
bool getPids(const yarp::dev::PidControlTypeEnum &pidtype, yarp::dev::Pid *pids) override
Get current pid value for a specific joint.
Definition: ControlBoardWrapper.cpp:1224
ControlBoardWrapper
Definition: ControlBoardWrapper.h:225
ControlBoardWrapper::getDutyCycle
bool getDutyCycle(int j, double *v) override
Gets the current dutycycle of the output of the amplifier (i.e.
Definition: ControlBoardWrapper.cpp:4864
ControlBoardWrapper::getMaxCurrent
bool getMaxCurrent(int j, double *v) override
Returns the maximum electric current allowed for a given motor.
Definition: ControlBoardWrapper.cpp:3328
ControlBoardWrapper::getRefAccelerations
bool getRefAccelerations(double *accs) override
Get reference acceleration of all joints.
Definition: ControlBoardWrapper.cpp:2221
yarp::dev::IControlMode
Definition: IControlMode.h:27
ControlBoardWrapper::getEncoder
bool getEncoder(int j, double *v) override
Read the value of an encoder.
Definition: ControlBoardWrapper.cpp:2536
ControlBoardWrapper::checkMotionDone
bool checkMotionDone(int j, bool *flag) override
Check if the current trajectory is terminated.
Definition: ControlBoardWrapper.cpp:1738
ControlBoardWrapper::setTemperatureLimit
bool setTemperatureLimit(int m, const double val) override
Set the temperature limit for a specific motor.
Definition: ControlBoardWrapper.cpp:2802
MultiJointData::subdevices_p
SubDevice ** subdevices_p
Definition: ControlBoardWrapper.h:78
ControlBoardWrapper::getRefTorques
bool getRefTorques(double *refs) override
Get the reference value of the torque for all joints.
Definition: ControlBoardWrapper.cpp:3749
yarp::dev::IControlCalibration
Definition: IControlCalibration.h:84
ControlBoardWrapper::getRefVelocity
bool getRefVelocity(const int joint, double *vel) override
Get the last reference speed set by velocityMove for single joint.
Definition: ControlBoardWrapper.cpp:4484
ControlBoardWrapper::getMotorEncoderSpeed
bool getMotorEncoderSpeed(int m, double *sp) override
Read the istantaneous speed of an axis.
Definition: ControlBoardWrapper.cpp:3120
yarp::dev::IAxisInfo
Definition: IAxisInfo.h:42
yarp::dev::PidControlTypeEnum
PidControlTypeEnum
Definition: PidEnums.h:20
ControlBoardWrapper::getPWM
bool getPWM(int m, double *val) override
Definition: ControlBoardWrapper.cpp:3420
MultiJointData
Definition: ControlBoardWrapper.h:68
ControlBoardWrapper::setMotorEncoderCountsPerRevolution
bool setMotorEncoderCountsPerRevolution(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
Definition: ControlBoardWrapper.cpp:2989
ROSTopicUsageType
ROSTopicUsageType
Definition: yarpRosHelper.h:18
PolyDriver.h
ControlBoardWrapper::getVelLimits
bool getVelLimits(int j, double *min, double *max) override
Get the software velocity limits for a particular axis.
Definition: ControlBoardWrapper.cpp:3554
ControlBoardWrapper::getAmpStatus
bool getAmpStatus(int *st) override
Get the status of the amplifiers, coded in a 32 bits integer for each amplifier (at the moment contai...
Definition: ControlBoardWrapper.cpp:3266
yarp::os::NetUint32
std::uint32_t NetUint32
Definition of the NetUint32 type.
Definition: NetUint32.h:33
ControlBoardWrapper::disablePid
bool disablePid(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Disable the pid computation for a joint.
Definition: ControlBoardWrapper.cpp:1364
ControlBoardWrapper::relativeMove
bool relativeMove(int j, double delta) override
Set relative position.
Definition: ControlBoardWrapper.cpp:1650
ControlBoardWrapper::setMotorEncoders
bool setMotorEncoders(const double *vals) override
Set the value of all encoders.
Definition: ControlBoardWrapper.cpp:2967
ControlBoardWrapper::getNumberOfMotors
bool getNumberOfMotors(int *num) override
Retrieves the number of controlled axes from the current physical interface.
Definition: ControlBoardWrapper.cpp:2733
Stamp.h
ControlBoardWrapper::getEncoderTimed
bool getEncoderTimed(int j, double *v, double *t) override
Read the instantaneous acceleration of all axes.
Definition: ControlBoardWrapper.cpp:2620
ControlBoardWrapper::getMotorEncoderSpeeds
bool getMotorEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
Definition: ControlBoardWrapper.cpp:3136
ControlBoardWrapper::setRefSpeed
bool setRefSpeed(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
Definition: ControlBoardWrapper.cpp:1856
ControlBoardWrapper::quitPark
bool quitPark() override
quitPark: interrupt the park procedure
Definition: ControlBoardWrapper.cpp:3650
yarp::dev::IControlLimits
Definition: IControlLimits.h:32
yarp::dev::PolyDriver
Definition: PolyDriver.h:25
ControlBoardWrapper::getRefDutyCycles
bool getRefDutyCycles(double *v) override
Gets the last reference sent using the setRefDutyCycles function.
Definition: ControlBoardWrapper.cpp:4831
yarp::dev::InteractionModeEnum
InteractionModeEnum
Definition: IInteractionMode.h:20
ControlBoardWrapper::setRefTorques
bool setRefTorques(const double *t) override
Set the reference value of the torque for all joints.
Definition: ControlBoardWrapper.cpp:3798
ControlBoardWrapper::setRefCurrent
bool setRefCurrent(int j, double t) override
Set the reference value of the current for a single motor.
Definition: ControlBoardWrapper.cpp:5058
ControlBoardWrapper::setVelLimits
bool setVelLimits(int j, double min, double max) override
Set the software velocity limits for a particular axis, the behavior of the control card when these l...
Definition: ControlBoardWrapper.cpp:3538
ControlBoardWrapper::setPids
bool setPids(const yarp::dev::PidControlTypeEnum &pidtype, const yarp::dev::Pid *ps) override
Set new pid value on multiple axes.
Definition: ControlBoardWrapper.cpp:992
Node.h
StreamingMessagesParser
Callback implementation after buffered input.
Definition: StreamingMessagesParser.h:57
ControlBoardWrapper::setLimits
bool setLimits(int j, double min, double max) override
Set the software limits for a particular axis, the behavior of the control card when these limits are...
Definition: ControlBoardWrapper.cpp:3500
ControlBoardWrapper::setControlModes
bool setControlModes(const int n_joints, const int *joints, int *modes) override
Set the current control mode for a subset of axes.
Definition: ControlBoardWrapper.cpp:4182
ControlBoardWrapper::isPidEnabled
bool isPidEnabled(const yarp::dev::PidControlTypeEnum &pidtype, int j, bool *enabled) override
Get the current status (enabled/disabled) of the pid.
Definition: ControlBoardWrapper.cpp:1394
ControlBoardWrapper::getRefPositions
bool getRefPositions(double *refs) override
Get the last position reference for all axes.
Definition: ControlBoardWrapper.cpp:4359
ControlBoardWrapper::~ControlBoardWrapper
~ControlBoardWrapper() override
ControlBoardWrapper::getTargetPosition
bool getTargetPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
Definition: ControlBoardWrapper.cpp:1534
ControlBoardWrapper::setPidOffset
bool setPidOffset(const yarp::dev::PidControlTypeEnum &pidtype, int j, double v) override
Set offset value for a given controller.
Definition: ControlBoardWrapper.cpp:1191
ControlBoardWrapper::resetEncoders
bool resetEncoders() override
Reset encoders.
Definition: ControlBoardWrapper.cpp:2477
ControlBoardWrapper::setPidErrorLimits
bool setPidErrorLimits(const yarp::dev::PidControlTypeEnum &pidtype, const double *limits) override
Get the error limit for the controller on all joints.
Definition: ControlBoardWrapper.cpp:1070
yarp::os::Node
The Node class.
Definition: Node.h:26
ControlBoardWrapper::getControlMode
bool getControlMode(int j, int *mode) override
Get the current control mode.
Definition: ControlBoardWrapper.cpp:4092
yarp::dev::IPWMControl
Definition: IPWMControl.h:27
ControlBoardWrapper::setRefAcceleration
bool setRefAcceleration(int j, double acc) override
Set reference acceleration for a joint.
Definition: ControlBoardWrapper.cpp:1964
ControlBoardWrapper::getAxes
bool getAxes(int *ax) override
Get the number of controlled axes.
Definition: ControlBoardWrapper.cpp:1420
MultiJointData::MultiJointData
MultiJointData()=default
ControlBoardWrapper::getRefSpeeds
bool getRefSpeeds(double *spds) override
Get reference speed of all joints.
Definition: ControlBoardWrapper.cpp:2097
MultiJointData::deviceNum
int deviceNum
Definition: ControlBoardWrapper.h:71
BufferedPort.h
IMultipleWrapper.h
ControlBoardWrapper::velocityMove
bool velocityMove(int j, double v) override
Set new reference speed for a single axis.
Definition: ControlBoardWrapper.cpp:2405
ControlBoardWrapper::getTorqueRange
bool getTorqueRange(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
Definition: ControlBoardWrapper.cpp:3988
yarp::os::PeriodicThread
Definition: PeriodicThread.h:24
ControlBoardWrapper::ControlBoardWrapper
ControlBoardWrapper()
Definition: ControlBoardWrapper.cpp:31
ControlBoardWrapper::getPidReferences
bool getPidReferences(const yarp::dev::PidControlTypeEnum &pidtype, double *refs) override
Get the current reference of all pid controllers.
Definition: ControlBoardWrapper.cpp:1270
ControlBoardWrapper::getEncoderAcceleration
bool getEncoderAcceleration(int j, double *acc) override
Read the instantaneous acceleration of an axis.
Definition: ControlBoardWrapper.cpp:2684
yarp::dev::IMotorEncoders
Definition: IMotorEncoders.h:154
ControlBoardWrapper::getInteractionMode
bool getInteractionMode(int j, yarp::dev::InteractionModeEnum *mode) override
Get the current interaction mode of the robot, values can be stiff or compliant.
Definition: ControlBoardWrapper.cpp:4595
ControlBoardWrapper::disableAmp
bool disableAmp(int j) override
Disable the amplifier on a specific joint.
Definition: ControlBoardWrapper.cpp:3241
yarp::dev::IAmplifierControl
Definition: IAmplifierControl.h:32
ControlBoardWrapper::detachAll
bool detachAll() override
Detach the object (you must have first called attach).
Definition: ControlBoardWrapper.cpp:856
ControlBoardWrapper::attachAll
bool attachAll(const yarp::dev::PolyDriverList &l) override
Attach to a list of objects.
Definition: ControlBoardWrapper.cpp:789
ControlBoardWrapper::getAxisName
bool getAxisName(int j, std::string &name) override
Definition: ControlBoardWrapper.cpp:3717
ControlBoardWrapper::getMotorEncodersTimed
bool getMotorEncodersTimed(double *encs, double *t) override
Read the instantaneous position of all motor encoders.
Definition: ControlBoardWrapper.cpp:3069
PeriodicThread.h
ControlBoardWrapper::getRefCurrent
bool getRefCurrent(int j, double *t) override
Get the reference value of the current for a single motor.
Definition: ControlBoardWrapper.cpp:5143
yarp::dev::IMotor
Definition: IMotor.h:98
yarp::dev::IVelocityControl
Definition: IVelocityControl.h:159
ControlBoardWrapper::getPeakCurrent
bool getPeakCurrent(int m, double *val) override
Definition: ControlBoardWrapper.cpp:3368
ControlBoardWrapper::getCurrent
bool getCurrent(int j, double *val) override
Read the electric current going to a given motor.
Definition: ControlBoardWrapper.cpp:4960
yarp::os::Stamp
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:24
ControlBoardWrapper::getPWMLimit
bool getPWMLimit(int m, double *val) override
Definition: ControlBoardWrapper.cpp:3440
ControlBoardWrapper::setEncoders
bool setEncoders(const double *vals) override
Set the value of all encoders.
Definition: ControlBoardWrapper.cpp:2514
ControlBoardWrapper::setNominalCurrent
bool setNominalCurrent(int m, const double val) override
Definition: ControlBoardWrapper.cpp:3404
ControlBoardWrapper::calibrationDone
bool calibrationDone(int j) override
Check whether the calibration has been completed.
Definition: ControlBoardWrapper.cpp:3687
RPCMessagesParser
Helper object for parsing RPC port messages.
Definition: RPCMessagesParser.h:55
yarp::dev::ICurrentControl
Definition: ICurrentControl.h:27
ControlBoardWrapper::getLastInputStamp
yarp::os::Stamp getLastInputStamp() override
Return the time stamp relative to the last acquisition.
Definition: ControlBoardWrapper.cpp:4333
jointData.h
ControlBoardWrapper::setPositions
bool setPositions(const int n_joints, const int *joints, const double *dpos) override
Set new reference point for all axes.
Definition: ControlBoardWrapper.cpp:4273
yarp::dev::IPidControl
Definition: IPidControl.h:210
MultiJointData::destroy
void destroy()
Definition: ControlBoardWrapper.h:108
ControlBoardWrapper::isCalibratorDevicePresent
bool isCalibratorDevicePresent(bool *isCalib) override
isCalibratorDevicePresent: check if a calibrator device has been set
Definition: ControlBoardWrapper.cpp:3582
ControlBoardWrapper::getRefVelocities
bool getRefVelocities(double *vels) override
Get the last reference speed set by velocityMove for all joints.
Definition: ControlBoardWrapper.cpp:4507
Publisher.h
ControlBoardWrapper::getImpedance
bool getImpedance(int j, double *stiff, double *damp) override
Get current impedance gains (stiffness,damping,offset) for a specific joint.
Definition: ControlBoardWrapper.cpp:4041
yarp::dev::IPositionDirect
Definition: IPositionDirect.h:31
ControlBoardWrapper::getTemperatures
bool getTemperatures(double *vals) override
Get temperature of all the motors.
Definition: ControlBoardWrapper.cpp:2754
ControlBoardWrapper::setRemoteVariable
bool setRemoteVariable(std::string key, const yarp::os::Bottle &val) override
Definition: ControlBoardWrapper.cpp:2866
yarp::dev::IEncodersTimed
Control board, extend encoder interface with timestamps.
Definition: IEncodersTimed.h:58
yarp::dev::IMultipleWrapper
Definition: IMultipleWrapper.h:29
ControlBoardWrapper::getPowerSupplyVoltage
bool getPowerSupplyVoltage(int m, double *val) override
Definition: ControlBoardWrapper.cpp:3477
ControlBoardWrapper::setPeakCurrent
bool setPeakCurrent(int m, const double val) override
Definition: ControlBoardWrapper.cpp:3388
ControlBoardWrapper::getMotorEncoderAccelerations
bool getMotorEncoderAccelerations(double *accs) override
Read the istantaneous acceleration of all axes.
Definition: ControlBoardWrapper.cpp:3184
ControlBoardWrapper::getTorques
bool getTorques(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
Definition: ControlBoardWrapper.cpp:3955
ControlBoardWrapper::getMotorEncoders
bool getMotorEncoders(double *encs) override
Read the position of all axes.
Definition: ControlBoardWrapper.cpp:3036
Vocab.h
ControlBoardInterfacesImpl.h
yarp::rosmsg::sensor_msgs::JointState
Definition: JointState.h:55
MultiJointData::modes
int ** modes
Definition: ControlBoardWrapper.h:76
ControlBoardWrapper::resetMotorEncoders
bool resetMotorEncoders() override
Reset encoders.
Definition: ControlBoardWrapper.cpp:2930
ControlBoardWrapper::getCurrentRange
bool getCurrentRange(int j, double *min, double *max) override
Get the full scale of the current measurement for a given motor (e.g.
Definition: ControlBoardWrapper.cpp:4981
yarp::dev::Pid
Contains the parameters for a PID.
Definition: ControlBoardPid.h:28
Time.h
ControlBoardWrapper::parkWholePart
bool parkWholePart() override
parkWholePart: start the parking procedure for the whole part
Definition: ControlBoardWrapper.cpp:3632
ControlBoardWrapper::verbose
bool verbose() const
Return the value of the verbose flag.
Definition: ControlBoardWrapper.h:338
yarp::dev::ITorqueControl
Definition: ITorqueControl.h:38
yarp::dev::CalibrationParameters
Definition: IControlCalibration.h:31
ControlBoardWrapper::getTemperature
bool getTemperature(int m, double *val) override
Get temperature of a motor.
Definition: ControlBoardWrapper.cpp:2738
ControlBoardWrapper::getEncodersTimed
bool getEncodersTimed(double *encs, double *t) override
Read the instantaneous acceleration of all axes.
Definition: ControlBoardWrapper.cpp:2585
RPCMessagesParser.h
ControlBoardWrapper::getPid
bool getPid(const yarp::dev::PidControlTypeEnum &pidtype, int j, yarp::dev::Pid *p) override
Get current pid value for a specific joint.
Definition: ControlBoardWrapper.cpp:1207
WrappedDevice::getSubdevice
SubDevice * getSubdevice(unsigned int i)
Definition: SubDevice.h:129
ControlBoardWrapper::setRefSpeeds
bool setRefSpeeds(const double *spds) override
Set reference speed on all joints.
Definition: ControlBoardWrapper.cpp:1876
SubDevice
Definition: SubDevice.h:55
ControlBoardWrapper::calibrateWholePart
bool calibrateWholePart() override
calibrateWholePart: call the procedure for calibrating the whole device
Definition: ControlBoardWrapper.cpp:3596
ControlBoardWrapper::homingWholePart
bool homingWholePart() override
homingWholePart: call the homing procedure for a the whole part/device
Definition: ControlBoardWrapper.cpp:3614
yarp::dev::IRemoteCalibrator
IRemoteCalibrator interface is meant to remotize the access of the calibration device in order to all...
Definition: IRemoteCalibrator.h:29
yarp::dev::IPreciselyTimed
Definition: IPreciselyTimed.h:20
ControlBoardWrapper::getGearboxRatio
bool getGearboxRatio(int m, double *val) override
Get the gearbox ratio for a specific motor.
Definition: ControlBoardWrapper.cpp:2817
ControlBoardWrapper::positionMove
bool positionMove(int j, double ref) override
Set new reference point for a single axis.
Definition: ControlBoardWrapper.cpp:1431
yarp::dev::JointTypeEnum
JointTypeEnum
Definition: IAxisInfo.h:28
ControlBoardWrapper::open
bool open(yarp::os::Searchable &prop) override
Open the device driver.
Definition: ControlBoardWrapper.cpp:394
yarp::dev::IPositionControl
Definition: IPositionControl.h:256
ControlBoardWrapper::getCalibratorDevice
yarp::dev::IRemoteCalibrator * getCalibratorDevice() override
getCalibratorDevice: return the pointer stored with the setCalibratorDevice
Definition: ControlBoardWrapper.cpp:3576
ControlBoardWrapper::calibrateSingleJoint
bool calibrateSingleJoint(int j) override
calibrateSingleJoint: call the calibration procedure for the single joint
Definition: ControlBoardWrapper.cpp:3588
ControlBoardWrapper::quitCalibrate
bool quitCalibrate() override
quitCalibrate: interrupt the calibration procedure
Definition: ControlBoardWrapper.cpp:3641
ControlBoardWrapper::setPidErrorLimit
bool setPidErrorLimit(const yarp::dev::PidControlTypeEnum &pidtype, int j, double limit) override
Set the error limit for the controller on a specifi joint.
Definition: ControlBoardWrapper.cpp:1054
yarp::os::Property
A class for storing options and configuration information.
Definition: Property.h:34
ControlBoardWrapper::getPidError
bool getPidError(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *err) override
Get the current error for a joint.
Definition: ControlBoardWrapper.cpp:1093
StreamingMessagesParser.h
ControlBoardWrapper::getEncoderAccelerations
bool getEncoderAccelerations(double *accs) override
Read the istantaneous acceleration of all axes.
Definition: ControlBoardWrapper.cpp:2700
ControlBoardWrapper::getMotorEncoderAcceleration
bool getMotorEncoderAcceleration(int m, double *acc) override
Read the instantaneous acceleration of an axis.
Definition: ControlBoardWrapper.cpp:3168
ControlBoardWrapper::abortPark
bool abortPark() override
Definition: ControlBoardWrapper.cpp:3703