1 |
|
|
// |
2 |
|
|
// Copyright (C) 2012 LAAS-CNRS |
3 |
|
|
// |
4 |
|
|
// Author: Florent Lamiraux |
5 |
|
|
// |
6 |
|
|
|
7 |
|
|
#ifndef SOT_TOOLS_CUBIC_INTERPOLATION_HH |
8 |
|
|
#define SOT_TOOLS_CUBIC_INTERPOLATION_HH |
9 |
|
|
|
10 |
|
|
#include <dynamic-graph/entity.h> |
11 |
|
|
#include <dynamic-graph/linear-algebra.h> |
12 |
|
|
#include <dynamic-graph/signal-ptr.h> |
13 |
|
|
#include <dynamic-graph/signal-time-dependent.h> |
14 |
|
|
|
15 |
|
|
namespace dynamicgraph { |
16 |
|
|
namespace sot { |
17 |
|
|
namespace tools { |
18 |
|
|
class CubicInterpolation : public Entity { |
19 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
20 |
|
|
|
21 |
|
|
public: |
22 |
|
|
virtual ~CubicInterpolation(); |
23 |
|
|
CubicInterpolation(const std::string& name); |
24 |
|
|
/// Start tracking |
25 |
|
|
void start(const double& duration); |
26 |
|
|
/// Reset state to 0 before starting a new motion |
27 |
|
|
void reset(); |
28 |
|
|
/// Documentation |
29 |
|
|
virtual std::string getDocString() const; |
30 |
|
|
/// Set sampling period of control discretization |
31 |
|
|
void setSamplingPeriod(const double& period); |
32 |
|
|
|
33 |
|
|
protected: |
34 |
|
|
virtual void doStart(const double& duration); |
35 |
|
|
dynamicgraph::Signal<Vector, int> soutSOUT_; |
36 |
|
|
dynamicgraph::Signal<Vector, int> soutdotSOUT_; |
37 |
|
|
dynamicgraph::SignalPtr<Vector, int> initSIN_; |
38 |
|
|
dynamicgraph::SignalPtr<Vector, int> goalSIN_; |
39 |
|
|
|
40 |
|
|
Vector& computeSout(Vector& sout, const int& inTime); |
41 |
|
|
Vector& computeSoutdot(Vector& sout, const int& inTime); |
42 |
|
|
|
43 |
|
|
int startTime_; |
44 |
|
|
double samplingPeriod_; |
45 |
|
|
double duration_; |
46 |
|
|
// 0: motion not started, 1: motion in progress, 2: motion finished |
47 |
|
|
unsigned state_; |
48 |
|
|
|
49 |
|
|
Vector p0_; |
50 |
|
|
Vector p1_; |
51 |
|
|
Vector p2_; |
52 |
|
|
Vector p3_; |
53 |
|
|
}; // class CubicInterpolation |
54 |
|
|
} // namespace tools |
55 |
|
|
} // namespace sot |
56 |
|
|
} // namespace dynamicgraph |
57 |
|
|
|
58 |
|
|
#endif // SOT_TOOLS_CUBIC_INTERPOLATION_SE3_HH |