sot-torque-control  1.6.1
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
torque-offset-estimator.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2014-2017, Andrea Del Prete, Rohan Budhiraja LAAS-CNRS
3  *
4  */
5 
6 #ifndef __sot_torque_control_TorqueOffsetEstimator_H__
7 #define __sot_torque_control_TorqueOffsetEstimator_H__
8 /* --------------------------------------------------------------------- */
9 /* --- API ------------------------------------------------------------- */
10 /* --------------------------------------------------------------------- */
11 
12 #if defined(WIN32)
13 #if defined(torque_offset_estimator_EXPORTS)
14 #define TORQUEOFFSETESTIMATOR_EXPORT __declspec(dllexport)
15 #else
16 #define TORQUEOFFSETESTIMATOR_EXPORT __declspec(dllimport)
17 #endif
18 #else
19 #define TORQUEOFFSETESTIMATOR_EXPORT
20 #endif
21 
22 //#define VP_DEBUG 1 /// enable debug output
23 //#define VP_DEBUG_MODE 20
24 
25 /* --------------------------------------------------------------------- */
26 /* --- INCLUDE --------------------------------------------------------- */
27 /* --------------------------------------------------------------------- */
28 
29 #include <boost/circular_buffer.hpp>
30 #include <Eigen/StdVector>
31 
32 /*Motor model*/
33 #include <pinocchio/multibody/model.hpp>
34 #include <pinocchio/parsers/urdf.hpp>
35 #include <pinocchio/algorithm/rnea.hpp>
36 #include <pinocchio/algorithm/kinematics.hpp>
37 
38 /* HELPER */
39 #include <dynamic-graph/signal-helper.h>
40 #include <sot/core/matrix-geometry.hh>
41 #include <sot/core/robot-utils.hh>
42 #include <sot/core/stop-watch.hh>
43 
44 /*Motor model*/
46 
47 namespace dynamicgraph {
48 namespace sot {
49 namespace torque_control {
50 
51 class TORQUEOFFSETESTIMATOR_EXPORT TorqueOffsetEstimator : public ::dynamicgraph::Entity {
52  DYNAMIC_GRAPH_ENTITY_DECL();
53 
54  public: /* --- SIGNALS --- */
55  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
56  typedef int dummy;
57  typedef std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > stdAlignedVector;
58 
60  TorqueOffsetEstimator(const std::string& name);
61  void init(const std::string& urdfFile, const Eigen::Matrix4d& _m_torso_X_imu, const double& gyro_epsilon,
62  const std::string& ffJointName, const std::string& torsoJointName);
63  void computeOffset(const int& nIterations, const double& epsilon);
64 
65  virtual void display(std::ostream& os) const;
66 
67  DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
68  DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
69  DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
70  DECLARE_SIGNAL_IN(jointTorques, dynamicgraph::Vector);
71  DECLARE_SIGNAL_INNER(collectSensorData, dummy);
72  DECLARE_SIGNAL_OUT(jointTorquesEstimated, dynamicgraph::Vector);
73 
74  protected:
75  RobotUtilShrPtr m_robot_util;
76  pinocchio::Model m_model;
77  pinocchio::Data* m_data;
78  int n_iterations; // Number of iterations to consider
79  double epsilon;
80  double gyro_epsilon;
81 
82  pinocchio::JointIndex ffIndex, torsoIndex; // Index of the free-flyer and torso frames
83  Eigen::VectorXd jointTorqueOffsets;
84  pinocchio::SE3 m_torso_X_imu; // Definition of the imu in the chest frame.
85 
86  // stdAlignedVector encSignals;
87  // stdAlignedVector accSignals;
88  // stdAlignedVector gyrSignals;
89  // stdAlignedVector tauSignals;
90 
91  // stdAlignedVector stdVecJointTorqueOffsets;
92 
93  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
94  logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
95  }
96 
97  private:
98  enum { PRECOMPUTATION, INPROGRESS, COMPUTED } sensor_offset_status;
99 
100  // void calculateSensorOffsets();
101  int current_progress;
102 }; // class TorqueOffsetEstimator
103 
104 } // namespace torque_control
105 } // namespace sot
106 } // namespace dynamicgraph
107 
108 #endif // #ifndef __sot_torque_control_TorqueOffsetEstimator_H__
dynamicgraph::sot::torque_control::TorqueOffsetEstimator::epsilon
double epsilon
Definition: torque-offset-estimator.hh:79
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::TorqueOffsetEstimator::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Definition: torque-offset-estimator.hh:93
dynamicgraph::sot::torque_control::TorqueOffsetEstimator::jointTorqueOffsets
Eigen::VectorXd jointTorqueOffsets
Definition: torque-offset-estimator.hh:83
motor-model.hh
torque_control
Definition: __init__.py:1
TORQUEOFFSETESTIMATOR_EXPORT
#define TORQUEOFFSETESTIMATOR_EXPORT
Definition: torque-offset-estimator.hh:19
dynamicgraph::sot::torque_control::TorqueOffsetEstimator::n_iterations
int n_iterations
Pinocchio robot data.
Definition: torque-offset-estimator.hh:78
dynamicgraph::sot::torque_control::TorqueOffsetEstimator::m_torso_X_imu
pinocchio::SE3 m_torso_X_imu
Definition: torque-offset-estimator.hh:84
dynamicgraph::sot::torque_control::TorqueOffsetEstimator::gyro_epsilon
double gyro_epsilon
Definition: torque-offset-estimator.hh:80
dynamicgraph::sot::torque_control::TorqueOffsetEstimator
Definition: torque-offset-estimator.hh:51
dynamicgraph::sot::torque_control::dummy
int dummy
Definition: torque-offset-estimator.cpp:30
dynamicgraph::sot::torque_control::TorqueOffsetEstimator::m_model
pinocchio::Model m_model
Definition: torque-offset-estimator.hh:76
dynamicgraph::sot::torque_control::TorqueOffsetEstimator::torsoIndex
pinocchio::JointIndex torsoIndex
Definition: torque-offset-estimator.hh:82
dynamicgraph::sot::torque_control::TorqueOffsetEstimator::m_robot_util
RobotUtilShrPtr m_robot_util
Definition: torque-offset-estimator.hh:75
dynamicgraph::sot::torque_control::TorqueOffsetEstimator::m_data
pinocchio::Data * m_data
Pinocchio robot model.
Definition: torque-offset-estimator.hh:77
dynamicgraph::sot::torque_control::TorqueOffsetEstimator::stdAlignedVector
std::vector< Eigen::VectorXd, Eigen::aligned_allocator< Eigen::VectorXd > > stdAlignedVector
Definition: torque-offset-estimator.hh:57
dynamicgraph::sot::torque_control::TorqueOffsetEstimator::dummy
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef int dummy
Definition: torque-offset-estimator.hh:56