kinematic-planner.hh
Go to the documentation of this file.
1 //
2 // Copyright (C) 2016 LAAS-CNRS
3 //
4 // Author: Rohan Budhiraja
5 //
6 
7 #ifndef SOT_TOOLS_KINEMATIC_PLANNER_HH
8 #define SOT_TOOLS_KINEMATIC_PLANNER_HH
9 
10 /* STD */
11 #include <string>
12 #include <sstream>
13 #include <list>
14 #include <complex>
15 
16 /* dynamic-graph */
17 #include <dynamic-graph/entity.h>
18 #include <dynamic-graph/factory.h>
19 #include <dynamic-graph/signal-base.h>
20 #include <dynamic-graph/signal-ptr.h>
21 #include <dynamic-graph/signal-time-dependent.h>
22 
23 #include <dynamic-graph/linear-algebra.h>
24 #include <sot/core/debug.hh>
25 
26 /*Eigen*/
27 #include <Eigen/StdVector>
28 #include <unsupported/Eigen/FFT>
29 #include <unsupported/Eigen/Splines>
30 #include <unsupported/Eigen/MatrixFunctions>
31 
32 /* BOOST */
33 //#include <boost/filesystem.hpp>
34 
35 namespace dynamicgraph {
36 namespace sot {
37 namespace tools {
38 
39 using dynamicgraph::Entity;
40 
41 class KinematicPlanner : public Entity {
42  public:
44  typedef std::vector<Eigen::ArrayXd, Eigen::aligned_allocator<Eigen::ArrayXd> > stdVectorofArrayXd;
45 
46  typedef std::vector<Eigen::ArrayXXd, Eigen::aligned_allocator<Eigen::ArrayXXd> > stdVectorofArrayXXd;
47 
48  /*-----SIGNALS--------*/
49  typedef int Dummy;
50  /*
51  dynamicgraph::SignalPtr<double,int> distToDrawerSIN;
52  dynamicgraph::SignalPtr<double,int> objectPositionInDrawerSIN;
53 
54  dynamicgraph::SignalTimeDependent<Dummy,int> trajectoryReadySINTERN;
55 
56  dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> upperBodyJointPositionSOUT;
57  dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> upperBodyJointVelocitySOUT;
58  dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> freeFlyerVelocitySOUT;
59  */
60  /* --- CONSTRUCTOR --- */
61  KinematicPlanner(const std::string& name);
62  virtual ~KinematicPlanner(void);
63  // Sources
64  Eigen::ArrayXd npSource;
65  Eigen::ArrayXXd pSource1;
66  Eigen::ArrayXXd pSource2;
67 
68  stdVectorofArrayXXd pSourceDelayed1;
69  stdVectorofArrayXXd pSourceDelayed2;
70  // Delays
71  Eigen::ArrayXXd pDelay1;
72  Eigen::ArrayXXd pDelay2;
73 
74  // Non Periodic Weights
75  Eigen::ArrayXXd wNonPeriodic; // Eigen::Array<double, 480,4>
76 
77  // Periodic Weights
78  stdVectorofArrayXXd wPeriodic1;
79  stdVectorofArrayXXd wPeriodic2;
80 
81  // Mean joint angles
82  Eigen::ArrayXXd mJointAngle;
83 
84  // Number of Trajectories Created
85  // int nTrajectories; //30
86  int nJoints; // 16
87  int nGaitCycles; // 4
88  int nTimeSteps; // 160
89  int nSources1; // 5
90  int nSources2; // 4
92  std::list<dynamicgraph::SignalBase<int>*> genericSignalRefs;
93 
94  // Load Motion Capture outputs
95  template <typename Derived>
96  void read2DArray(std::string& fileName, Eigen::DenseBase<Derived>& outArr);
97 
98  void setParams(const double& _distanceToDrawer, const double& _objectPositionInDrawer, const std::string& dir);
99  void loadSourceDelays(const std::string& dir);
100  void loadTrainingParams(const std::string& dir, dynamicgraph::Matrix& q, dynamicgraph::Matrix& beta3,
101  Eigen::ArrayXd& mwwn, double& sigma2, int& N, int& K);
102  dynamicgraph::Vector createSubGoals(double D, double P);
103  void delaySources();
104  void blending();
105  void smoothEnds(Eigen::Ref<Eigen::ArrayXd> tr);
106  void bSplineInterpolate(Eigen::ArrayXXd& tr, int factor);
107  int& runKinematicPlanner(int& dummy, int time);
108  void goalAdaption(dynamicgraph::Vector& goals, const std::string&);
109  void savitzkyGolayFilter(Eigen::Ref<Eigen::ArrayXXd> allJointTraj, int polyOrder, int frameSize);
110 
112 }; // class KinematicPlanner
113 } // namespace tools
114 } // namespace sot
115 } // namespace dynamicgraph
116 
117 #endif // SOT_TOOLS_KINEMATIC_PLANNER_HH
Eigen::ArrayXXd pDelay1
Definition: kinematic-planner.hh:71
void savitzkyGolayFilter(Eigen::Ref< Eigen::ArrayXXd > allJointTraj, int polyOrder, int frameSize)
Definition: kinematic-planner.cc:678
Eigen::ArrayXXd pSource1
Definition: kinematic-planner.hh:65
int nGaitCycles
Definition: kinematic-planner.hh:87
Eigen::ArrayXd npSource
Definition: kinematic-planner.hh:64
std::vector< Eigen::ArrayXXd, Eigen::aligned_allocator< Eigen::ArrayXXd > > stdVectorofArrayXXd
Definition: kinematic-planner.hh:46
virtual ~KinematicPlanner(void)
Definition: kinematic-planner.cc:74
Eigen::ArrayXXd wNonPeriodic
Definition: kinematic-planner.hh:75
dynamicgraph::Vector createSubGoals(double D, double P)
Definition: kinematic-planner.cc:723
int nSources2
Definition: kinematic-planner.hh:90
int & runKinematicPlanner(int &dummy, int time)
Definition: kinematic-planner.cc:732
void bSplineInterpolate(Eigen::ArrayXXd &tr, int factor)
Definition: kinematic-planner.cc:523
void loadSourceDelays(const std::string &dir)
Definition: kinematic-planner.cc:195
Definition: kinematic-planner.hh:41
void delaySources()
Definition: kinematic-planner.cc:276
int nTimeSteps
Definition: kinematic-planner.hh:88
std::vector< Eigen::ArrayXd, Eigen::aligned_allocator< Eigen::ArrayXd > > stdVectorofArrayXd
Definition: kinematic-planner.hh:44
int Dummy
Definition: kinematic-planner.hh:49
std::list< dynamicgraph::SignalBase< int > * > genericSignalRefs
Definition: kinematic-planner.hh:92
void read2DArray(std::string &fileName, Eigen::DenseBase< Derived > &outArr)
Definition: kinematic-planner.cc:96
Eigen::ArrayXXd mJointAngle
Definition: kinematic-planner.hh:82
void goalAdaption(dynamicgraph::Vector &goals, const std::string &)
Definition: kinematic-planner.cc:543
KinematicPlanner(const std::string &name)
Definition: kinematic-planner.cc:16
bool parametersSet
Definition: kinematic-planner.hh:111
void smoothEnds(Eigen::Ref< Eigen::ArrayXd > tr)
Definition: kinematic-planner.cc:505
stdVectorofArrayXXd pSourceDelayed2
Definition: kinematic-planner.hh:69
void blending()
Definition: kinematic-planner.cc:336
stdVectorofArrayXXd wPeriodic2
Definition: kinematic-planner.hh:79
int nSources1
Definition: kinematic-planner.hh:89
Eigen::ArrayXXd pDelay2
Definition: kinematic-planner.hh:72
stdVectorofArrayXXd wPeriodic1
Definition: kinematic-planner.hh:78
stdVectorofArrayXXd pSourceDelayed1
Definition: kinematic-planner.hh:68
void setParams(const double &_distanceToDrawer, const double &_objectPositionInDrawer, const std::string &dir)
Definition: kinematic-planner.cc:117
int nJoints
Definition: kinematic-planner.hh:86
Eigen::ArrayXXd pSource2
Definition: kinematic-planner.hh:66
void loadTrainingParams(const std::string &dir, dynamicgraph::Matrix &q, dynamicgraph::Matrix &beta3, Eigen::ArrayXd &mwwn, double &sigma2, int &N, int &K)
Definition: kinematic-planner.cc:165
Definition: cubic-interpolation-se3.hh:15