19 #ifndef __sot_talos_balance_nd_trajectory_generator_H__ 20 #define __sot_talos_balance_nd_trajectory_generator_H__ 27 # if defined (nd_position_controller_EXPORTS) 28 # define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllexport) 30 # define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllimport) 33 # define SOTNDTRAJECTORYGENERATOR_EXPORT 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> 50 #include <dynamic-graph/signal-helper.h> 51 #include <sot/core/matrix-geometry.hh> 54 #include "boost/assign.hpp" 59 namespace talos_balance {
66 :public::dynamicgraph::Entity
69 DYNAMIC_GRAPH_ENTITY_DECL();
75 void init(
const double& dt,
const unsigned int& n);
79 DECLARE_SIGNAL_IN(trigger,
bool);
91 void playTrajectoryFile(
const std::string& fileName);
93 void playSpline(
const std::string& fileName);
95 void setSpline(
const std::string& filename,
const double& timeToInitConf);
99 void getValue(
const int&
id);
106 void move(
const int&
id,
const double& xFinal,
const double& time);
112 void set(
const int& id,
const double& xVal);
119 void startSinusoid(
const int&
id,
const double& xFinal,
const double& time);
134 void startConstAcc(
const int&
id,
const double& xFinal,
const double& time);
144 void startLinearChirp(
const int&
id,
const double& xFinal,
const double& f0,
const double& f1,
const double& time);
150 void stop(
const int&
id);
153 virtual void display( std::ostream& os )
const;
177 std::vector<parametriccurves::AbstractCurve<double, dynamicgraph::sot::Vector1d>* >
m_currentTrajGen;
180 std::vector<parametriccurves::InfiniteSinusoid<double,1>* >
m_sinTrajGen;
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
AdmittanceControllerEndEffector EntityClassName
#define SOTNDTRAJECTORYGENERATOR_EXPORT
bool m_splineReady
last iter index
unsigned int m_n
current control loop time.
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
unsigned int m_iterLast
size of ouput vector
double m_t
control loop time step.
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.