sot-talos-balance  1.5.0
NdTrajectoryGenerator Class Reference

#include <sot/talos_balance/nd-trajectory-generator.hh>

Inheritance diagram for NdTrajectoryGenerator:
[legend]
Collaboration diagram for NdTrajectoryGenerator:
[legend]

Public Member Functions

 NdTrajectoryGenerator (const std::string &name)
 
 DECLARE_SIGNAL (x, OUT, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (initial_value, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_IN (trigger, bool)
 
 DECLARE_SIGNAL_OUT (dx, dynamicgraph::Vector)
 
 DECLARE_SIGNAL_OUT (ddx, dynamicgraph::Vector)
 
virtual void display (std::ostream &os) const
 
void getValue (const int &id)
 
void init (const double &dt, const unsigned int &n)
 
void move (const int &id, const double &xFinal, const double &time)
 
void playSpline (const std::string &fileName)
 
void playTrajectoryFile (const std::string &fileName)
 
void setSpline (const std::string &filename, const double &timeToInitConf)
 
void startConstAcc (const int &id, const double &xFinal, const double &time)
 
void startLinearChirp (const int &id, const double &xFinal, const double &f0, const double &f1, const double &time)
 
void startSinusoid (const int &id, const double &xFinal, const double &time)
 
void startSpline ()
 
void stop (const int &id)
 

Protected Types

enum  JTG_Status {
  JTG_STOP,
  JTG_SINUSOID,
  JTG_MIN_JERK,
  JTG_LIN_CHIRP,
  JTG_CONST_ACC,
  JTG_TEXT_FILE,
  JTG_SPLINE
}
 

Protected Member Functions

 DECLARE_SIGNAL_OUT_FUNCTION (x, dynamicgraph::Vector)
 

Protected Attributes

std::vector< parametriccurves::InfiniteConstAcc< double, 1 > *> m_constAccTrajGen
 
std::vector< parametriccurves::AbstractCurve< double, dynamicgraph::sot::Vector1d > *> m_currentTrajGen
 status of the component More...
 
double m_dt
 true if it is the first iteration, false otherwise More...
 
bool m_firstIter
 true if the entity has been successfully initialized More...
 
bool m_initSucceeded
 
unsigned int m_iterLast
 size of ouput vector More...
 
std::vector< parametriccurves::LinearChirp< double, 1 > * > m_linChirpTrajGen
 
std::vector< parametriccurves::MinimumJerk< double, 1 > *> m_minJerkTrajGen
 
unsigned int m_n
 current control loop time. More...
 
std::vector< parametriccurves::Constant< double, 1 > *> m_noTrajGen
 
std::vector< parametriccurves::InfiniteSinusoid< double, 1 > *> m_sinTrajGen
 
bool m_splineReady
 last iter index More...
 
parametriccurves::Spline< double, Eigen::Dynamic > * m_splineTrajGen
 
std::vector< JTG_Statusm_status
 true if the spline has been successfully loaded. More...
 
double m_t
 control loop time step. More...
 
parametriccurves::TextFile< double, Eigen::Dynamic > * m_textFileTrajGen
 

Detailed Description

Definition at line 65 of file nd-trajectory-generator.hh.

Member Enumeration Documentation

◆ JTG_Status

enum JTG_Status
protected
Enumerator
JTG_STOP 
JTG_SINUSOID 
JTG_MIN_JERK 
JTG_LIN_CHIRP 
JTG_CONST_ACC 
JTG_TEXT_FILE 
JTG_SPLINE 

Definition at line 150 of file nd-trajectory-generator.hh.

Constructor & Destructor Documentation

◆ NdTrajectoryGenerator()

Member Function Documentation

◆ DECLARE_SIGNAL()

DECLARE_SIGNAL ( ,
OUT  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [1/2]

DECLARE_SIGNAL_IN ( initial_value  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_IN() [2/2]

DECLARE_SIGNAL_IN ( trigger  ,
bool   
)

◆ DECLARE_SIGNAL_OUT() [1/2]

DECLARE_SIGNAL_OUT ( dx  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT() [2/2]

DECLARE_SIGNAL_OUT ( ddx  ,
dynamicgraph::Vector   
)

◆ DECLARE_SIGNAL_OUT_FUNCTION()

DECLARE_SIGNAL_OUT_FUNCTION ( ,
dynamicgraph::Vector   
)
protected

◆ display()

void display ( std::ostream &  os) const
virtual

Definition at line 666 of file nd-trajectory-generator.cpp.

◆ getValue()

void getValue ( const int &  id)

Print the current value of the specified component.

Definition at line 364 of file nd-trajectory-generator.cpp.

References NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_n, and NdTrajectoryGenerator::m_t.

Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().

◆ init()

◆ move()

void move ( const int &  id,
const double &  xFinal,
const double &  time 
)

Move a component to a position with a minimum-jerk trajectory.

Parameters
idinteger index.
xFinalThe desired final position of the component.
timeThe time to go from the current position to xFinal [sec].

Definition at line 591 of file nd-trajectory-generator.cpp.

References NdTrajectoryGenerator::JTG_MIN_JERK, NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_minJerkTrajGen, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_noTrajGen, NdTrajectoryGenerator::m_status, and NdTrajectoryGenerator::m_t.

Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().

◆ playSpline()

void playSpline ( const std::string &  fileName)

◆ playTrajectoryFile()

◆ setSpline()

◆ startConstAcc()

void startConstAcc ( const int &  id,
const double &  xFinal,
const double &  time 
)

Start an infinite triangle trajectory.

Parameters
idinteger index.
xFinalThe position of the component corresponding to the max amplitude of the trajectory.
timeThe time to go from the current position to xFinal [sec]. Start an infinite trajectory with piece-wise constant acceleration.
idinteger index.
xFinalThe position of the component corresponding to the max amplitude of the trajectory.
timeThe time to go from the current position to xFinal [sec].
TaccThe time during witch acceleration is keept constant [sec].

Definition at line 536 of file nd-trajectory-generator.cpp.

References NdTrajectoryGenerator::JTG_CONST_ACC, NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::m_constAccTrajGen, NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_noTrajGen, NdTrajectoryGenerator::m_status, and NdTrajectoryGenerator::m_t.

Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().

◆ startLinearChirp()

void startLinearChirp ( const int &  id,
const double &  xFinal,
const double &  f0,
const double &  f1,
const double &  time 
)

Start a linear-chirp trajectory, that is a sinusoidal trajectory with frequency being a linear function of time.

Parameters
idinteger index.
xFinalThe position of the component corresponding to the max amplitude of the sinusoid [rad].
f0The initial (min) frequency of the sinusoid [Hz]
f1The final (max) frequency of the sinusoid [Hz]
timeThe time to get from f0 to f1 [sec]

Definition at line 558 of file nd-trajectory-generator.cpp.

References NdTrajectoryGenerator::JTG_LIN_CHIRP, NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_linChirpTrajGen, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_noTrajGen, NdTrajectoryGenerator::m_status, and NdTrajectoryGenerator::m_t.

Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().

◆ startSinusoid()

void startSinusoid ( const int &  id,
const double &  xFinal,
const double &  time 
)

Start an infinite sinusoidal trajectory.

Parameters
idinteger index.
xFinalThe position of the component corresponding to the max amplitude of the sinusoid.
timeThe time to go from the current position to xFinal [sec].

Definition at line 485 of file nd-trajectory-generator.cpp.

References NdTrajectoryGenerator::JTG_SINUSOID, NdTrajectoryGenerator::JTG_STOP, NdTrajectoryGenerator::m_currentTrajGen, NdTrajectoryGenerator::m_initSucceeded, NdTrajectoryGenerator::m_n, NdTrajectoryGenerator::m_noTrajGen, NdTrajectoryGenerator::m_sinTrajGen, NdTrajectoryGenerator::m_status, and NdTrajectoryGenerator::m_t.

Referenced by NdTrajectoryGenerator::NdTrajectoryGenerator().

◆ startSpline()

◆ stop()

void stop ( const int &  id)

Member Data Documentation

◆ m_constAccTrajGen

std::vector<parametriccurves::InfiniteConstAcc<double,1>* > m_constAccTrajGen
protected

◆ m_currentTrajGen

◆ m_dt

double m_dt
protected

true if it is the first iteration, false otherwise

Definition at line 164 of file nd-trajectory-generator.hh.

Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and NdTrajectoryGenerator::init().

◆ m_firstIter

bool m_firstIter
protected

true if the entity has been successfully initialized

Definition at line 163 of file nd-trajectory-generator.hh.

Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and NdTrajectoryGenerator::init().

◆ m_initSucceeded

◆ m_iterLast

unsigned int m_iterLast
protected

size of ouput vector

Definition at line 167 of file nd-trajectory-generator.hh.

Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().

◆ m_linChirpTrajGen

std::vector<parametriccurves::LinearChirp<double,1>*> m_linChirpTrajGen
protected

◆ m_minJerkTrajGen

std::vector<parametriccurves::MinimumJerk<double, 1>* > m_minJerkTrajGen
protected

◆ m_n

◆ m_noTrajGen

◆ m_sinTrajGen

std::vector<parametriccurves::InfiniteSinusoid<double,1>* > m_sinTrajGen
protected

◆ m_splineReady

◆ m_splineTrajGen

parametriccurves::Spline<double, Eigen::Dynamic>* m_splineTrajGen
protected

◆ m_status

◆ m_t

◆ m_textFileTrajGen

parametriccurves::TextFile<double, Eigen::Dynamic>* m_textFileTrajGen
protected

The documentation for this class was generated from the following files: