GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/cubic-interpolation-se3.cc Lines: 0 82 0.0 %
Date: 2023-01-29 11:05:01 Branches: 0 234 0.0 %

Line Branch Exec Source
1
//
2
// Copyright (C) 2012 LAAS-CNRS
3
//
4
// Author: Florent Lamiraux
5
//
6
7
#include "sot/tools/cubic-interpolation-se3.hh"
8
9
#include <dynamic-graph/command-bind.h>
10
#include <dynamic-graph/command-setter.h>
11
#include <dynamic-graph/command.h>
12
#include <dynamic-graph/factory.h>
13
14
namespace dynamicgraph {
15
namespace sot {
16
namespace tools {
17
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(CubicInterpolationSE3,
18
                                   "CubicInterpolationSE3");
19
CubicInterpolationSE3::CubicInterpolationSE3(const std::string& name)
20
    : Entity(name),
21
      soutSOUT_("CubicInterpolationSE3(" + name +
22
                ")::output(MatrixHomo)::sout"),
23
      soutdotSOUT_("CubicInterpolationSE3(" + name +
24
                   ")::output(vector)::soutdot"),
25
      initSIN_(NULL,
26
               "CubicInterpolationSE3(" + name + ")::input(MatrixHomo)::init"),
27
      goalSIN_(NULL,
28
               "CubicInterpolationSE3(" + name + ")::input(MatrixHomo)::goal"),
29
      startTime_(0),
30
      samplingPeriod_(0.),
31
      state_(0),
32
      p0_(3),
33
      p1_(3),
34
      p2_(3),
35
      p3_(3),
36
      position_(3),
37
      soutdot_(3) {
38
  signalRegistration(soutSOUT_);
39
  signalRegistration(soutdotSOUT_);
40
  signalRegistration(initSIN_);
41
  signalRegistration(goalSIN_);
42
  soutSOUT_.setFunction(
43
      boost::bind(&CubicInterpolationSE3::computeSout, this, _1, _2));
44
  soutdot_.setZero();
45
  soutdotSOUT_.setConstant(soutdot_);
46
47
  std::string docstring;
48
  docstring =
49
      "\n"
50
      "    Set sampling period of control discretization.\n"
51
      "\n"
52
      "    Input:\n"
53
      "      - a floating point value.\n"
54
      "\n";
55
  addCommand("setSamplingPeriod",
56
             new command::Setter<CubicInterpolationSE3, double>(
57
                 *this, &CubicInterpolationSE3::setSamplingPeriod, docstring));
58
  docstring =
59
      "\n"
60
      "    Start tracking.\n"
61
      "\n"
62
      "    Input\n"
63
      "      - duration of the motion.\n"
64
      "\n"
65
      "\n  Read init and goal signals, compute output trajectory and start\n"
66
      "tracking.\n";
67
  addCommand("start", new command::Setter<CubicInterpolationSE3, double>(
68
                          *this, &CubicInterpolationSE3::start, docstring));
69
  docstring =
70
      "  Reset interpolation before calling start again\n"
71
      "\n"
72
      "    After the end of an interpolation, goal signal is copied into\n"
73
      "    sout signal. Calling reset make the entity copy init signal into\n"
74
      "    sout signal.\n";
75
  addCommand("reset", command::makeCommandVoid0(
76
                          *this, &CubicInterpolationSE3::reset, docstring));
77
}
78
79
CubicInterpolationSE3::~CubicInterpolationSE3() {}
80
81
std::string CubicInterpolationSE3::getDocString() const {
82
  std::string doc =
83
      "Perform a cubic interpolation in SE(3) between two poses.\n"
84
      "\n"
85
      "  Initial pose is given by signal 'init', Target position is given"
86
      " by signal\n"
87
      "  'goal'. Interpolation is performed with zero velocities at start"
88
      " and goal\n"
89
      "  positions.\n";
90
  return doc;
91
}
92
93
void CubicInterpolationSE3::reset() { state_ = 0; }
94
95
sot::MatrixHomogeneous& CubicInterpolationSE3::computeSout(
96
    sot::MatrixHomogeneous& sout, const int& inTime) {
97
  double t;
98
  switch (state_) {
99
    case 0:
100
      sout = initSIN_.accessCopy();
101
      break;
102
    case 1:
103
      t = (inTime - startTime_) * samplingPeriod_;
104
      position_ = p0_ + (p1_ + (p2_ + p3_ * t) * t) * t;
105
      sout(0, 3) = position_(0);
106
      sout(1, 3) = position_(1);
107
      sout(2, 3) = position_(2);
108
      soutdot_ = p1_ + (p2_ * 2 + p3_ * (3 * t)) * t;
109
      soutdotSOUT_.setConstant(soutdot_);
110
      if (t >= duration_) {
111
        state_ = 2;
112
      }
113
      break;
114
    case 2:
115
      soutdot_.setZero();
116
      soutdotSOUT_.setConstant(soutdot_);
117
      sout = goalSIN_.accessCopy();
118
    default:
119
      break;
120
  }
121
  return sout;
122
}
123
124
void CubicInterpolationSE3::setSamplingPeriod(const double& period) {
125
  samplingPeriod_ = period;
126
}
127
128
void CubicInterpolationSE3::start(const double& duration) { doStart(duration); }
129
130
void CubicInterpolationSE3::doStart(const double& duration) {
131
  // Check that sampling period has been initialized
132
  if (samplingPeriod_ <= 0)
133
    throw ExceptionSignal(ExceptionSignal::NOT_INITIALIZED,
134
                          "CubicInterpolationSE3: samplingPeriod should"
135
                          " be positive. Are you sure you did\n"
136
                          "initialize it?");
137
  if (state_ == 0) {
138
    duration_ = duration;
139
    startTime_ = soutSOUT_.getTime();
140
    double T = duration;
141
    // Initial position
142
    p0_(0) = initSIN_.accessCopy()(0, 3);
143
    p0_(1) = initSIN_.accessCopy()(1, 3);
144
    p0_(2) = initSIN_.accessCopy()(2, 3);
145
    // Initial velocity
146
    p1_(0) = 0.;
147
    p1_(1) = 0.;
148
    p1_(2) = 0.;
149
    // Goal position
150
    Vector P_T(3);
151
    P_T(0) = goalSIN_.accessCopy()(0, 3);
152
    P_T(1) = goalSIN_.accessCopy()(1, 3);
153
    P_T(2) = goalSIN_.accessCopy()(2, 3);
154
    // Final velocity
155
    Vector D_T(3);
156
    D_T.setZero();
157
    p2_ = (D_T + p1_ * 2) * (-1. / T) + (P_T - p0_) * (3. / (T * T));
158
    p3_ = (P_T - p0_) * (-2 / (T * T * T)) + (p1_ + D_T) * (1. / (T * T));
159
    state_ = 1;
160
  }
161
}
162
}  // namespace tools
163
}  // namespace sot
164
}  // namespace dynamicgraph