sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
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 <dynamic-graph/factory.h>
7
8#include <Eigen/Dense>
9#include <sot/core/debug.hh>
13
14namespace dynamicgraph {
15namespace sot {
16namespace torque_control {
17
18#define ALL_INPUT_SIGNALS m_xSIN
19
20#define ALL_OUTPUT_SIGNALS m_x_filteredSOUT << m_dxSOUT << m_ddxSOUT
21
22namespace dynamicgraph = ::dynamicgraph;
23using namespace dynamicgraph;
24using namespace dynamicgraph::command;
25using namespace Eigen;
26
29typedef NumericalDifference EntityClassName;
30
31/* --- DG FACTORY ------------------------------------------------------- */
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(
48 "getTimestep",
49 makeDirectGetter(*this, &m_dt,
50 docDirectGetter("Control timestep [s ]", "double")));
51 addCommand(
52 "getDelay",
53 makeDirectGetter(
54 *this, &m_delay,
55 docDirectGetter("Delay in the estimation of signal x", "double")));
56 addCommand("getSize",
57 makeDirectGetter(*this, &x_size,
58 docDirectGetter("Size of the x signal", "int")));
59 addCommand("init",
60 makeCommandVoid4(*this, &NumericalDifference::init,
61 docCommandVoid4("Initialize the estimator.",
62 "Control timestep [s].",
63 "Size of the input signal x",
64 "Estimation delay for signal x",
65 "Polynomial order")));
66}
67
68/* --- COMMANDS ---------------------------------------------------------- */
69/* --- COMMANDS ---------------------------------------------------------- */
70/* --- COMMANDS ---------------------------------------------------------- */
71void NumericalDifference::init(const double& timestep, const int& xSize,
72 const double& delay, const int& polyOrder) {
73 assert(timestep > 0.0 && "Timestep should be > 0");
74 assert(delay >= 1.5 * timestep &&
75 "Estimation delay should be >= 1.5*timestep");
76 m_dt = timestep;
77 m_delay = delay;
78 x_size = xSize;
79 int winSizeEnc = (int)(2 * delay / m_dt);
80 assert(winSizeEnc >= 3 && "Estimation-window's length should be >= 3");
81
82 if (polyOrder == 1)
83 m_filter = new LinEstimator(winSizeEnc, x_size, m_dt);
84 else if (polyOrder == 2)
85 m_filter = new QuadEstimator(winSizeEnc, x_size, m_dt);
86 else
87 SEND_MSG("Only polynomial orders 1 and 2 allowed. Reinitialize the filter",
88 MSG_TYPE_INFO);
90 m_dx_filter_std.resize(x_size);
91 m_x_filter_std.resize(x_size);
92 m_x_std.resize(x_size);
93}
94
95/* --- SIGNALS ---------------------------------------------------------- */
96/* --- SIGNALS ---------------------------------------------------------- */
97/* --- SIGNALS ---------------------------------------------------------- */
98
100DEFINE_SIGNAL_INNER_FUNCTION(x_dx_ddx, dynamicgraph::Vector) {
101 sotDEBUG(15) << "Compute x_dx_ddx inner signal " << iter << std::endl;
102
103 // read encoders and copy in std vector
104 const dynamicgraph::Vector& base_x = m_xSIN(iter);
105 COPY_VECTOR_TO_ARRAY(base_x, m_x_std);
106
107 // Signal Filters
108 m_filter->estimate(m_x_filter_std, m_x_std);
109 m_filter->getEstimateDerivative(m_dx_filter_std, 1);
110 m_filter->getEstimateDerivative(m_ddx_filter_std, 2);
111
112 // copy data in signal vector
113 if (s.size() != 3 * x_size) s.resize(3 * x_size);
114 for (int i = 0; i < x_size; i++) s(i) = m_x_filter_std[i];
115 for (int i = 0; i < x_size; i++) s(i + x_size) = m_dx_filter_std[i];
116 for (int i = 0; i < x_size; i++) s(i + 2 * x_size) = m_ddx_filter_std[i];
117
118 return s;
119}
120
126
127DEFINE_SIGNAL_OUT_FUNCTION(x_filtered, dynamicgraph::Vector) {
128 sotDEBUG(15) << "Compute x_filtered output signal " << iter << std::endl;
129
130 const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter);
131 if (s.size() != x_size) s.resize(x_size);
132 s = x_dx_ddx.head(x_size);
133 return s;
134}
135
136DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector) {
137 sotDEBUG(15) << "Compute dx output signal " << iter << std::endl;
138
139 const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter);
140 if (s.size() != x_size) s.resize(x_size);
141 s = x_dx_ddx.segment(x_size, x_size);
142 return s;
143}
144
145DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector) {
146 sotDEBUG(15) << "Compute ddx output signal " << iter << std::endl;
147
148 const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter);
149 if (s.size() != x_size) s.resize(x_size);
150 s = x_dx_ddx.segment(2 * x_size, x_size);
151 return s;
152}
153
154void NumericalDifference::display(std::ostream& os) const {
155 os << "NumericalDifference " << getName() << ":\n";
156 try {
157 getProfiler().report_all(3, os);
158 } catch (ExceptionSignal e) {
159 }
160}
161
162} // namespace torque_control
163} // namespace sot
164} // namespace dynamicgraph
EIGEN_MAKE_ALIGNED_OPERATOR_NEW NumericalDifference(const std::string &name)
void init(const double &timestep, const int &sigSize, const double &delay, const int &polyOrder)
double m_delay
sampling timestep of the input signal
#define ALL_OUTPUT_SIGNALS
#define ALL_INPUT_SIGNALS
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition: treeview.dox:22
#define COPY_VECTOR_TO_ARRAY(src, dest)