sot-talos-balance  1.7.0
nd-trajectory-generator.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2017,Thomas Flayols, LAAS-CNRS
3  * File derived from sot-torque-control package available on
4  * https://github.com/stack-of-tasks/sot-torque-control
5  *
6  * This file is part of sot-talos-balance.
7  * sot-talos-balance is free software: you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public License
9  * as published by the Free Software Foundation, either version 3 of
10  * the License, or (at your option) any later version.
11  * sot-torque-control is distributed in the hope that it will be
12  * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU Lesser General Public License for more details. You should
15  * have received a copy of the GNU Lesser General Public License along
16  * with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
17  */
18 
19 #ifndef __sot_talos_balance_nd_trajectory_generator_H__
20 #define __sot_talos_balance_nd_trajectory_generator_H__
21 
22 /* --------------------------------------------------------------------- */
23 /* --- API ------------------------------------------------------------- */
24 /* --------------------------------------------------------------------- */
25 
26 #if defined (WIN32)
27 # if defined (nd_position_controller_EXPORTS)
28 # define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllexport)
29 # else
30 # define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllimport)
31 # endif
32 #else
33 # define SOTNDTRAJECTORYGENERATOR_EXPORT
34 #endif
35 
36 
37 /* --------------------------------------------------------------------- */
38 /* --- INCLUDE --------------------------------------------------------- */
39 /* --------------------------------------------------------------------- */
40 
41 
42 #include <parametric-curves/spline.hpp>
43 #include <parametric-curves/constant.hpp>
44 #include <parametric-curves/text-file.hpp>
45 #include <parametric-curves/minimum-jerk.hpp>
46 #include <parametric-curves/linear-chirp.hpp>
47 #include <parametric-curves/infinite-sinusoid.hpp>
48 #include <parametric-curves/infinite-const-acc.hpp>
49 
50 #include <dynamic-graph/signal-helper.h>
51 #include <sot/core/matrix-geometry.hh>
52 
53 #include <map>
54 #include "boost/assign.hpp"
55 
56 
57 namespace dynamicgraph {
58  namespace sot {
59  namespace talos_balance {
60 
61  /* --------------------------------------------------------------------- */
62  /* --- CLASS ----------------------------------------------------------- */
63  /* --------------------------------------------------------------------- */
64 
66  :public::dynamicgraph::Entity
67  {
69  DYNAMIC_GRAPH_ENTITY_DECL();
70 
71  public:
72  /* --- CONSTRUCTOR ---- */
73  NdTrajectoryGenerator( const std::string & name );
74 
75  void init(const double& dt, const unsigned int& n);
76 
77  /* --- SIGNALS --- */
78  DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector);
79  DECLARE_SIGNAL_IN(trigger, bool);
80  DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector);
81  DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
82  DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
83 
84  protected:
85  DECLARE_SIGNAL_OUT_FUNCTION(x, dynamicgraph::Vector);
86 
87  public:
88 
89  /* --- COMMANDS --- */
90 
91  void playTrajectoryFile(const std::string& fileName);
92 
93  void playSpline(const std::string& fileName);
94 
95  void setSpline(const std::string& filename, const double& timeToInitConf);
96  void startSpline();
97 
99  void getValue(const int& id);
100 
106  void move(const int& id, const double& xFinal, const double& time);
107 
112  void set(const int& id, const double& xVal);
113 
119  void startSinusoid(const int& id, const double& xFinal, const double& time);
120 
126  //void startTriangle(const int& id, const double& xFinal, const double& time, const double& Tacc);
127 
134  void startConstAcc(const int& id, const double& xFinal, const double& time);
135 
144  void startLinearChirp(const int& id, const double& xFinal, const double& f0, const double& f1, const double& time);
145 
150  void stop(const int& id);
151 
152  /* --- ENTITY INHERITANCE --- */
153  virtual void display( std::ostream& os ) const;
154 
155  protected:
157  {
162  //JTG_TRIANGLE,
165  JTG_SPLINE
166  };
167 
169  bool m_firstIter;
170  double m_dt;
171  double m_t;
172  unsigned int m_n;
173  unsigned int m_iterLast;
175 
176  std::vector<JTG_Status> m_status;
177  std::vector<parametriccurves::AbstractCurve<double, dynamicgraph::sot::Vector1d>* > m_currentTrajGen;
178  std::vector<parametriccurves::Constant<double, 1>* > m_noTrajGen;
179  std::vector<parametriccurves::MinimumJerk<double, 1>* > m_minJerkTrajGen;
180  std::vector<parametriccurves::InfiniteSinusoid<double,1>* > m_sinTrajGen;
181  std::vector<parametriccurves::LinearChirp<double,1>*> m_linChirpTrajGen;
182  //std::vector<parametriccurves::InfiniteTriangular<double,1>* > m_triangleTrajGen;
183  std::vector<parametriccurves::InfiniteConstAcc<double,1>* > m_constAccTrajGen;
184  parametriccurves::TextFile<double, Eigen::Dynamic>* m_textFileTrajGen;
185  parametriccurves::Spline<double, Eigen::Dynamic>* m_splineTrajGen;
186 
187  }; // class NdTrajectoryGenerator
188 
189  } // namespace talos_balance
190  } // namespace sot
191 } // namespace dynamicgraph
192 
193 
194 
195 #endif // #ifndef __sot_talos_balance_nd_trajectory_generator_H__
std::vector< parametriccurves::LinearChirp< double, 1 > * > m_linChirpTrajGen
std::vector< parametriccurves::InfiniteConstAcc< double, 1 > *> m_constAccTrajGen
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
#define SOTNDTRAJECTORYGENERATOR_EXPORT
parametriccurves::Spline< double, Eigen::Dynamic > * m_splineTrajGen
double m_dt
true if it is the first iteration, false otherwise
std::vector< parametriccurves::InfiniteSinusoid< double, 1 > *> m_sinTrajGen
std::vector< parametriccurves::AbstractCurve< double, dynamicgraph::sot::Vector1d > *> m_currentTrajGen
status of the component
parametriccurves::TextFile< double, Eigen::Dynamic > * m_textFileTrajGen
std::vector< parametriccurves::MinimumJerk< double, 1 > *> m_minJerkTrajGen
std::vector< parametriccurves::Constant< double, 1 > *> m_noTrajGen
bool m_firstIter
true if the entity has been successfully initialized
std::vector< JTG_Status > m_status
true if the spline has been successfully loaded.