sot-core  4.11.6
Hierarchical task solver plug-in for dynamic-graph.
device.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * Florent Lamiraux
4  *
5  * CNRS
6  *
7  */
8 
9 #ifndef SOT_DEVICE_HH
10 #define SOT_DEVICE_HH
11 
12 /* --------------------------------------------------------------------- */
13 /* --- INCLUDE --------------------------------------------------------- */
14 /* --------------------------------------------------------------------- */
15 
16 #include <pinocchio/fwd.hpp>
17 
18 /* -- MaaL --- */
19 #include <dynamic-graph/linear-algebra.h>
20 
21 /* SOT */
22 #include "sot/core/api.hh"
24 #include <dynamic-graph/all-signals.h>
25 #include <dynamic-graph/entity.h>
27 
28 namespace dynamicgraph {
29 namespace sot {
30 
37 };
38 
39 const std::string ControlInput_s[] = {"noInteg", "oneInteg", "twoInteg"};
40 
41 /* --------------------------------------------------------------------- */
42 /* --- CLASS ----------------------------------------------------------- */
43 /* --------------------------------------------------------------------- */
44 
45 class SOT_CORE_EXPORT Device : public Entity {
46 public:
47  static const std::string CLASS_NAME;
48  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
49 
54  FORCE_SIGNAL_LARM
55  };
56 
57 protected:
58  dynamicgraph::Vector state_;
59  dynamicgraph::Vector velocity_;
61  dynamicgraph::Vector vel_control_;
63  bool withForceSignals[4];
66  double timestep_;
67 
72  Vector upperTorque_;
75  Vector lowerTorque_;
77 public:
78  /* --- CONSTRUCTION --- */
79  Device(const std::string &name);
80  /* --- DESTRUCTION --- */
81  virtual ~Device();
82 
83  virtual void setStateSize(const unsigned int &size);
84  virtual void setState(const dynamicgraph::Vector &st);
85  void setVelocitySize(const unsigned int &size);
86  virtual void setVelocity(const dynamicgraph::Vector &vel);
87  virtual void setSecondOrderIntegration();
88  virtual void setNoIntegration();
89  virtual void setControlInputType(const std::string &cit);
90  virtual void increment(const double &dt = 5e-2);
91 
94  void setSanityCheck(const bool &enableCheck);
95  void setPositionBounds(const Vector &lower, const Vector &upper);
96  void setVelocityBounds(const Vector &lower, const Vector &upper);
97  void setTorqueBounds(const Vector &lower, const Vector &upper);
99 
100  PeriodicCall &periodicCallBefore() { return periodicCallBefore_; }
101  PeriodicCall &periodicCallAfter() { return periodicCallAfter_; }
102 
103 public: /* --- DISPLAY --- */
104  virtual void display(std::ostream &os) const;
105  virtual void cmdDisplay();
106  SOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
107  const Device &r) {
108  r.display(os);
109  return os;
110  }
111 
112 public: /* --- SIGNALS --- */
113  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> controlSIN;
114  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> attitudeSIN;
115  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> zmpSIN;
116 
119  dynamicgraph::Signal<dynamicgraph::Vector, int> stateSOUT;
120  dynamicgraph::Signal<dynamicgraph::Vector, int> velocitySOUT;
121  dynamicgraph::Signal<MatrixRotation, int> attitudeSOUT;
123  dynamicgraph::Signal<dynamicgraph::Vector, int> motorcontrolSOUT;
124  dynamicgraph::Signal<dynamicgraph::Vector, int> previousControlSOUT;
126  dynamicgraph::Signal<dynamicgraph::Vector, int> ZMPPreviousControllerSOUT;
128 
135  dynamicgraph::Signal<dynamicgraph::Vector, int> robotState_;
137  dynamicgraph::Signal<dynamicgraph::Vector, int> robotVelocity_;
139  dynamicgraph::Signal<dynamicgraph::Vector, int> *forcesSOUT[4];
142  dynamicgraph::Signal<dynamicgraph::Vector, int> pseudoTorqueSOUT;
144 
145 protected:
147  void integrateRollPitchYaw(dynamicgraph::Vector &state,
148  const dynamicgraph::Vector &control, double dt);
163  virtual void integrate(const double &dt);
164 
165 protected:
167  const MatrixHomogeneous &freeFlyerPose() const;
168 
169 public:
170  virtual void setRoot(const dynamicgraph::Matrix &root);
171 
172  virtual void setRoot(const MatrixHomogeneous &worldMwaist);
173 
174 private:
175  // Intermediate variable to avoid dynamic allocation
176  dynamicgraph::Vector forceZero6;
177 };
178 } // namespace sot
179 } // namespace dynamicgraph
180 
181 #endif /* #ifndef SOT_DEVICE_HH */
dynamicgraph::sot::Device::pseudoTorqueSOUT
dynamicgraph::Signal< dynamicgraph::Vector, int > pseudoTorqueSOUT
Definition: device.hh:142
dynamicgraph::sot::Device::FORCE_SIGNAL_LLEG
@ FORCE_SIGNAL_LLEG
Definition: device.hh:52
dynamicgraph::sot::Device::getClassName
virtual const std::string & getClassName(void) const
Definition: device.hh:48
dynamicgraph::sot::Device::robotState_
dynamicgraph::Signal< dynamicgraph::Vector, int > robotState_
Definition: device.hh:135
dynamicgraph::sot::MatrixHomogeneous
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
Definition: matrix-geometry.hh:74
SOT_CORE_EXPORT
#define SOT_CORE_EXPORT
Definition: api.hh:20
dynamicgraph::sot::Device
Definition: device.hh:45
dynamicgraph::sot::ControlInput
ControlInput
Define the type of input expected by the robot.
Definition: device.hh:32
dynamicgraph::sot::Device::ZMPPreviousControllerSOUT
dynamicgraph::Signal< dynamicgraph::Vector, int > ZMPPreviousControllerSOUT
The ZMP reference send by the previous controller.
Definition: device.hh:126
dynamicgraph
Definition: abstract-sot-external-interface.hh:17
dynamicgraph::sot::Device::vel_control_
dynamicgraph::Vector vel_control_
Definition: device.hh:61
dynamicgraph::sot::Device::periodicCallBefore
PeriodicCall & periodicCallBefore()
Definition: device.hh:100
dynamicgraph::sot::Device::state_
dynamicgraph::Vector state_
Definition: device.hh:58
dynamicgraph::sot::Device::robotVelocity_
dynamicgraph::Signal< dynamicgraph::Vector, int > robotVelocity_
Motor velocities.
Definition: device.hh:137
dynamicgraph::sot::Device::controlSIN
dynamicgraph::SignalPtr< dynamicgraph::Vector, int > controlSIN
Definition: device.hh:113
dynamicgraph::sot::ControlInput_s
const std::string ControlInput_s[]
Definition: device.hh:39
dynamicgraph::sot::Device::periodicCallAfter_
PeriodicCall periodicCallAfter_
Definition: device.hh:65
dynamicgraph::sot::Device::upperVelocity_
Vector upperVelocity_
Definition: device.hh:71
dynamicgraph::sot::CONTROL_INPUT_SIZE
@ CONTROL_INPUT_SIZE
Definition: device.hh:36
dynamicgraph::sot::Device::FORCE_SIGNAL_RLEG
@ FORCE_SIGNAL_RLEG
Definition: device.hh:51
dynamicgraph::sot::Device::upperTorque_
Vector upperTorque_
Definition: device.hh:72
dynamicgraph::sot::CONTROL_INPUT_NO_INTEGRATION
@ CONTROL_INPUT_NO_INTEGRATION
Definition: device.hh:33
dynamicgraph::sot::Device::periodicCallBefore_
PeriodicCall periodicCallBefore_
Definition: device.hh:64
dynamicgraph::sot::Device::periodicCallAfter
PeriodicCall & periodicCallAfter()
Definition: device.hh:101
dynamicgraph::sot::Device::lowerTorque_
Vector lowerTorque_
Definition: device.hh:75
dynamicgraph::sot::Device::lowerPosition_
Vector lowerPosition_
Definition: device.hh:73
dynamicgraph::sot::Device::FORCE_SIGNAL_RARM
@ FORCE_SIGNAL_RARM
Definition: device.hh:53
api.hh
dynamicgraph::sot::Device::ForceSignalSource
ForceSignalSource
Definition: device.hh:50
dynamicgraph::sot::Device::motorcontrolSOUT
dynamicgraph::Signal< dynamicgraph::Vector, int > motorcontrolSOUT
The current state of the robot from the command viewpoint.
Definition: device.hh:123
dynamicgraph::sot::Device::upperPosition_
Vector upperPosition_
Definition: device.hh:70
periodic-call.hh
dynamicgraph::sot::Device::display
virtual void display(std::ostream &os) const
dynamicgraph::sot::Device::previousControlSOUT
dynamicgraph::Signal< dynamicgraph::Vector, int > previousControlSOUT
Definition: device.hh:124
dynamicgraph::sot::Device::sanityCheck_
bool sanityCheck_
Definition: device.hh:60
dynamicgraph::sot::Device::timestep_
double timestep_
Definition: device.hh:66
dynamicgraph::sot::Device::velocity_
dynamicgraph::Vector velocity_
Definition: device.hh:59
matrix-geometry.hh
dynamicgraph::sot::Device::controlInputType_
ControlInput controlInputType_
Definition: device.hh:62
dynamicgraph::sot::Device::attitudeSOUT
dynamicgraph::Signal< MatrixRotation, int > attitudeSOUT
Definition: device.hh:121
dynamicgraph::sot::Device::ffPose_
MatrixHomogeneous ffPose_
Store Position of free flyer joint.
Definition: device.hh:150
dynamicgraph::sot::Device::zmpSIN
dynamicgraph::SignalPtr< dynamicgraph::Vector, int > zmpSIN
Definition: device.hh:115
dynamicgraph::sot::PeriodicCall
Definition: periodic-call.hh:36
dynamicgraph::sot::Device::lowerVelocity_
Vector lowerVelocity_
Definition: device.hh:74
dynamicgraph::sot::Device::stateSOUT
dynamicgraph::Signal< dynamicgraph::Vector, int > stateSOUT
Definition: device.hh:119
dynamicgraph::sot::Device::CLASS_NAME
static const std::string CLASS_NAME
Definition: device.hh:47
dynamicgraph::sot::Device::attitudeSIN
dynamicgraph::SignalPtr< dynamicgraph::Vector, int > attitudeSIN
Definition: device.hh:114
dynamicgraph::sot::CONTROL_INPUT_ONE_INTEGRATION
@ CONTROL_INPUT_ONE_INTEGRATION
Definition: device.hh:34
dynamicgraph::sot::CONTROL_INPUT_TWO_INTEGRATION
@ CONTROL_INPUT_TWO_INTEGRATION
Definition: device.hh:35
dynamicgraph::sot::Device::velocitySOUT
dynamicgraph::Signal< dynamicgraph::Vector, int > velocitySOUT
Definition: device.hh:120
dynamicgraph::sot::Device::operator<<
SOT_CORE_EXPORT friend std::ostream & operator<<(std::ostream &os, const Device &r)
Definition: device.hh:106