GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/task/task-pd.cpp Lines: 0 28 0.0 %
Date: 2023-03-13 12:09:37 Branches: 0 54 0.0 %

Line Branch Exec Source
1
/*
2
 * Copyright 2010,
3
 * François Bleibel,
4
 * Olivier Stasse,
5
 *
6
 * CNRS/AIST
7
 *
8
 */
9
10
/* --------------------------------------------------------------------- */
11
/* --- INCLUDE --------------------------------------------------------- */
12
/* --------------------------------------------------------------------- */
13
14
/* SOT */
15
#include <dynamic-graph/all-commands.h>
16
17
#include <sot/core/debug.hh>
18
#include <sot/core/task-pd.hh>
19
20
using namespace std;
21
using namespace dynamicgraph::sot;
22
using namespace dynamicgraph;
23
24
#include <sot/core/factory.hh>
25
26
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskPD, "TaskPD");
27
28
/* --------------------------------------------------------------------- */
29
/* --- CLASS ----------------------------------------------------------- */
30
/* --------------------------------------------------------------------- */
31
32
TaskPD::TaskPD(const std::string &n)
33
    : Task(n),
34
      previousError(),
35
      beta(1),
36
      errorDotSOUT(boost::bind(&TaskPD::computeErrorDot, this, _1, _2),
37
                   errorSOUT,
38
                   "sotTaskPD(" + n + ")::output(vector)::errorDotOUT"),
39
      errorDotSIN(NULL, "sotTaskPD(" + n + ")::input(vector)::errorDot") {
40
  taskSOUT.setFunction(boost::bind(&TaskPD::computeTaskModif, this, _1, _2));
41
  taskSOUT.addDependency(errorDotSOUT);
42
43
  signalRegistration(errorDotSOUT << errorDotSIN);
44
  initCommand();
45
  errorDotSIN.plug(&errorDotSOUT);
46
}
47
48
/* --- COMPUTATION ---------------------------------------------------------- */
49
/* --- COMPUTATION ---------------------------------------------------------- */
50
/* --- COMPUTATION ---------------------------------------------------------- */
51
52
dynamicgraph::Vector &TaskPD::computeErrorDot(dynamicgraph::Vector &errorDot,
53
                                              int time) {
54
  sotDEBUG(15) << "# In {" << endl;
55
56
  const dynamicgraph::Vector &errCur = errorSOUT(time);
57
  if (previousError.size() == errCur.size()) {
58
    errorDot = errCur;
59
    errorDot -= previousError;
60
    previousError = errCur;
61
  } else {
62
    errorDot.resize(errCur.size());
63
    errorDot.setZero();
64
    previousError = errCur;
65
  }
66
  sotDEBUG(15) << "# Out }" << endl;
67
  return errorDot;
68
}
69
70
VectorMultiBound &TaskPD::computeTaskModif(VectorMultiBound &task, int time) {
71
  sotDEBUG(15) << "# In {" << endl;
72
73
  const dynamicgraph::Vector &errorDot = errorDotSIN(time);
74
  Task::computeTaskExponentialDecrease(task, time);
75
76
  sotDEBUG(25) << " Task = " << task;
77
  sotDEBUG(25) << " edot = " << errorDot;
78
79
  for (unsigned int i = 0; i < task.size(); ++i) {
80
    task[i] = task[i].getSingleBound() - (beta * errorDot(i));
81
  }
82
83
  sotDEBUG(15) << "# Out }" << endl;
84
  return task;
85
}
86
87
/* --- PARAMS --------------------------------------------------------------- */
88
/* --- PARAMS --------------------------------------------------------------- */
89
/* --- PARAMS --------------------------------------------------------------- */
90
#include <sot/core/pool.hh>
91
92
void TaskPD::initCommand(void) {
93
  using namespace command;
94
  addCommand("setBeta",
95
             makeDirectSetter(*this, &beta, docDirectSetter("beta", "double")));
96
}