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/abstract-sot-external-interface.hh>
#include <sot/core/matrix-geometry.hh>

#include "sot/core/api.hh"

namespace dynamicgraph {
namespace sot {

enum ControlInput {
  POSITION_CONTROL = 0,
  VELOCITY_CONTROL = 1,
  TORQUE_CONTROL = 2,
  CONTROL_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];
  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);
  // Set number of joints that are controlled by the device.
  void setControlSize(const int &size);
  // Get the number of joints that are controlled by the device.
  int getControlSize() const;
  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);
  void getControl(std::map<std::string, ControlValues> &anglesOut,
                  const double &period);

  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);

 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;

 public:
  virtual void setRoot(const dynamicgraph::Matrix &root);

  virtual void setRoot(const MatrixHomogeneous &worldMwaist);

 private:
  int lastTimeControlWasRead_;
  int controlSize_;
  // Intermediate variable to avoid dynamic allocation
  dynamicgraph::Vector forceZero6;
};
}  // namespace sot
}  // namespace dynamicgraph

#endif /* #ifndef SOT_DEVICE_HH */