openrave-yarp-plugins
Loading...
Searching...
No Matches
YarpOpenraveControlBoard.hpp
1// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2
3#ifndef __YARP_OPENRAVE_CONTROL_BOARD_HPP__
4#define __YARP_OPENRAVE_CONTROL_BOARD_HPP__
5
6#include <vector>
7
8#include <yarp/conf/version.h>
9
10#include <yarp/dev/ControlBoardInterfaces.h>
11
12#include <openrave/openrave.h>
13
14#include "YarpOpenraveBase.hpp"
15
16namespace roboticslab
17{
18
31class YarpOpenraveControlBoard : YarpOpenraveBase,
32 public yarp::dev::DeviceDriver,
33 public yarp::dev::IAxisInfo,
34 public yarp::dev::IControlLimits,
35 public yarp::dev::IControlMode,
36 public yarp::dev::IEncodersTimed,
37 public yarp::dev::IPositionControl,
38 public yarp::dev::IPositionDirect,
39 public yarp::dev::IVelocityControl
40{
41public:
42 // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp --------
43 bool open(yarp::os::Searchable& config) override;
44 bool close() override;
45
46 // ------- IAxisInfo declarations. Implementation in IAxisInfoImpl.cpp -------
47#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
48 yarp::dev::ReturnValue getAxisName(int axis, std::string & name) override;
49 yarp::dev::ReturnValue getJointType(int axis, yarp::dev::JointTypeEnum & type) override;
50#else
51 bool getAxisName(int axis, std::string & name) override;
52 bool getJointType(int axis, yarp::dev::JointTypeEnum & type) override;
53#endif
54
55 // ------- IPositionControl declarations. Implementation in IPositionControlImpl.cpp -------
56#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
57 yarp::dev::ReturnValue getAxes(std::size_t & ax) override;
58 yarp::dev::ReturnValue positionMove(int j, double ref) override;
59 yarp::dev::ReturnValue positionMove(const double * refs) override;
60 yarp::dev::ReturnValue positionMove(int n_joint, const int * joints, const double * refs) override;
61 yarp::dev::ReturnValue relativeMove(int j, double delta) override;
62 yarp::dev::ReturnValue relativeMove(const double * deltas) override;
63 yarp::dev::ReturnValue relativeMove(int n_joint, const int * joints, const double * deltas) override;
64 yarp::dev::ReturnValue checkMotionDone(int j, bool & flag) override;
65 yarp::dev::ReturnValue checkMotionDone(bool & flag) override;
66 yarp::dev::ReturnValue checkMotionDone(const std::vector<int> & joints, bool & flags) override;
67 yarp::dev::ReturnValue setTrajSpeed(int j, double sp) override;
68 yarp::dev::ReturnValue setTrajSpeeds(const double * spds) override;
69 yarp::dev::ReturnValue setTrajSpeeds(int n_joint, const int * joints, const double * spds) override;
70 yarp::dev::ReturnValue setTrajAcceleration(int j, double acc) override;
71 yarp::dev::ReturnValue setTrajAccelerations(const double * accs) override;
72 yarp::dev::ReturnValue setTrajAccelerations(int n_joint, const int * joints, const double * accs) override;
73 yarp::dev::ReturnValue getTrajSpeed(int j, double * ref) override;
74 yarp::dev::ReturnValue getTrajSpeeds(double * spds) override;
75 yarp::dev::ReturnValue getTrajSpeeds(int n_joint, const int * joints, double * spds) override;
76 yarp::dev::ReturnValue getTrajAcceleration(int j, double * acc) override;
77 yarp::dev::ReturnValue getTrajAccelerations(double * accs) override;
78 yarp::dev::ReturnValue getTrajAccelerations(int n_joint, const int * joints, double * accs) override;
79 yarp::dev::ReturnValue stop(int j) override;
80 yarp::dev::ReturnValue stop() override;
81 yarp::dev::ReturnValue stop(int n_joint, const int * joints) override;
82 yarp::dev::ReturnValue getTargetPosition(int joint, double * ref) override;
83 yarp::dev::ReturnValue getTargetPositions(double * refs) override;
84 yarp::dev::ReturnValue getTargetPositions(int n_joint, const int * joints, double * refs) override;
85#else
86 bool getAxes(int * ax) override;
87 bool positionMove(int j, double ref) override;
88 bool positionMove(const double * refs) override;
89 bool positionMove(int n_joint, const int * joints, const double * refs) override;
90 bool relativeMove(int j, double delta) override;
91 bool relativeMove(const double * deltas) override;
92 bool relativeMove(int n_joint, const int * joints, const double * deltas) override;
93 bool checkMotionDone(int j, bool * flag) override;
94 bool checkMotionDone(bool * flag) override;
95 bool checkMotionDone(int n_joint, const int * joints, bool * flags) override;
96 bool setRefSpeed(int j, double sp) override;
97 bool setRefSpeeds(const double * spds) override;
98 bool setRefSpeeds(int n_joint, const int * joints, const double * spds) override;
99 bool setRefAcceleration(int j, double acc) override;
100 bool setRefAccelerations(const double * accs) override;
101 bool setRefAccelerations(int n_joint, const int * joints, const double * accs) override;
102 bool getRefSpeed(int j, double * ref) override;
103 bool getRefSpeeds(double * spds) override;
104 bool getRefSpeeds(int n_joint, const int * joints, double * spds) override;
105 bool getRefAcceleration(int j, double * acc) override;
106 bool getRefAccelerations(double * accs) override;
107 bool getRefAccelerations(int n_joint, const int * joints, double * accs) override;
108 bool stop(int j) override;
109 bool stop() override;
110 bool stop(int n_joint, const int * joints) override;
111 bool getTargetPosition(int joint, double * ref) override;
112 bool getTargetPositions(double * refs) override;
113 bool getTargetPositions(int n_joint, const int * joints, double * refs) override;
114#endif
115
116 // ------- IPositionDirect declarations. Implementation in IPositionDirectImpl.cpp -------
117#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
118 yarp::dev::ReturnValue setPosition(int j, double ref) override;
119 yarp::dev::ReturnValue setPositions(const double * refs) override;
120 yarp::dev::ReturnValue setPositions(int n_joint, const int * joints, const double * refs) override;
121 yarp::dev::ReturnValue getRefPosition(int joint, double * ref) override;
122 yarp::dev::ReturnValue getRefPositions(double * refs) override;
123 yarp::dev::ReturnValue getRefPositions(int n_joint, const int * joints, double * refs) override;
124#else
125 bool setPosition(int j, double ref) override;
126 bool setPositions(const double * refs) override;
127 bool setPositions(int n_joint, const int * joints, const double * refs) override;
128 bool getRefPosition(int joint, double * ref) override;
129 bool getRefPositions(double * refs) override;
130 bool getRefPositions(int n_joint, const int * joints, double * refs) override;
131#endif
132
133 // ---------- IEncoders Declarations. Implementation in IEncodersImpl.cpp ----------
134#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
135 yarp::dev::ReturnValue resetEncoder(int j) override;
136 yarp::dev::ReturnValue resetEncoders() override;
137 yarp::dev::ReturnValue setEncoder(int j, double val) override;
138 yarp::dev::ReturnValue setEncoders(const double * vals) override;
139 yarp::dev::ReturnValue getEncoder(int j, double * v) override;
140 yarp::dev::ReturnValue getEncoders(double * encs) override;
141 yarp::dev::ReturnValue getEncoderSpeed(int j, double * sp) override;
142 yarp::dev::ReturnValue getEncoderSpeeds(double * spds) override;
143 yarp::dev::ReturnValue getEncoderAcceleration(int j, double * spds) override;
144 yarp::dev::ReturnValue getEncoderAccelerations(double *accs) override;
145 yarp::dev::ReturnValue getEncodersTimed(double * encs, double * time) override;
146 yarp::dev::ReturnValue getEncoderTimed(int j, double * encs, double * time) override;
147#else
148 bool resetEncoder(int j) override;
149 bool resetEncoders() override;
150 bool setEncoder(int j, double val) override;
151 bool setEncoders(const double * vals) override;
152 bool getEncoder(int j, double * v) override;
153 bool getEncoders(double * encs) override;
154 bool getEncoderSpeed(int j, double * sp) override;
155 bool getEncoderSpeeds(double * spds) override;
156 bool getEncoderAcceleration(int j, double * spds) override;
157 bool getEncoderAccelerations(double *accs) override;
158 bool getEncodersTimed(double * encs, double * time) override;
159 bool getEncoderTimed(int j, double * encs, double * time) override;
160#endif
161
162 // --------- IVelocityControl Declarations. Implementation in IVelocityControlImpl.cpp ---------
163#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
164 yarp::dev::ReturnValue velocityMove(int j, double sp) override;
165 yarp::dev::ReturnValue velocityMove(const double * sp) override;
166 yarp::dev::ReturnValue velocityMove(int n_joint, const int * joints, const double * spds) override;
167 yarp::dev::ReturnValue getTargetVelocity(const int joint, double * vel) override;
168 yarp::dev::ReturnValue getTargetVelocities(double * vels) override;
169 yarp::dev::ReturnValue getTargetVelocities(int n_joint, const int * joints, double * vels) override;
170#else
171 bool velocityMove(int j, double sp) override;
172 bool velocityMove(const double * sp) override;
173 bool velocityMove(int n_joint, const int * joints, const double * spds) override;
174 bool getRefVelocity(const int joint, double * vel) override;
175 bool getRefVelocities(double * vels) override;
176 bool getRefVelocities(int n_joint, const int * joints, double * vels) override;
177#endif
178
179 // --------- IControlLimits Declarations. Implementation in IControlLimitsImpl.cpp ---------
180#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
181 yarp::dev::ReturnValue setPosLimits(int axis, double min, double max) override;
182 yarp::dev::ReturnValue getPosLimits(int axis, double * min, double * max) override;
183 yarp::dev::ReturnValue setVelLimits(int axis, double min, double max) override;
184 yarp::dev::ReturnValue getVelLimits(int axis, double * min, double * max) override;
185#else
186 bool setLimits(int axis, double min, double max) override;
187 bool getLimits(int axis, double * min, double * max) override;
188 bool setVelLimits(int axis, double min, double max) override;
189 bool getVelLimits(int axis, double * min, double * max) override;
190#endif
191
192 // --------- IControlMode Declarations. Implementation in IControlModeImpl.cpp ---------
193#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
194 yarp::dev::ReturnValue getAvailableControlModes(int j, std::vector<yarp::dev::SelectableControlModeEnum> & avail) override;
195 yarp::dev::ReturnValue getControlMode(int j, yarp::dev::ControlModeEnum & mode) override;
196 yarp::dev::ReturnValue getControlModes(std::vector<yarp::dev::ControlModeEnum> & modes) override;
197 yarp::dev::ReturnValue getControlModes(const std::vector<int> & joints, std::vector<yarp::dev::ControlModeEnum> & modes) override;
198 yarp::dev::ReturnValue setControlMode(int j, yarp::dev::SelectableControlModeEnum mode) override;
199 yarp::dev::ReturnValue setControlModes(const std::vector<yarp::dev::SelectableControlModeEnum> & modes) override;
200 yarp::dev::ReturnValue setControlModes(const std::vector<int> & joints, const std::vector<yarp::dev::SelectableControlModeEnum> & modes) override;
201#else
202 bool getControlMode(int j, int * mode) override;
203 bool getControlModes(int * modes) override;
204 bool getControlModes(int n_joint, const int * joints, int * modes) override;
205 bool setControlMode(int j, int mode) override;
206 bool setControlModes(int * modes) override;
207 bool setControlModes(int n_joint, const int * joints, int * modes) override;
208#endif
209
210private:
211 // General Joint Motion Controller parameters //
212 unsigned int axes;
213 std::vector<yarp::conf::vocab32_t> controlModes;
214 std::vector<double> refSpeeds; // Exposed.
215
216 //OpenRAVE//
217 OpenRAVE::MultiControllerBasePtr multi;
218 std::vector<OpenRAVE::ControllerBasePtr> pcontrols;
219 std::vector<int> manipulatorIDs;
220 std::vector<OpenRAVE::dReal> manipulatorTargetRads;
221 std::vector<OpenRAVE::RobotBase::JointPtr> vectorOfJointPtr;
222
228 inline double degToRad(double deg)
229 { return deg * M_PI / 180.0; }
230
236 inline double radToDeg(double rad)
237 { return rad * 180.0 / M_PI; }
238
245 inline double degToRadIfNotPrismatic(int j, double deg)
246 {
247 if (vectorOfJointPtr[j]->IsPrismatic(0)) return deg;
248 else return degToRad(deg); // revolute, circular
249 }
250
257 inline double radToDegIfNotPrismatic(int j, double rad)
258 {
259 if (vectorOfJointPtr[j]->IsPrismatic(0)) return rad;
260 else return radToDeg(rad); // revolute, circular
261 }
262};
263
264} // namespace roboticslab
265
266#endif // __YARP_OPENRAVE_CONTROL_BOARD_HPP__
roboticslab::YarpOpenraveBase
Implements shared configuration functions.
Definition YarpOpenraveBase.hpp:32
roboticslab::YarpOpenraveControlBoard
Implements the YARP_dev IPositionControl, IVelocityControl, IEncodersTimed, etc. interface class memb...
Definition YarpOpenraveControlBoard.hpp:40
roboticslab::YarpOpenraveControlBoard::radToDeg
double radToDeg(double rad)
Converts radians to degrees.
Definition YarpOpenraveControlBoard.hpp:236
roboticslab::YarpOpenraveControlBoard::radToDegIfNotPrismatic
double radToDegIfNotPrismatic(int j, double rad)
Converts radians to degrees, unless joint j is prismatic.
Definition YarpOpenraveControlBoard.hpp:257
roboticslab::YarpOpenraveControlBoard::degToRadIfNotPrismatic
double degToRadIfNotPrismatic(int j, double deg)
Converts degrees to radians, unless joint j is prismatic.
Definition YarpOpenraveControlBoard.hpp:245
roboticslab::YarpOpenraveControlBoard::degToRad
double degToRad(double deg)
Converts degrees to radians.
Definition YarpOpenraveControlBoard.hpp:228
roboticslab
The main, catch-all namespace for Robotics Lab UC3M.
Definition groups.dox:5
Generated by doxygen 1.9.8