sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
numerical-difference.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2014-2017, Andrea Del Prete, Rohan Budhiraja LAAS-CNRS
3  *
4  */
5 
6 #include <Eigen/Dense>
7 
8 #include <dynamic-graph/factory.h>
9 #include <sot/core/debug.hh>
13 
14 namespace dynamicgraph {
15 namespace sot {
16 namespace torque_control {
17 
18 #define ALL_INPUT_SIGNALS m_xSIN
19 
20 #define ALL_OUTPUT_SIGNALS m_x_filteredSOUT << m_dxSOUT << m_ddxSOUT
21 
22 namespace dynamicgraph = ::dynamicgraph;
23 using namespace dynamicgraph;
24 using namespace dynamicgraph::command;
25 using namespace Eigen;
26 
29 typedef NumericalDifference EntityClassName;
30 
31 /* --- DG FACTORY ------------------------------------------------------- */
32 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NumericalDifference, "NumericalDifference");
33 
34 /* --- CONSTRUCTION ----------------------------------------------------- */
35 /* --- CONSTRUCTION ----------------------------------------------------- */
36 /* --- CONSTRUCTION ----------------------------------------------------- */
38  : Entity(name),
39  CONSTRUCT_SIGNAL_IN(x, dynamicgraph::Vector),
40  CONSTRUCT_SIGNAL_OUT(x_filtered, dynamicgraph::Vector, m_x_dx_ddxSINNER),
41  CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_x_dx_ddxSINNER),
42  CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_x_dx_ddxSINNER),
43  CONSTRUCT_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector, m_xSIN) {
44  Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
45 
46  /* Commands. */
47  addCommand("getTimestep", makeDirectGetter(*this, &m_dt, docDirectGetter("Control timestep [s ]", "double")));
48  addCommand("getDelay",
49  makeDirectGetter(*this, &m_delay, docDirectGetter("Delay in the estimation of signal x", "double")));
50  addCommand("getSize", makeDirectGetter(*this, &x_size, docDirectGetter("Size of the x signal", "int")));
51  addCommand("init", makeCommandVoid4(*this, &NumericalDifference::init,
52  docCommandVoid4("Initialize the estimator.", "Control timestep [s].",
53  "Size of the input signal x", "Estimation delay for signal x",
54  "Polynomial order")));
55 }
56 
57 /* --- COMMANDS ---------------------------------------------------------- */
58 /* --- COMMANDS ---------------------------------------------------------- */
59 /* --- COMMANDS ---------------------------------------------------------- */
60 void NumericalDifference::init(const double& timestep, const int& xSize, const double& delay, const int& polyOrder) {
61  assert(timestep > 0.0 && "Timestep should be > 0");
62  assert(delay >= 1.5 * timestep && "Estimation delay should be >= 1.5*timestep");
63  m_dt = timestep;
64  m_delay = delay;
65  x_size = xSize;
66  int winSizeEnc = (int)(2 * delay / m_dt);
67  assert(winSizeEnc >= 3 && "Estimation-window's length should be >= 3");
68 
69  if (polyOrder == 1)
70  m_filter = new LinEstimator(winSizeEnc, x_size, m_dt);
71  else if (polyOrder == 2)
72  m_filter = new QuadEstimator(winSizeEnc, x_size, m_dt);
73  else
74  SEND_MSG("Only polynomial orders 1 and 2 allowed. Reinitialize the filter", MSG_TYPE_INFO);
75  m_ddx_filter_std.resize(x_size);
76  m_dx_filter_std.resize(x_size);
77  m_x_filter_std.resize(x_size);
78  m_x_std.resize(x_size);
79 }
80 
81 /* --- SIGNALS ---------------------------------------------------------- */
82 /* --- SIGNALS ---------------------------------------------------------- */
83 /* --- SIGNALS ---------------------------------------------------------- */
84 
86 DEFINE_SIGNAL_INNER_FUNCTION(x_dx_ddx, dynamicgraph::Vector) {
87  sotDEBUG(15) << "Compute x_dx_ddx inner signal " << iter << std::endl;
88 
89  // read encoders and copy in std vector
90  const dynamicgraph::Vector& base_x = m_xSIN(iter);
91  COPY_VECTOR_TO_ARRAY(base_x, m_x_std);
92 
93  // Signal Filters
94  m_filter->estimate(m_x_filter_std, m_x_std);
95  m_filter->getEstimateDerivative(m_dx_filter_std, 1);
96  m_filter->getEstimateDerivative(m_ddx_filter_std, 2);
97 
98  // copy data in signal vector
99  if (s.size() != 3 * x_size) s.resize(3 * x_size);
100  for (int i = 0; i < x_size; i++) s(i) = m_x_filter_std[i];
101  for (int i = 0; i < x_size; i++) s(i + x_size) = m_dx_filter_std[i];
102  for (int i = 0; i < x_size; i++) s(i + 2 * x_size) = m_ddx_filter_std[i];
103 
104  return s;
105 }
106 
111 
112 DEFINE_SIGNAL_OUT_FUNCTION(x_filtered, dynamicgraph::Vector) {
113  sotDEBUG(15) << "Compute x_filtered output signal " << iter << std::endl;
114 
115  const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter);
116  if (s.size() != x_size) s.resize(x_size);
117  s = x_dx_ddx.head(x_size);
118  return s;
119 }
120 
121 DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector) {
122  sotDEBUG(15) << "Compute dx output signal " << iter << std::endl;
123 
124  const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter);
125  if (s.size() != x_size) s.resize(x_size);
126  s = x_dx_ddx.segment(x_size, x_size);
127  return s;
128 }
129 
130 DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector) {
131  sotDEBUG(15) << "Compute ddx output signal " << iter << std::endl;
132 
133  const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter);
134  if (s.size() != x_size) s.resize(x_size);
135  s = x_dx_ddx.segment(2 * x_size, x_size);
136  return s;
137 }
138 
139 void NumericalDifference::display(std::ostream& os) const {
140  os << "NumericalDifference " << getName() << ":\n";
141  try {
142  getProfiler().report_all(3, os);
143  } catch (ExceptionSignal e) {
144  }
145 }
146 
147 } // namespace torque_control
148 } // namespace sot
149 } // namespace dynamicgraph
#define ALL_OUTPUT_SIGNALS
double m_delay
sampling timestep of the input signal
EIGEN_MAKE_ALIGNED_OPERATOR_NEW NumericalDifference(const std::string &name)
#define ALL_INPUT_SIGNALS
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
void init(const double &timestep, const int &sigSize, const double &delay, const int &polyOrder)
#define COPY_VECTOR_TO_ARRAY(src, dest)
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition: treeview.dox:22