1 |
|
|
// |
2 |
|
|
// Copyright (C) 2012 LAAS-CNRS |
3 |
|
|
// |
4 |
|
|
// Author: Florent Lamiraux |
5 |
|
|
// |
6 |
|
|
|
7 |
|
|
#ifndef SOT_TOOLS_CUBIC_INTERPOLATION_SE3_HH |
8 |
|
|
#define SOT_TOOLS_CUBIC_INTERPOLATION_SE3_HH |
9 |
|
|
|
10 |
|
|
#include <dynamic-graph/entity.h> |
11 |
|
|
#include <dynamic-graph/signal-ptr.h> |
12 |
|
|
#include <dynamic-graph/signal-time-dependent.h> |
13 |
|
|
|
14 |
|
|
#include <sot/core/matrix-geometry.hh> |
15 |
|
|
|
16 |
|
|
namespace dynamicgraph { |
17 |
|
|
namespace sot { |
18 |
|
|
namespace tools { |
19 |
|
|
class CubicInterpolationSE3 : public Entity { |
20 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
21 |
|
|
|
22 |
|
|
public: |
23 |
|
|
virtual ~CubicInterpolationSE3(); |
24 |
|
|
CubicInterpolationSE3(const std::string& name); |
25 |
|
|
/// Start tracking |
26 |
|
|
void start(const double& duration); |
27 |
|
|
/// Reset state to 0 before starting a new motion |
28 |
|
|
void reset(); |
29 |
|
|
/// Documentation |
30 |
|
|
virtual std::string getDocString() const; |
31 |
|
|
/// Set sampling period of control discretization |
32 |
|
|
void setSamplingPeriod(const double& period); |
33 |
|
|
|
34 |
|
|
protected: |
35 |
|
|
virtual void doStart(const double& duration); |
36 |
|
|
dynamicgraph::Signal<MatrixHomogeneous, int> soutSOUT_; |
37 |
|
|
dynamicgraph::Signal<Vector, int> soutdotSOUT_; |
38 |
|
|
dynamicgraph::SignalPtr<MatrixHomogeneous, int> initSIN_; |
39 |
|
|
dynamicgraph::SignalPtr<MatrixHomogeneous, int> goalSIN_; |
40 |
|
|
|
41 |
|
|
MatrixHomogeneous& computeSout(MatrixHomogeneous& 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 |
|
|
|
54 |
|
|
Vector position_; |
55 |
|
|
Vector soutdot_; |
56 |
|
|
}; // class CubicInterpolationSE3 |
57 |
|
|
} // namespace tools |
58 |
|
|
} // namespace sot |
59 |
|
|
} // namespace dynamicgraph |
60 |
|
|
|
61 |
|
|
#endif // SOT_TOOLS_CUBIC_INTERPOLATION_SE3_HH |