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