device.hh

Go to the documentation of this file.

/*
 * Copyright 2010,
 * Florent Lamiraux
 *
 * CNRS
 *
 */
 
#ifndef SOT_DEVICE_HH
#define SOT_DEVICE_HH
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#include <pinocchio/fwd.hpp>
 
/* -- MaaL --- */
#include <dynamic-graph/linear-algebra.h>
 
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
 
#include <sot/core/matrix-geometry.hh>
 
#include "sot/core/api.hh"
#include "sot/core/periodic-call.hh"
 
namespace dynamicgraph {
namespace sot {
 
enum ControlInput {
  CONTROL_INPUT_NO_INTEGRATION = 0,
  CONTROL_INPUT_ONE_INTEGRATION = 1,
  CONTROL_INPUT_TWO_INTEGRATION = 2,
  CONTROL_INPUT_SIZE = 3
};
 
const std::string ControlInput_s[] = {"noInteg", "oneInteg", "twoInteg"};
 
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
class SOT_CORE_EXPORT Device : public Entity {
 public:
  static const std::string CLASS_NAME;
  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
 
  enum ForceSignalSource {
    FORCE_SIGNAL_RLEG,
    FORCE_SIGNAL_LLEG,
    FORCE_SIGNAL_RARM,
    FORCE_SIGNAL_LARM
  };
 
 protected:
  dynamicgraph::Vector state_;
  dynamicgraph::Vector velocity_;
  bool sanityCheck_;
  dynamicgraph::Vector vel_control_;
  ControlInput controlInputType_;
  bool withForceSignals[4];
  PeriodicCall periodicCallBefore_;
  PeriodicCall periodicCallAfter_;
  double timestep_;
 
  Vector upperPosition_;
  Vector upperVelocity_;
  Vector upperTorque_;
  Vector lowerPosition_;
  Vector lowerVelocity_;
  Vector lowerTorque_;
 public:
  /* --- CONSTRUCTION --- */
  Device(const std::string &name);
  /* --- DESTRUCTION --- */
  virtual ~Device();
 
  virtual void setStateSize(const unsigned int &size);
  virtual void setState(const dynamicgraph::Vector &st);
  void setVelocitySize(const unsigned int &size);
  virtual void setVelocity(const dynamicgraph::Vector &vel);
  virtual void setSecondOrderIntegration();
  virtual void setNoIntegration();
  virtual void setControlInputType(const std::string &cit);
  virtual void increment(const double &dt = 5e-2);
 
  void setSanityCheck(const bool &enableCheck);
  void setPositionBounds(const Vector &lower, const Vector &upper);
  void setVelocityBounds(const Vector &lower, const Vector &upper);
  void setTorqueBounds(const Vector &lower, const Vector &upper);
 
  PeriodicCall &periodicCallBefore() { return periodicCallBefore_; }
  PeriodicCall &periodicCallAfter() { return periodicCallAfter_; }
 
 public: /* --- DISPLAY --- */
  virtual void display(std::ostream &os) const;
  virtual void cmdDisplay();
  SOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
                                                  const Device &r) {
    r.display(os);
    return os;
  }
 
 public: /* --- SIGNALS --- */
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> controlSIN;
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> attitudeSIN;
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> zmpSIN;
 
  dynamicgraph::Signal<dynamicgraph::Vector, int> stateSOUT;
  dynamicgraph::Signal<dynamicgraph::Vector, int> velocitySOUT;
  dynamicgraph::Signal<MatrixRotation, int> attitudeSOUT;
  dynamicgraph::Signal<dynamicgraph::Vector, int> motorcontrolSOUT;
  dynamicgraph::Signal<dynamicgraph::Vector, int> previousControlSOUT;
  dynamicgraph::Signal<dynamicgraph::Vector, int> ZMPPreviousControllerSOUT;
 
  dynamicgraph::Signal<dynamicgraph::Vector, int> robotState_;
  dynamicgraph::Signal<dynamicgraph::Vector, int> robotVelocity_;
  dynamicgraph::Signal<dynamicgraph::Vector, int> *forcesSOUT[4];
  dynamicgraph::Signal<dynamicgraph::Vector, int> pseudoTorqueSOUT;
 
 protected:
  void integrateRollPitchYaw(dynamicgraph::Vector &state,
                             const dynamicgraph::Vector &control, double dt);
  MatrixHomogeneous ffPose_;
  virtual void integrate(const double &dt);
 
 protected:
  const MatrixHomogeneous &freeFlyerPose() const;
 
 public:
  virtual void setRoot(const dynamicgraph::Matrix &root);
 
  virtual void setRoot(const MatrixHomogeneous &worldMwaist);
 
 private:
  // Intermediate variable to avoid dynamic allocation
  dynamicgraph::Vector forceZero6;
};
}  // namespace sot
}  // namespace dynamicgraph
 
#endif /* #ifndef SOT_DEVICE_HH */