openrave-yarp-plugins
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 #include <yarp/dev/ControlBoardInterfaces.h>
8 
9 #include <openrave/openrave.h>
10 
11 #include "YarpOpenraveBase.hpp"
12 
13 namespace roboticslab
14 {
15 
28 class YarpOpenraveControlBoard : YarpOpenraveBase,
29  public yarp::dev::DeviceDriver,
30  public yarp::dev::IAxisInfo,
31  public yarp::dev::IControlLimits,
32  public yarp::dev::IControlMode,
33  public yarp::dev::IEncodersTimed,
34  public yarp::dev::IPositionControl,
35  public yarp::dev::IPositionDirect,
36  public yarp::dev::IVelocityControl
37 {
38 public:
39  // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp --------
40  bool open(yarp::os::Searchable& config) override;
41  bool close() override;
42 
43  // ------- IAxisInfo declarations. Implementation in IAxisInfoImpl.cpp -------
44  bool getAxisName(int axis, std::string& name) override;
45  bool getJointType(int axis, yarp::dev::JointTypeEnum& type) override;
46 
47  // ------- IPositionControl declarations. Implementation in IPositionControlImpl.cpp -------
48  bool getAxes(int *ax) override;
49  bool positionMove(int j, double ref) override;
50  bool positionMove(const double *refs) override;
51  bool relativeMove(int j, double delta) override;
52  bool relativeMove(const double *deltas) override;
53  bool checkMotionDone(int j, bool *flag) override;
54  bool checkMotionDone(bool *flag) override;
55  bool setRefSpeed(int j, double sp) override;
56  bool setRefSpeeds(const double *spds) override;
57  bool setRefAcceleration(int j, double acc) override;
58  bool setRefAccelerations(const double *accs) override;
59  bool getRefSpeed(int j, double *ref) override;
60  bool getRefSpeeds(double *spds) override;
61  bool getRefAcceleration(int j, double *acc) override;
62  bool getRefAccelerations(double *accs) override;
63  bool stop(int j) override;
64  bool stop() override;
65  bool positionMove(const int n_joint, const int *joints, const double *refs) override;
66  bool relativeMove(const int n_joint, const int *joints, const double *deltas) override;
67  bool checkMotionDone(const int n_joint, const int *joints, bool *flags) override;
68  bool setRefSpeeds(const int n_joint, const int *joints, const double *spds) override;
69  bool setRefAccelerations(const int n_joint, const int *joints, const double *accs) override;
70  bool getRefSpeeds(const int n_joint, const int *joints, double *spds) override;
71  bool getRefAccelerations(const int n_joint, const int *joints, double *accs) override;
72  bool stop(const int n_joint, const int *joints) override;
73  bool getTargetPosition(const int joint, double *ref) override;
74  bool getTargetPositions(double *refs) override;
75  bool getTargetPositions(const int n_joint, const int *joints, double *refs) override;
76 
77  // ------- IPositionDirect declarations. Implementation in IPositionDirectImpl.cpp -------
78  bool setPosition(int j, double ref) override;
79  bool setPositions(const int n_joint, const int *joints, const double *refs) override;
80  bool setPositions(const double *refs) override;
81  bool getRefPosition(const int joint, double *ref) override;
82  bool getRefPositions(double *refs) override;
83  bool getRefPositions(const int n_joint, const int *joints, double *refs) override;
84 
85  // ---------- IEncoders Declarations. Implementation in IEncodersImpl.cpp ----------
86  bool resetEncoder(int j) override;
87  bool resetEncoders() override;
88  bool setEncoder(int j, double val) override;
89  bool setEncoders(const double *vals) override;
90  bool getEncoder(int j, double *v) override;
91  bool getEncoders(double *encs) override;
92  bool getEncoderSpeed(int j, double *sp) override;
93  bool getEncoderSpeeds(double *spds) override;
94  bool getEncoderAcceleration(int j, double *spds) override;
95  bool getEncoderAccelerations(double *accs) override;
96  bool getEncodersTimed(double *encs, double *time) override;
97  bool getEncoderTimed(int j, double *encs, double *time) override;
98 
99  // --------- IVelocityControl Declarations. Implementation in IVelocityControlImpl.cpp ---------
100  bool velocityMove(int j, double sp) override;
101  bool velocityMove(const double *sp) override;
102  bool velocityMove(const int n_joint, const int *joints, const double *spds) override;
103  bool getRefVelocity(const int joint, double *vel) override;
104  bool getRefVelocities(double *vels) override;
105  bool getRefVelocities(const int n_joint, const int *joints, double *vels) override;
106 
107  // --------- IControlLimits Declarations. Implementation in IControlLimitsImpl.cpp ---------
108  bool setLimits(int axis, double min, double max) override;
109  bool getLimits(int axis, double *min, double *max) override;
110  bool setVelLimits(int axis, double min, double max) override;
111  bool getVelLimits(int axis, double *min, double *max) override;
112 
113  // --------- IControlMode Declarations. Implementation in IControlModeImpl.cpp ---------
114  bool getControlMode(int j, int *mode) override;
115  bool getControlModes(int *modes) override;
116  bool getControlModes(const int n_joint, const int *joints, int *modes) override;
117  bool setControlMode(const int j, const int mode) override;
118  bool setControlModes(const int n_joint, const int *joints, int *modes) override;
119  bool setControlModes(int *modes) override;
120 
121 private:
122  // General Joint Motion Controller parameters //
123  unsigned int axes;
124  std::vector<int> controlModes;
125  std::vector<double> refSpeeds; // Exposed.
126 
127  //OpenRAVE//
128  OpenRAVE::MultiControllerBasePtr multi;
129  std::vector<OpenRAVE::ControllerBasePtr> pcontrols;
130  std::vector<int> manipulatorIDs;
131  std::vector<OpenRAVE::dReal> manipulatorTargetRads;
132  std::vector<OpenRAVE::RobotBase::JointPtr> vectorOfJointPtr;
133 
139  inline double degToRad(double deg) { return deg * M_PI / 180.0; }
140 
146  inline double radToDeg(double rad) { return rad * 180.0 / M_PI; }
147 
154  inline double degToRadIfNotPrismatic(int j, double deg)
155  {
156  if (vectorOfJointPtr[j]->IsPrismatic(0)) return deg;
157  else return degToRad(deg); // revolute, circular
158  }
159 
166  inline double radToDegIfNotPrismatic(int j, double rad)
167  {
168  if (vectorOfJointPtr[j]->IsPrismatic(0)) return rad;
169  else return radToDeg(rad); // revolute, circular
170  }
171 };
172 
173 } // namespace roboticslab
174 
175 #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:37
roboticslab::YarpOpenraveControlBoard::radToDeg
double radToDeg(double rad)
Converts radians to degrees.
Definition: YarpOpenraveControlBoard.hpp:146
roboticslab::YarpOpenraveControlBoard::radToDegIfNotPrismatic
double radToDegIfNotPrismatic(int j, double rad)
Converts radians to degrees, unless joint j is prismatic.
Definition: YarpOpenraveControlBoard.hpp:166
roboticslab::YarpOpenraveControlBoard::degToRadIfNotPrismatic
double degToRadIfNotPrismatic(int j, double deg)
Converts degrees to radians, unless joint j is prismatic.
Definition: YarpOpenraveControlBoard.hpp:154
roboticslab::YarpOpenraveControlBoard::degToRad
double degToRad(double deg)
Converts degrees to radians.
Definition: YarpOpenraveControlBoard.hpp:139
roboticslab
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:5
Generated on Thu Oct 17 2024 17:50:10 for openrave-yarp-plugins by doxygen 1.9.1