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#include <yarp/dev/ControlBoardInterfaces.h>
8
9#include <openrave/openrave.h>
10
11#include "YarpOpenraveBase.hpp"
12
13namespace roboticslab
14{
15
28class 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{
38public:
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
121private:
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 by doxygen 1.9.8