sot-hrp2-device.hh 2.64 KB
Newer Older
1 2 3 4 5 6 7 8
/*
 * Copyright 2011,
 *
 * Olivier Stasse
 *
 * LAAS, CNRS
 *
 * This file is part of HRP2Controller.
andreadelprete's avatar
andreadelprete committed
9
 * HRP2Controller is not a free software,
10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26
 * it contains information related to HRP-2 which involves
 * that you either purchased the proper license to havec access to
 * those informations, or that you signed the appropriate
 * Non-Disclosure agreement.
 *
 *
 */

#ifndef _SOT_HRP2Device_H_
#define _SOT_HRP2Device_H_

#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/device.hh>
#include <sot/core/abstract-sot-external-interface.hh>
27
#include <sot/core/matrix-geometry.hh>
28 29

namespace dgsot=dynamicgraph::sot;
30
namespace dg=dynamicgraph;
31 32

class SoTHRP2Device: public 
andreadelprete's avatar
andreadelprete committed
33
    dgsot::Device
34
{
andreadelprete's avatar
andreadelprete committed
35
public:
36 37 38 39

  static const std::string CLASS_NAME;
  static const double TIMESTEP_DEFAULT;

andreadelprete's avatar
andreadelprete committed
40 41 42
  virtual const std::string& getClassName () const
  {
    return CLASS_NAME;
43 44 45 46 47
  }
  
  SoTHRP2Device(std::string RobotName);
  virtual ~SoTHRP2Device();
  
48 49
  void setSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);

50 51 52 53 54 55 56 57 58 59 60 61 62 63
  void setupSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);

  void nominalSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);

  void cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);

  void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);

protected:

  /// \brief Current integration step.
  double timestep_;
  
  /// \brief Previous robot configuration.
64
  dg::Vector previousState_;
65 66 67 68 69 70 71
  
  /// \brief Robot state provided by OpenHRP.
  ///
  /// This corresponds to the real encoders values and take into
  /// account the stabilization step. Therefore, this usually
  /// does *not* match the state control input signal.
  ///
72
  dynamicgraph::Signal<dg::Vector, int> robotState_;
73

74
  /// Accelerations measured by accelerometers
75
  dynamicgraph::Signal <dg::Vector, int> accelerometerSOUT_;
76
  /// Rotation velocity measured by gyrometers
77
  dynamicgraph::Signal <dg::Vector, int> gyrometerSOUT_;
78
  /// motor currents
79
  dynamicgraph::Signal <dg::Vector, int> currentSOUT_;
80 81

  /// proportional and derivative position-control gains
82 83
  dynamicgraph::Signal <dg::Vector, int> p_gainsSOUT_;
  dynamicgraph::Signal <dg::Vector, int> d_gainsSOUT_;
84

85
  /// Intermediate variables to avoid allocation during control
86 87
  dg::Vector mlforces;
  dg::Vector mlRobotState;
88
  dgsot::MatrixRotation pose;
89 90
  dg::Vector accelerometer_;
  dg::Vector gyrometer_;
91
  std::vector<double> baseff_;
92 93 94 95
  dg::Vector torques_;
  dg::Vector currents_;
  dg::Vector p_gains_;
  dg::Vector d_gains_;
96
};
97
#endif /* _SOT_HRP2Device_H_*/